internal/ceres/parameter_block_test.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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, SetLocalParameterizationDiesOnSizeMismatch) {
  40   double x[3] = {1.0, 2.0, 3.0};
  41   ParameterBlock parameter_block(x, 3, -1);
  42   std::vector<int> indices;
  43   indices.push_back(1);
  44   SubsetParameterization subset_wrong_size(4, indices);
  45   EXPECT_DEATH_IF_SUPPORTED(
  46       parameter_block.SetParameterization(&subset_wrong_size), "global");
  47 }
  48
  49 TEST(ParameterBlock, SetLocalParameterizationWithSameExistingParameterization) {
  50   double x[3] = {1.0, 2.0, 3.0};
  51   ParameterBlock parameter_block(x, 3, -1);
  52   std::vector<int> indices;
  53   indices.push_back(1);
  54   SubsetParameterization subset(3, indices);
  55   parameter_block.SetParameterization(&subset);
  56   parameter_block.SetParameterization(&subset);
  57 }
  58
  59 TEST(ParameterBlock, SetLocalParameterizationDiesWhenResettingToNull) {
  60   double x[3] = {1.0, 2.0, 3.0};
  61   ParameterBlock parameter_block(x, 3, -1);
  62   std::vector<int> indices;
  63   indices.push_back(1);
  64   SubsetParameterization subset(3, indices);
  65   parameter_block.SetParameterization(&subset);
  66   EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL");
  67 }
  68
  69 TEST(ParameterBlock,
  70      SetLocalParameterizationDiesWhenResettingToDifferentParameterization) {
  71   double x[3] = {1.0, 2.0, 3.0};
  72   ParameterBlock parameter_block(x, 3, -1);
  73   std::vector<int> indices;
  74   indices.push_back(1);
  75   SubsetParameterization subset(3, indices);
  76   parameter_block.SetParameterization(&subset);
  77   SubsetParameterization subset_different(3, indices);
  78   EXPECT_DEATH_IF_SUPPORTED(
  79       parameter_block.SetParameterization(&subset_different), "re-set");
  80 }
  81
  82 TEST(ParameterBlock, SetLocalParameterizationDiesOnNullParameterization) {
  83   double x[3] = {1.0, 2.0, 3.0};
  84   ParameterBlock parameter_block(x, 3, -1);
  85   std::vector<int> indices;
  86   indices.push_back(1);
  87   EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(NULL), "NULL");
  88 }
  89
  90 TEST(ParameterBlock, SetParameterizationDiesOnZeroLocalSize) {
  91   double x[3] = {1.0, 2.0, 3.0};
  92   ParameterBlock parameter_block(x, 3, -1);
  93   std::vector<int> indices;
  94   indices.push_back(0);
  95   indices.push_back(1);
  96   indices.push_back(2);
  97   SubsetParameterization subset(3, indices);
  98   EXPECT_DEATH_IF_SUPPORTED(parameter_block.SetParameterization(&subset),
  99                             "positive dimensional tangent");
 100 }
 101
 102 TEST(ParameterBlock, SetLocalParameterizationAndNormalOperation) {
 103   double x[3] = { 1.0, 2.0, 3.0 };
 104   ParameterBlock parameter_block(x, 3, -1);
 105   std::vector<int> indices;
 106   indices.push_back(1);
 107   SubsetParameterization subset(3, indices);
 108   parameter_block.SetParameterization(&subset);
 109
 110   // Ensure the local parameterization jacobian result is correctly computed.
 111   ConstMatrixRef local_parameterization_jacobian(
 112       parameter_block.LocalParameterizationJacobian(),
 113       3,
 114       2);
 115   ASSERT_EQ(1.0, local_parameterization_jacobian(0, 0));
 116   ASSERT_EQ(0.0, local_parameterization_jacobian(0, 1));
 117   ASSERT_EQ(0.0, local_parameterization_jacobian(1, 0));
 118   ASSERT_EQ(0.0, local_parameterization_jacobian(1, 1));
 119   ASSERT_EQ(0.0, local_parameterization_jacobian(2, 0));
 120   ASSERT_EQ(1.0, local_parameterization_jacobian(2, 1));
 121
 122   // Check that updating works as expected.
 123   double x_plus_delta[3];
 124   double delta[2] = { 0.5, 0.3 };
 125   parameter_block.Plus(x, delta, x_plus_delta);
 126   ASSERT_EQ(1.5, x_plus_delta[0]);
 127   ASSERT_EQ(2.0, x_plus_delta[1]);
 128   ASSERT_EQ(3.3, x_plus_delta[2]);
 129 }
 130
 131 struct TestParameterization : public LocalParameterization {
 132  public:
 133   virtual ~TestParameterization() {}
 134   virtual bool Plus(const double* x,
 135                     const double* delta,
 136                     double* x_plus_delta) const {
 137     LOG(FATAL) << "Shouldn't get called.";
 138     return true;
 139   }
 140   virtual bool ComputeJacobian(const double* x,
 141                                double* jacobian) const {
 142     jacobian[0] = *x * 2;
 143     return true;
 144   }
 145
 146   virtual int GlobalSize() const { return 1; }
 147   virtual int LocalSize() const { return 1; }
 148 };
 149
 150 TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) {
 151   TestParameterization test_parameterization;
 152   double x[1] = { 1.0 };
 153   ParameterBlock parameter_block(x, 1, -1, &test_parameterization);
 154
 155   EXPECT_EQ(2.0, *parameter_block.LocalParameterizationJacobian());
 156
 157   x[0] = 5.5;
 158   parameter_block.SetState(x);
 159   EXPECT_EQ(11.0, *parameter_block.LocalParameterizationJacobian());
 160 }
 161
 162 TEST(ParameterBlock, PlusWithNoLocalParameterization) {
 163   double x[2] = { 1.0, 2.0 };
 164   ParameterBlock parameter_block(x, 2, -1);
 165
 166   double delta[2] = { 0.2, 0.3 };
 167   double x_plus_delta[2];
 168   parameter_block.Plus(x, delta, x_plus_delta);
 169   EXPECT_EQ(1.2, x_plus_delta[0]);
 170   EXPECT_EQ(2.3, x_plus_delta[1]);
 171 }
 172
 173 // Stops computing the jacobian after the first time.
 174 class BadLocalParameterization : public LocalParameterization {
 175  public:
 176   BadLocalParameterization()
 177       : calls_(0) {
 178   }
 179
 180   virtual ~BadLocalParameterization() {}
 181   virtual bool Plus(const double* x,
 182                     const double* delta,
 183                     double* x_plus_delta) const {
 184     *x_plus_delta = *x + *delta;
 185     return true;
 186   }
 187
 188   virtual bool ComputeJacobian(const double* x, double* jacobian) const {
 189     if (calls_ == 0) {
 190       jacobian[0] = 0;
 191     }
 192     ++calls_;
 193     return true;
 194   }
 195
 196   virtual int GlobalSize() const { return 1;}
 197   virtual int LocalSize()  const { return 1;}
 198
 199  private:
 200   mutable int calls_;
 201 };
 202
 203 TEST(ParameterBlock, DetectBadLocalParameterization) {
 204   double x = 1;
 205   BadLocalParameterization bad_parameterization;
 206   ParameterBlock parameter_block(&x, 1, -1, &bad_parameterization);
 207   double y = 2;
 208   EXPECT_FALSE(parameter_block.SetState(&y));
 209 }
 210
 211 TEST(ParameterBlock, DefaultBounds) {
 212   double x[2];
 213   ParameterBlock parameter_block(x, 2, -1, NULL);
 214   EXPECT_EQ(parameter_block.UpperBoundForParameter(0),
 215             std::numeric_limits<double>::max());
 216   EXPECT_EQ(parameter_block.UpperBoundForParameter(1),
 217             std::numeric_limits<double>::max());
 218   EXPECT_EQ(parameter_block.LowerBoundForParameter(0),
 219             -std::numeric_limits<double>::max());
 220   EXPECT_EQ(parameter_block.LowerBoundForParameter(1),
 221             -std::numeric_limits<double>::max());
 222 }
 223
 224 TEST(ParameterBlock, SetBounds) {
 225   double x[2];
 226   ParameterBlock parameter_block(x, 2, -1, NULL);
 227   parameter_block.SetLowerBound(0, 1);
 228   parameter_block.SetUpperBound(1, 1);
 229
 230   EXPECT_EQ(parameter_block.LowerBoundForParameter(0), 1.0);
 231   EXPECT_EQ(parameter_block.LowerBoundForParameter(1),
 232             -std::numeric_limits<double>::max());
 233
 234   EXPECT_EQ(parameter_block.UpperBoundForParameter(0),
 235             std::numeric_limits<double>::max());
 236   EXPECT_EQ(parameter_block.UpperBoundForParameter(1), 1.0);
 237 }
 238
 239 TEST(ParameterBlock, PlusWithBoundsConstraints) {
 240   double x[] = {1.0, 0.0};
 241   double delta[] = {2.0, -10.0};
 242   ParameterBlock parameter_block(x, 2, -1, NULL);
 243   parameter_block.SetUpperBound(0, 2.0);
 244   parameter_block.SetLowerBound(1, -1.0);
 245   double x_plus_delta[2];
 246   parameter_block.Plus(x, delta, x_plus_delta);
 247   EXPECT_EQ(x_plus_delta[0], 2.0);
 248   EXPECT_EQ(x_plus_delta[1], -1.0);
 249 }
 250
 251 }  // namespace internal
 252 }  // namespace ceres