1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9//   this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11//   this list of conditions and the following disclaimer in the documentation
12//   and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14//   used to endorse or promote products derived from this software without
15//   specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: keir@google.com (Keir Mierle)
30
31#include "ceres/parameter_block.h"
32
33#include "gtest/gtest.h"
34#include "ceres/internal/eigen.h"
35
36namespace ceres {
37namespace internal {
38
39TEST(ParameterBlock, SetLocalParameterization) {
40  double x[3] = { 1.0, 2.0, 3.0 };
41  ParameterBlock parameter_block(x, 3, -1);
42
43  // The indices to set constant within the parameter block (used later).
44  vector<int> indices;
45  indices.push_back(1);
46
47  // Can't set the parameterization if the sizes don't match.
48  SubsetParameterization subset_wrong_size(4, indices);
49  EXPECT_DEATH_IF_SUPPORTED(
50      parameter_block.SetParameterization(&subset_wrong_size), "global");
51
52  // Can't set parameterization to NULL from NULL.
53  EXPECT_DEATH_IF_SUPPORTED
54      (parameter_block.SetParameterization(NULL), "NULL");
55
56  // Now set the parameterization.
57  SubsetParameterization subset(3, indices);
58  parameter_block.SetParameterization(&subset);
59
60  // Re-setting the parameterization to the same value is supported.
61  parameter_block.SetParameterization(&subset);
62
63  // Can't set parameterization to NULL from another parameterization.
64  EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL");
65
66  // Can't set the parameterization more than once.
67  SubsetParameterization subset_different(3, indices);
68  EXPECT_DEATH_IF_SUPPORTED
69      (parameter_block.SetParameterization(&subset_different), "re-set");
70
71  // Ensure the local parameterization jacobian result is correctly computed.
72  ConstMatrixRef local_parameterization_jacobian(
73      parameter_block.LocalParameterizationJacobian(),
74      3,
75      2);
76  ASSERT_EQ(1.0, local_parameterization_jacobian(0, 0));
77  ASSERT_EQ(0.0, local_parameterization_jacobian(0, 1));
78  ASSERT_EQ(0.0, local_parameterization_jacobian(1, 0));
79  ASSERT_EQ(0.0, local_parameterization_jacobian(1, 1));
80  ASSERT_EQ(0.0, local_parameterization_jacobian(2, 0));
81  ASSERT_EQ(1.0, local_parameterization_jacobian(2, 1));
82
83  // Check that updating works as expected.
84  double x_plus_delta[3];
85  double delta[2] = { 0.5, 0.3 };
86  parameter_block.Plus(x, delta, x_plus_delta);
87  ASSERT_EQ(1.5, x_plus_delta[0]);
88  ASSERT_EQ(2.0, x_plus_delta[1]);
89  ASSERT_EQ(3.3, x_plus_delta[2]);
90}
91
92struct TestParameterization : public LocalParameterization {
93 public:
94  virtual ~TestParameterization() {}
95  virtual bool Plus(const double* x,
96                    const double* delta,
97                    double* x_plus_delta) const {
98    LOG(FATAL) << "Shouldn't get called.";
99    return true;
100  }
101  virtual bool ComputeJacobian(const double* x,
102                               double* jacobian) const {
103    jacobian[0] = *x * 2;
104    return true;
105  }
106
107  virtual int GlobalSize() const { return 1; }
108  virtual int LocalSize() const { return 1; }
109};
110
111TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) {
112  TestParameterization test_parameterization;
113  double x[1] = { 1.0 };
114  ParameterBlock parameter_block(x, 1, -1, &test_parameterization);
115
116  EXPECT_EQ(2.0, *parameter_block.LocalParameterizationJacobian());
117
118  x[0] = 5.5;
119  parameter_block.SetState(x);
120  EXPECT_EQ(11.0, *parameter_block.LocalParameterizationJacobian());
121}
122
123TEST(ParameterBlock, PlusWithNoLocalParameterization) {
124  double x[2] = { 1.0, 2.0 };
125  ParameterBlock parameter_block(x, 2, -1);
126
127  double delta[2] = { 0.2, 0.3 };
128  double x_plus_delta[2];
129  parameter_block.Plus(x, delta, x_plus_delta);
130  EXPECT_EQ(1.2, x_plus_delta[0]);
131  EXPECT_EQ(2.3, x_plus_delta[1]);
132}
133
134// Stops computing the jacobian after the first time.
135class BadLocalParameterization : public LocalParameterization {
136 public:
137  BadLocalParameterization()
138      : calls_(0) {
139  }
140
141  virtual ~BadLocalParameterization() {}
142  virtual bool Plus(const double* x,
143                    const double* delta,
144                    double* x_plus_delta) const {
145    *x_plus_delta = *x + *delta;
146    return true;
147  }
148
149  virtual bool ComputeJacobian(const double* x, double* jacobian) const {
150    if (calls_ == 0) {
151      jacobian[0] = 0;
152    }
153    ++calls_;
154    return true;
155  }
156
157  virtual int GlobalSize() const { return 1;}
158  virtual int LocalSize()  const { return 1;}
159
160 private:
161  mutable int calls_;
162};
163
164TEST(ParameterBlock, DetectBadLocalParameterization) {
165  double x = 1;
166  BadLocalParameterization bad_parameterization;
167  ParameterBlock parameter_block(&x, 1, -1, &bad_parameterization);
168  double y = 2;
169  EXPECT_FALSE(parameter_block.SetState(&y));
170}
171
172TEST(ParameterBlock, DefaultBounds) {
173  double x[2];
174  ParameterBlock parameter_block(x, 2, -1, NULL);
175  EXPECT_EQ(parameter_block.UpperBoundForParameter(0),
176            std::numeric_limits<double>::max());
177  EXPECT_EQ(parameter_block.UpperBoundForParameter(1),
178            std::numeric_limits<double>::max());
179  EXPECT_EQ(parameter_block.LowerBoundForParameter(0),
180            -std::numeric_limits<double>::max());
181  EXPECT_EQ(parameter_block.LowerBoundForParameter(1),
182            -std::numeric_limits<double>::max());
183}
184
185TEST(ParameterBlock, SetBounds) {
186  double x[2];
187  ParameterBlock parameter_block(x, 2, -1, NULL);
188  parameter_block.SetLowerBound(0, 1);
189  parameter_block.SetUpperBound(1, 1);
190
191  EXPECT_EQ(parameter_block.LowerBoundForParameter(0), 1.0);
192  EXPECT_EQ(parameter_block.LowerBoundForParameter(1),
193            -std::numeric_limits<double>::max());
194
195  EXPECT_EQ(parameter_block.UpperBoundForParameter(0),
196            std::numeric_limits<double>::max());
197  EXPECT_EQ(parameter_block.UpperBoundForParameter(1), 1.0);
198}
199
200TEST(ParameterBlock, PlusWithBoundsConstraints) {
201  double x[] = {1.0, 0.0};
202  double delta[] = {2.0, -10.0};
203  ParameterBlock parameter_block(x, 2, -1, NULL);
204  parameter_block.SetUpperBound(0, 2.0);
205  parameter_block.SetLowerBound(1, -1.0);
206  double x_plus_delta[2];
207  parameter_block.Plus(x, delta, x_plus_delta);
208  EXPECT_EQ(x_plus_delta[0], 2.0);
209  EXPECT_EQ(x_plus_delta[1], -1.0);
210}
211
212}  // namespace internal
213}  // namespace ceres
214