| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2015 Google Inc. All rights reserved. |
| // http://ceres-solver.org/ |
| // |
| // Redistribution and use in source and binary forms, with or without |
| // modification, are permitted provided that the following conditions are met: |
| // |
| // * Redistributions of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // * Redistributions in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // * Neither the name of Google Inc. nor the names of its contributors may be |
| // used to endorse or promote products derived from this software without |
| // specific prior written permission. |
| // |
| // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // POSSIBILITY OF SUCH DAMAGE. |
| // |
| // Author: keir@google.com (Keir Mierle) |
| |
| #include "ceres/parameter_block.h" |
| |
| #include "gtest/gtest.h" |
| #include "ceres/internal/eigen.h" |
| |
| namespace ceres { |
| namespace internal { |
| |
| TEST(ParameterBlock, SetLocalParameterizationDiesOnSizeMismatch) { |
| double x[3] = {1.0, 2.0, 3.0}; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(1); |
| SubsetParameterization subset_wrong_size(4, indices); |
| EXPECT_DEATH_IF_SUPPORTED( |
| parameter_block.SetParameterization(&subset_wrong_size), "global"); |
| } |
| |
| TEST(ParameterBlock, SetLocalParameterizationWithSameExistingParameterization) { |
| double x[3] = {1.0, 2.0, 3.0}; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(1); |
| SubsetParameterization subset(3, indices); |
| parameter_block.SetParameterization(&subset); |
| parameter_block.SetParameterization(&subset); |
| } |
| |
| TEST(ParameterBlock, SetLocalParameterizationDiesWhenResettingToNull) { |
| double x[3] = {1.0, 2.0, 3.0}; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(1); |
| SubsetParameterization subset(3, indices); |
| parameter_block.SetParameterization(&subset); |
| EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL"); |
| } |
| |
| TEST(ParameterBlock, |
| SetLocalParameterizationDiesWhenResettingToDifferentParameterization) { |
| double x[3] = {1.0, 2.0, 3.0}; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(1); |
| SubsetParameterization subset(3, indices); |
| parameter_block.SetParameterization(&subset); |
| SubsetParameterization subset_different(3, indices); |
| EXPECT_DEATH_IF_SUPPORTED( |
| parameter_block.SetParameterization(&subset_different), "re-set"); |
| } |
| |
| TEST(ParameterBlock, SetLocalParameterizationDiesOnNullParameterization) { |
| double x[3] = {1.0, 2.0, 3.0}; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(1); |
| EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL"); |
| } |
| |
| TEST(ParameterBlock, SetParameterizationDiesOnZeroLocalSize) { |
| double x[3] = {1.0, 2.0, 3.0}; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(0); |
| indices.push_back(1); |
| indices.push_back(2); |
| SubsetParameterization subset(3, indices); |
| EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(&subset), |
| "positive dimensional tangent"); |
| } |
| |
| TEST(ParameterBlock, SetLocalParameterizationAndNormalOperation) { |
| double x[3] = { 1.0, 2.0, 3.0 }; |
| ParameterBlock parameter_block(x, 3, -1); |
| std::vector<int> indices; |
| indices.push_back(1); |
| SubsetParameterization subset(3, indices); |
| parameter_block.SetParameterization(&subset); |
| |
| // Ensure the local parameterization jacobian result is correctly computed. |
| ConstMatrixRef local_parameterization_jacobian( |
| parameter_block.LocalParameterizationJacobian(), |
| 3, |
| 2); |
| ASSERT_EQ(1.0, local_parameterization_jacobian(0, 0)); |
| ASSERT_EQ(0.0, local_parameterization_jacobian(0, 1)); |
| ASSERT_EQ(0.0, local_parameterization_jacobian(1, 0)); |
| ASSERT_EQ(0.0, local_parameterization_jacobian(1, 1)); |
| ASSERT_EQ(0.0, local_parameterization_jacobian(2, 0)); |
| ASSERT_EQ(1.0, local_parameterization_jacobian(2, 1)); |
| |
| // Check that updating works as expected. |
| double x_plus_delta[3]; |
| double delta[2] = { 0.5, 0.3 }; |
| parameter_block.Plus(x, delta, x_plus_delta); |
| ASSERT_EQ(1.5, x_plus_delta[0]); |
| ASSERT_EQ(2.0, x_plus_delta[1]); |
| ASSERT_EQ(3.3, x_plus_delta[2]); |
| } |
| |
| struct TestParameterization : public LocalParameterization { |
| public: |
| virtual ~TestParameterization() {} |
| virtual bool Plus(const double* x, |
| const double* delta, |
| double* x_plus_delta) const { |
| LOG(FATAL) << "Shouldn't get called."; |
| return true; |
| } |
| virtual bool ComputeJacobian(const double* x, |
| double* jacobian) const { |
| jacobian[0] = *x * 2; |
| return true; |
| } |
| |
| virtual int GlobalSize() const { return 1; } |
| virtual int LocalSize() const { return 1; } |
| }; |
| |
| TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) { |
| TestParameterization test_parameterization; |
| double x[1] = { 1.0 }; |
| ParameterBlock parameter_block(x, 1, -1, &test_parameterization); |
| |
| EXPECT_EQ(2.0, *parameter_block.LocalParameterizationJacobian()); |
| |
| x[0] = 5.5; |
| parameter_block.SetState(x); |
| EXPECT_EQ(11.0, *parameter_block.LocalParameterizationJacobian()); |
| } |
| |
| TEST(ParameterBlock, PlusWithNoLocalParameterization) { |
| double x[2] = { 1.0, 2.0 }; |
| ParameterBlock parameter_block(x, 2, -1); |
| |
| double delta[2] = { 0.2, 0.3 }; |
| double x_plus_delta[2]; |
| parameter_block.Plus(x, delta, x_plus_delta); |
| EXPECT_EQ(1.2, x_plus_delta[0]); |
| EXPECT_EQ(2.3, x_plus_delta[1]); |
| } |
| |
| // Stops computing the jacobian after the first time. |
| class BadLocalParameterization : public LocalParameterization { |
| public: |
| BadLocalParameterization() |
| : calls_(0) { |
| } |
| |
| virtual ~BadLocalParameterization() {} |
| virtual bool Plus(const double* x, |
| const double* delta, |
| double* x_plus_delta) const { |
| *x_plus_delta = *x + *delta; |
| return true; |
| } |
| |
| virtual bool ComputeJacobian(const double* x, double* jacobian) const { |
| if (calls_ == 0) { |
| jacobian[0] = 0; |
| } |
| ++calls_; |
| return true; |
| } |
| |
| virtual int GlobalSize() const { return 1;} |
| virtual int LocalSize() const { return 1;} |
| |
| private: |
| mutable int calls_; |
| }; |
| |
| TEST(ParameterBlock, DetectBadLocalParameterization) { |
| double x = 1; |
| BadLocalParameterization bad_parameterization; |
| ParameterBlock parameter_block(&x, 1, -1, &bad_parameterization); |
| double y = 2; |
| EXPECT_FALSE(parameter_block.SetState(&y)); |
| } |
| |
| TEST(ParameterBlock, DefaultBounds) { |
| double x[2]; |
| ParameterBlock parameter_block(x, 2, -1, NULL); |
| EXPECT_EQ(parameter_block.UpperBoundForParameter(0), |
| std::numeric_limits<double>::max()); |
| EXPECT_EQ(parameter_block.UpperBoundForParameter(1), |
| std::numeric_limits<double>::max()); |
| EXPECT_EQ(parameter_block.LowerBoundForParameter(0), |
| -std::numeric_limits<double>::max()); |
| EXPECT_EQ(parameter_block.LowerBoundForParameter(1), |
| -std::numeric_limits<double>::max()); |
| } |
| |
| TEST(ParameterBlock, SetBounds) { |
| double x[2]; |
| ParameterBlock parameter_block(x, 2, -1, NULL); |
| parameter_block.SetLowerBound(0, 1); |
| parameter_block.SetUpperBound(1, 1); |
| |
| EXPECT_EQ(parameter_block.LowerBoundForParameter(0), 1.0); |
| EXPECT_EQ(parameter_block.LowerBoundForParameter(1), |
| -std::numeric_limits<double>::max()); |
| |
| EXPECT_EQ(parameter_block.UpperBoundForParameter(0), |
| std::numeric_limits<double>::max()); |
| EXPECT_EQ(parameter_block.UpperBoundForParameter(1), 1.0); |
| } |
| |
| TEST(ParameterBlock, PlusWithBoundsConstraints) { |
| double x[] = {1.0, 0.0}; |
| double delta[] = {2.0, -10.0}; |
| ParameterBlock parameter_block(x, 2, -1, NULL); |
| parameter_block.SetUpperBound(0, 2.0); |
| parameter_block.SetLowerBound(1, -1.0); |
| double x_plus_delta[2]; |
| parameter_block.Plus(x, delta, x_plus_delta); |
| EXPECT_EQ(x_plus_delta[0], 2.0); |
| EXPECT_EQ(x_plus_delta[1], -1.0); |
| } |
| |
| } // namespace internal |
| } // namespace ceres |