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
36 namespace ceres {
37 namespace internal {
38
TEST(ParameterBlock,SetLocalParameterization)39 TEST(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
92 struct TestParameterization : public LocalParameterization {
93 public:
~TestParameterizationceres::internal::TestParameterization94 virtual ~TestParameterization() {}
Plusceres::internal::TestParameterization95 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 }
ComputeJacobianceres::internal::TestParameterization101 virtual bool ComputeJacobian(const double* x,
102 double* jacobian) const {
103 jacobian[0] = *x * 2;
104 return true;
105 }
106
GlobalSizeceres::internal::TestParameterization107 virtual int GlobalSize() const { return 1; }
LocalSizeceres::internal::TestParameterization108 virtual int LocalSize() const { return 1; }
109 };
110
TEST(ParameterBlock,SetStateUpdatesLocalParameterizationJacobian)111 TEST(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
TEST(ParameterBlock,PlusWithNoLocalParameterization)123 TEST(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.
135 class BadLocalParameterization : public LocalParameterization {
136 public:
BadLocalParameterization()137 BadLocalParameterization()
138 : calls_(0) {
139 }
140
~BadLocalParameterization()141 virtual ~BadLocalParameterization() {}
Plus(const double * x,const double * delta,double * x_plus_delta) const142 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
ComputeJacobian(const double * x,double * jacobian) const149 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
GlobalSize() const157 virtual int GlobalSize() const { return 1;}
LocalSize() const158 virtual int LocalSize() const { return 1;}
159
160 private:
161 mutable int calls_;
162 };
163
TEST(ParameterBlock,DetectBadLocalParameterization)164 TEST(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
TEST(ParameterBlock,DefaultBounds)172 TEST(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
TEST(ParameterBlock,SetBounds)185 TEST(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
TEST(ParameterBlock,PlusWithBoundsConstraints)200 TEST(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