blob: f01e0ad7fff11f6507f5018467200b274d4dbf97 [file] [log] [blame]
Keir Mierle8ebb0732012-04-30 23:09:08 -07001// 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 };
Keir Mierle04938ef2013-02-17 12:37:55 -080041 ParameterBlock parameter_block(x, 3, -1);
Keir Mierle8ebb0732012-04-30 23:09:08 -070042
43 // The indices to set constant within the parameter block (used later).
Sameer Agarwalbcc865f2014-12-17 07:35:09 -080044 std::vector<int> indices;
Keir Mierle8ebb0732012-04-30 23:09:08 -070045 indices.push_back(1);
46
47 // Can't set the parameterization if the sizes don't match.
48 SubsetParameterization subset_wrong_size(4, indices);
Sameer Agarwalc0149972012-09-18 13:55:18 -070049 EXPECT_DEATH_IF_SUPPORTED(
50 parameter_block.SetParameterization(&subset_wrong_size), "global");
Keir Mierle8ebb0732012-04-30 23:09:08 -070051
52 // Can't set parameterization to NULL from NULL.
Sameer Agarwalc0149972012-09-18 13:55:18 -070053 EXPECT_DEATH_IF_SUPPORTED
54 (parameter_block.SetParameterization(NULL), "NULL");
Keir Mierle8ebb0732012-04-30 23:09:08 -070055
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.
Sameer Agarwalc0149972012-09-18 13:55:18 -070064 EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL");
Keir Mierle8ebb0732012-04-30 23:09:08 -070065
66 // Can't set the parameterization more than once.
67 SubsetParameterization subset_different(3, indices);
Sameer Agarwalc0149972012-09-18 13:55:18 -070068 EXPECT_DEATH_IF_SUPPORTED
69 (parameter_block.SetParameterization(&subset_different), "re-set");
Keir Mierle8ebb0732012-04-30 23:09:08 -070070
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 };
Keir Mierle04938ef2013-02-17 12:37:55 -0800114 ParameterBlock parameter_block(x, 1, -1, &test_parameterization);
Keir Mierle8ebb0732012-04-30 23:09:08 -0700115
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 };
Keir Mierle04938ef2013-02-17 12:37:55 -0800125 ParameterBlock parameter_block(x, 2, -1);
Keir Mierle8ebb0732012-04-30 23:09:08 -0700126
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;
Keir Mierle04938ef2013-02-17 12:37:55 -0800167 ParameterBlock parameter_block(&x, 1, -1, &bad_parameterization);
Keir Mierle8ebb0732012-04-30 23:09:08 -0700168 double y = 2;
169 EXPECT_FALSE(parameter_block.SetState(&y));
170}
171
Sameer Agarwal5cf867b2014-02-18 15:58:59 -0800172TEST(ParameterBlock, DefaultBounds) {
173 double x[2];
174 ParameterBlock parameter_block(x, 2, -1, NULL);
Sameer Agarwala536ae72014-05-04 21:18:42 -0700175 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());
Sameer Agarwal5cf867b2014-02-18 15:58:59 -0800183}
184
185TEST(ParameterBlock, SetBounds) {
186 double x[2];
187 ParameterBlock parameter_block(x, 2, -1, NULL);
Sameer Agarwal5cf867b2014-02-18 15:58:59 -0800188 parameter_block.SetLowerBound(0, 1);
Sameer Agarwala536ae72014-05-04 21:18:42 -0700189 parameter_block.SetUpperBound(1, 1);
Sameer Agarwal5cf867b2014-02-18 15:58:59 -0800190
Sameer Agarwala536ae72014-05-04 21:18:42 -0700191 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);
Sameer Agarwal5cf867b2014-02-18 15:58:59 -0800198}
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
Keir Mierle8ebb0732012-04-30 23:09:08 -0700212} // namespace internal
213} // namespace ceres