internal/ceres/cost_function_to_functor_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: sameeragarwal@google.com (Sameer Agarwal)
  30
  31 #include "ceres/cost_function_to_functor.h"
  32 #include "ceres/dynamic_autodiff_cost_function.h"
  33 #include "ceres/dynamic_cost_function_to_functor.h"
  34 #include "ceres/autodiff_cost_function.h"
  35 #include "gtest/gtest.h"
  36
  37 namespace ceres {
  38 namespace internal {
  39
  40 using std::vector;
  41 const double kTolerance = 1e-18;
  42
  43 void ExpectCostFunctionsAreEqual(const CostFunction& cost_function,
  44                                  const CostFunction& actual_cost_function) {
  45   EXPECT_EQ(cost_function.num_residuals(),
  46             actual_cost_function.num_residuals());
  47   const int num_residuals = cost_function.num_residuals();
  48   const vector<int32>& parameter_block_sizes =
  49       cost_function.parameter_block_sizes();
  50   const vector<int32>& actual_parameter_block_sizes =
  51       actual_cost_function.parameter_block_sizes();
  52   EXPECT_EQ(parameter_block_sizes.size(),
  53             actual_parameter_block_sizes.size());
  54
  55   int num_parameters = 0;
  56   for (int i = 0; i < parameter_block_sizes.size(); ++i) {
  57     EXPECT_EQ(parameter_block_sizes[i], actual_parameter_block_sizes[i]);
  58     num_parameters += parameter_block_sizes[i];
  59   }
  60
  61   scoped_array<double> parameters(new double[num_parameters]);
  62   for (int i = 0; i < num_parameters; ++i) {
  63     parameters[i] = static_cast<double>(i) + 1.0;
  64   }
  65
  66   scoped_array<double> residuals(new double[num_residuals]);
  67   scoped_array<double> jacobians(new double[num_parameters * num_residuals]);
  68
  69   scoped_array<double> actual_residuals(new double[num_residuals]);
  70   scoped_array<double> actual_jacobians
  71       (new double[num_parameters * num_residuals]);
  72
  73   scoped_array<double*> parameter_blocks(
  74       new double*[parameter_block_sizes.size()]);
  75   scoped_array<double*> jacobian_blocks(
  76       new double*[parameter_block_sizes.size()]);
  77   scoped_array<double*> actual_jacobian_blocks(
  78       new double*[parameter_block_sizes.size()]);
  79
  80   num_parameters = 0;
  81   for (int i = 0; i < parameter_block_sizes.size(); ++i) {
  82     parameter_blocks[i] = parameters.get() + num_parameters;
  83     jacobian_blocks[i] = jacobians.get() + num_parameters * num_residuals;
  84     actual_jacobian_blocks[i] =
  85         actual_jacobians.get() + num_parameters * num_residuals;
  86     num_parameters += parameter_block_sizes[i];
  87   }
  88
  89   EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(),
  90                                      residuals.get(), NULL));
  91   EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(),
  92                                             actual_residuals.get(), NULL));
  93   for (int i = 0; i < num_residuals; ++i) {
  94     EXPECT_NEAR(residuals[i], actual_residuals[i], kTolerance)
  95         << "residual id: " << i;
  96   }
  97
  98
  99   EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(),
 100                                      residuals.get(),
 101                                      jacobian_blocks.get()));
 102   EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(),
 103                                             actual_residuals.get(),
 104                                             actual_jacobian_blocks.get()));
 105   for (int i = 0; i < num_residuals; ++i) {
 106     EXPECT_NEAR(residuals[i], actual_residuals[i], kTolerance)
 107         << "residual : " << i;
 108   }
 109
 110   for (int i = 0; i < num_residuals * num_parameters; ++i) {
 111     EXPECT_NEAR(jacobians[i], actual_jacobians[i], kTolerance)
 112         << "jacobian : " << i << " "
 113         << jacobians[i] << " " << actual_jacobians[i];
 114   }
 115 }
 116
 117 struct OneParameterBlockFunctor {
 118  public:
 119   template <typename T>
 120   bool operator()(const T* x1, T* residuals) const {
 121     residuals[0] = x1[0] * x1[0];
 122     residuals[1] = x1[1] * x1[1];
 123     return true;
 124   }
 125 };
 126
 127 struct TwoParameterBlockFunctor {
 128  public:
 129   template <typename T>
 130   bool operator()(const T* x1, const T* x2, T* residuals) const {
 131     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0];
 132     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1];
 133     return true;
 134   }
 135 };
 136
 137 struct ThreeParameterBlockFunctor {
 138  public:
 139   template <typename T>
 140   bool operator()(const T* x1, const T* x2, const T* x3, T* residuals) const {
 141     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0];
 142     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1];
 143     return true;
 144   }
 145 };
 146
 147 struct FourParameterBlockFunctor {
 148  public:
 149   template <typename T>
 150   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 151                   T* residuals) const {
 152     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 153         + x4[0] * x4[0];
 154     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 155         + x4[1] * x4[1];
 156     return true;
 157   }
 158 };
 159
 160 struct FiveParameterBlockFunctor {
 161  public:
 162   template <typename T>
 163   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 164                   const T* x5, T* residuals) const {
 165     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 166         + x4[0] * x4[0] + x5[0] * x5[0];
 167     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 168         + x4[1] * x4[1] + x5[1] * x5[1];
 169     return true;
 170   }
 171 };
 172
 173 struct SixParameterBlockFunctor {
 174  public:
 175   template <typename T>
 176   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 177                   const T* x5, const T* x6,  T* residuals) const {
 178     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 179         + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0];
 180     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 181         + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1];
 182     return true;
 183   }
 184 };
 185
 186 struct SevenParameterBlockFunctor {
 187  public:
 188   template <typename T>
 189   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 190                   const T* x5, const T* x6, const T* x7, T* residuals) const {
 191     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 192         + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0];
 193     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 194         + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1];
 195     return true;
 196   }
 197 };
 198
 199 struct EightParameterBlockFunctor {
 200  public:
 201   template <typename T>
 202   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 203                   const T* x5, const T* x6, const T* x7, const T* x8,
 204                   T* residuals) const {
 205     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 206         + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0]
 207         + x8[0] * x8[0];
 208     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 209         + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1]
 210         + x8[1] * x8[1];
 211     return true;
 212   }
 213 };
 214
 215 struct NineParameterBlockFunctor {
 216  public:
 217   template <typename T>
 218   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 219                   const T* x5, const T* x6, const T* x7, const T* x8,
 220                   const T* x9, T* residuals) const {
 221     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 222         + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0]
 223         + x8[0] * x8[0] + x9[0] * x9[0];
 224     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 225         + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1]
 226         + x8[1] * x8[1] + x9[1] * x9[1];
 227     return true;
 228   }
 229 };
 230
 231 struct TenParameterBlockFunctor {
 232  public:
 233   template <typename T>
 234   bool operator()(const T* x1, const T* x2, const T* x3, const T* x4,
 235                   const T* x5, const T* x6, const T* x7, const T* x8,
 236                   const T* x9, const T* x10, T* residuals) const {
 237     residuals[0] = x1[0] * x1[0]  + x2[0] * x2[0] + x3[0] * x3[0]
 238         + x4[0] * x4[0] + x5[0] * x5[0] + x6[0] * x6[0] + x7[0] * x7[0]
 239         + x8[0] * x8[0] + x9[0] * x9[0] + x10[0] * x10[0];
 240     residuals[1] = x1[1] * x1[1]  + x2[1] * x2[1] + x3[1] * x3[1]
 241         + x4[1] * x4[1] + x5[1] * x5[1] + x6[1] * x6[1] + x7[1] * x7[1]
 242         + x8[1] * x8[1] + x9[1] * x9[1] + x10[1] * x10[1];
 243     return true;
 244   }
 245 };
 246
 247 class DynamicTwoParameterBlockFunctor {
 248  public:
 249   template <typename T>
 250   bool operator()(T const* const* parameters, T* residuals) const {
 251     for (int i = 0; i < 2; ++i) {
 252       residuals[0] = parameters[i][0] * parameters[i][0];
 253       residuals[1] = parameters[i][1] * parameters[i][1];
 254     }
 255     return true;
 256   }
 257 };
 258
 259 #define TEST_BODY(NAME)                                                 \
 260   TEST(CostFunctionToFunctor, NAME) {                                   \
 261     scoped_ptr<CostFunction> cost_function(                             \
 262         new AutoDiffCostFunction<                                       \
 263             CostFunctionToFunctor<2, PARAMETER_BLOCK_SIZES >,           \
 264                 2, PARAMETER_BLOCK_SIZES>(new CostFunctionToFunctor<    \
 265                     2, PARAMETER_BLOCK_SIZES >(                         \
 266                         new AutoDiffCostFunction<                       \
 267                             NAME##Functor, 2, PARAMETER_BLOCK_SIZES >(  \
 268                   new NAME##Functor))));                                \
 269                                                                         \
 270 scoped_ptr<CostFunction> actual_cost_function(                          \
 271     new AutoDiffCostFunction<NAME##Functor, 2, PARAMETER_BLOCK_SIZES >( \
 272         new NAME##Functor));                                            \
 273 ExpectCostFunctionsAreEqual(*cost_function, *actual_cost_function);     \
 274 }
 275
 276 #define PARAMETER_BLOCK_SIZES 2
 277 TEST_BODY(OneParameterBlock)
 278 #undef PARAMETER_BLOCK_SIZES
 279
 280 #define PARAMETER_BLOCK_SIZES 2,2
 281 TEST_BODY(TwoParameterBlock)
 282 #undef PARAMETER_BLOCK_SIZES
 283
 284 #define PARAMETER_BLOCK_SIZES 2,2,2
 285 TEST_BODY(ThreeParameterBlock)
 286 #undef PARAMETER_BLOCK_SIZES
 287
 288 #define PARAMETER_BLOCK_SIZES 2,2,2,2
 289 TEST_BODY(FourParameterBlock)
 290 #undef PARAMETER_BLOCK_SIZES
 291
 292 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2
 293 TEST_BODY(FiveParameterBlock)
 294 #undef PARAMETER_BLOCK_SIZES
 295
 296 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2
 297 TEST_BODY(SixParameterBlock)
 298 #undef PARAMETER_BLOCK_SIZES
 299
 300 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2
 301 TEST_BODY(SevenParameterBlock)
 302 #undef PARAMETER_BLOCK_SIZES
 303
 304 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2
 305 TEST_BODY(EightParameterBlock)
 306 #undef PARAMETER_BLOCK_SIZES
 307
 308 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2,2
 309 TEST_BODY(NineParameterBlock)
 310 #undef PARAMETER_BLOCK_SIZES
 311
 312 #define PARAMETER_BLOCK_SIZES 2,2,2,2,2,2,2,2,2,2
 313 TEST_BODY(TenParameterBlock)
 314 #undef PARAMETER_BLOCK_SIZES
 315
 316 #undef TEST_BODY
 317
 318 TEST(CostFunctionToFunctor, DynamicNumberOfResiduals) {
 319   scoped_ptr<CostFunction> cost_function(
 320       new AutoDiffCostFunction<
 321       CostFunctionToFunctor<ceres::DYNAMIC, 2, 2 >, ceres::DYNAMIC, 2, 2>(
 322           new CostFunctionToFunctor<ceres::DYNAMIC, 2, 2 >(
 323               new AutoDiffCostFunction<TwoParameterBlockFunctor, 2, 2, 2 >(
 324                   new TwoParameterBlockFunctor)), 2));
 325
 326   scoped_ptr<CostFunction> actual_cost_function(
 327       new AutoDiffCostFunction<TwoParameterBlockFunctor, 2, 2, 2 >(
 328           new TwoParameterBlockFunctor));
 329   ExpectCostFunctionsAreEqual(*cost_function, *actual_cost_function);
 330 }
 331
 332 TEST(CostFunctionToFunctor, DynamicCostFunctionToFunctor) {
 333   DynamicAutoDiffCostFunction<DynamicTwoParameterBlockFunctor>*
 334       actual_cost_function(
 335       new DynamicAutoDiffCostFunction<DynamicTwoParameterBlockFunctor>(
 336           new DynamicTwoParameterBlockFunctor));
 337   actual_cost_function->AddParameterBlock(2);
 338   actual_cost_function->AddParameterBlock(2);
 339   actual_cost_function->SetNumResiduals(2);
 340
 341   DynamicAutoDiffCostFunction<DynamicCostFunctionToFunctor> cost_function(
 342       new DynamicCostFunctionToFunctor(actual_cost_function));
 343   cost_function.AddParameterBlock(2);
 344   cost_function.AddParameterBlock(2);
 345   cost_function.SetNumResiduals(2);
 346
 347   ExpectCostFunctionsAreEqual(cost_function, *actual_cost_function);
 348 }
 349
 350 }  // namespace internal
 351 }  // namespace ceres