Home | History | Annotate | Download | only in ceres
      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