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 (at) 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 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: 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 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 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: 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 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 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 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 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
