Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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 | |
| 39 | TEST(ParameterBlock, SetLocalParameterization) { |
| 40 | double x[3] = { 1.0, 2.0, 3.0 }; |
Keir Mierle | 04938ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 41 | ParameterBlock parameter_block(x, 3, -1); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 42 | |
| 43 | // The indices to set constant within the parameter block (used later). |
Sameer Agarwal | bcc865f | 2014-12-17 07:35:09 -0800 | [diff] [blame] | 44 | std::vector<int> indices; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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); |
Sameer Agarwal | c014997 | 2012-09-18 13:55:18 -0700 | [diff] [blame] | 49 | EXPECT_DEATH_IF_SUPPORTED( |
| 50 | parameter_block.SetParameterization(&subset_wrong_size), "global"); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 51 | |
| 52 | // Can't set parameterization to NULL from NULL. |
Sameer Agarwal | c014997 | 2012-09-18 13:55:18 -0700 | [diff] [blame] | 53 | EXPECT_DEATH_IF_SUPPORTED |
| 54 | (parameter_block.SetParameterization(NULL), "NULL"); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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. |
Sameer Agarwal | c014997 | 2012-09-18 13:55:18 -0700 | [diff] [blame] | 64 | EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL"); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 65 | |
| 66 | // Can't set the parameterization more than once. |
| 67 | SubsetParameterization subset_different(3, indices); |
Sameer Agarwal | c014997 | 2012-09-18 13:55:18 -0700 | [diff] [blame] | 68 | EXPECT_DEATH_IF_SUPPORTED |
| 69 | (parameter_block.SetParameterization(&subset_different), "re-set"); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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 }; |
Keir Mierle | 04938ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 114 | ParameterBlock parameter_block(x, 1, -1, &test_parameterization); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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 }; |
Keir Mierle | 04938ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 125 | ParameterBlock parameter_block(x, 2, -1); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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; |
Keir Mierle | 04938ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 167 | ParameterBlock parameter_block(&x, 1, -1, &bad_parameterization); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 168 | double y = 2; |
| 169 | EXPECT_FALSE(parameter_block.SetState(&y)); |
| 170 | } |
| 171 | |
Sameer Agarwal | 5cf867b | 2014-02-18 15:58:59 -0800 | [diff] [blame] | 172 | TEST(ParameterBlock, DefaultBounds) { |
| 173 | double x[2]; |
| 174 | ParameterBlock parameter_block(x, 2, -1, NULL); |
Sameer Agarwal | a536ae7 | 2014-05-04 21:18:42 -0700 | [diff] [blame] | 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()); |
Sameer Agarwal | 5cf867b | 2014-02-18 15:58:59 -0800 | [diff] [blame] | 183 | } |
| 184 | |
| 185 | TEST(ParameterBlock, SetBounds) { |
| 186 | double x[2]; |
| 187 | ParameterBlock parameter_block(x, 2, -1, NULL); |
Sameer Agarwal | 5cf867b | 2014-02-18 15:58:59 -0800 | [diff] [blame] | 188 | parameter_block.SetLowerBound(0, 1); |
Sameer Agarwal | a536ae7 | 2014-05-04 21:18:42 -0700 | [diff] [blame] | 189 | parameter_block.SetUpperBound(1, 1); |
Sameer Agarwal | 5cf867b | 2014-02-18 15:58:59 -0800 | [diff] [blame] | 190 | |
Sameer Agarwal | a536ae7 | 2014-05-04 21:18:42 -0700 | [diff] [blame] | 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); |
Sameer Agarwal | 5cf867b | 2014-02-18 15:58:59 -0800 | [diff] [blame] | 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 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 212 | } // namespace internal |
| 213 | } // namespace ceres |