internal/ceres/program_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/program.h"
  32
  33 #include <limits>
  34 #include <cmath>
  35 #include <vector>
  36 #include "ceres/sized_cost_function.h"
  37 #include "ceres/problem_impl.h"
  38 #include "ceres/residual_block.h"
  39 #include "ceres/triplet_sparse_matrix.h"
  40 #include "gtest/gtest.h"
  41
  42 namespace ceres {
  43 namespace internal {
  44
  45 using std::string;
  46 using std::vector;
  47
  48 // A cost function that simply returns its argument.
  49 class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
  50  public:
  51   virtual bool Evaluate(double const* const* parameters,
  52                         double* residuals,
  53                         double** jacobians) const {
  54     residuals[0] = parameters[0][0];
  55     if (jacobians != NULL && jacobians[0] != NULL) {
  56       jacobians[0][0] = 1.0;
  57     }
  58     return true;
  59   }
  60 };
  61
  62 // Templated base class for the CostFunction signatures.
  63 template <int kNumResiduals, int N0, int N1, int N2>
  64 class MockCostFunctionBase : public
  65 SizedCostFunction<kNumResiduals, N0, N1, N2> {
  66  public:
  67   virtual bool Evaluate(double const* const* parameters,
  68                         double* residuals,
  69                         double** jacobians) const {
  70     for (int i = 0; i < kNumResiduals; ++i) {
  71       residuals[i] = kNumResiduals +  N0 + N1 + N2;
  72     }
  73     return true;
  74   }
  75 };
  76
  77 class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
  78 class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
  79 class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
  80
  81 TEST(Program, RemoveFixedBlocksNothingConstant) {
  82   ProblemImpl problem;
  83   double x;
  84   double y;
  85   double z;
  86
  87   problem.AddParameterBlock(&x, 1);
  88   problem.AddParameterBlock(&y, 1);
  89   problem.AddParameterBlock(&z, 1);
  90   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
  91   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
  92   problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
  93
  94   vector<double*> removed_parameter_blocks;
  95   double fixed_cost = 0.0;
  96   string message;
  97   scoped_ptr<Program> reduced_program(
  98       CHECK_NOTNULL(problem
  99                     .program()
 100                     .CreateReducedProgram(&removed_parameter_blocks,
 101                                           &fixed_cost,
 102                                           &message)));
 103
 104   EXPECT_EQ(reduced_program->NumParameterBlocks(), 3);
 105   EXPECT_EQ(reduced_program->NumResidualBlocks(), 3);
 106   EXPECT_EQ(removed_parameter_blocks.size(), 0);
 107   EXPECT_EQ(fixed_cost, 0.0);
 108 }
 109
 110 TEST(Program, RemoveFixedBlocksAllParameterBlocksConstant) {
 111   ProblemImpl problem;
 112   double x = 1.0;
 113
 114   problem.AddParameterBlock(&x, 1);
 115   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
 116   problem.SetParameterBlockConstant(&x);
 117
 118   vector<double*> removed_parameter_blocks;
 119   double fixed_cost = 0.0;
 120   string message;
 121   scoped_ptr<Program> reduced_program(
 122       CHECK_NOTNULL(problem
 123                     .program()
 124                     .CreateReducedProgram(&removed_parameter_blocks,
 125                                           &fixed_cost,
 126                                           &message)));
 127   EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
 128   EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
 129   EXPECT_EQ(removed_parameter_blocks.size(), 1);
 130   EXPECT_EQ(removed_parameter_blocks[0], &x);
 131   EXPECT_EQ(fixed_cost, 9.0);
 132 }
 133
 134
 135 TEST(Program, RemoveFixedBlocksNoResidualBlocks) {
 136   ProblemImpl problem;
 137   double x;
 138   double y;
 139   double z;
 140
 141   problem.AddParameterBlock(&x, 1);
 142   problem.AddParameterBlock(&y, 1);
 143   problem.AddParameterBlock(&z, 1);
 144
 145   vector<double*> removed_parameter_blocks;
 146   double fixed_cost = 0.0;
 147   string message;
 148   scoped_ptr<Program> reduced_program(
 149       CHECK_NOTNULL(problem
 150                     .program()
 151                     .CreateReducedProgram(&removed_parameter_blocks,
 152                                           &fixed_cost,
 153                                           &message)));
 154   EXPECT_EQ(reduced_program->NumParameterBlocks(), 0);
 155   EXPECT_EQ(reduced_program->NumResidualBlocks(), 0);
 156   EXPECT_EQ(removed_parameter_blocks.size(), 3);
 157   EXPECT_EQ(fixed_cost, 0.0);
 158 }
 159
 160 TEST(Program, RemoveFixedBlocksOneParameterBlockConstant) {
 161   ProblemImpl problem;
 162   double x;
 163   double y;
 164   double z;
 165
 166   problem.AddParameterBlock(&x, 1);
 167   problem.AddParameterBlock(&y, 1);
 168   problem.AddParameterBlock(&z, 1);
 169
 170   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
 171   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
 172   problem.SetParameterBlockConstant(&x);
 173
 174   vector<double*> removed_parameter_blocks;
 175   double fixed_cost = 0.0;
 176   string message;
 177   scoped_ptr<Program> reduced_program(
 178       CHECK_NOTNULL(problem
 179                     .program()
 180                     .CreateReducedProgram(&removed_parameter_blocks,
 181                                           &fixed_cost,
 182                                           &message)));
 183   EXPECT_EQ(reduced_program->NumParameterBlocks(), 1);
 184   EXPECT_EQ(reduced_program->NumResidualBlocks(), 1);
 185 }
 186
 187 TEST(Program, RemoveFixedBlocksNumEliminateBlocks) {
 188   ProblemImpl problem;
 189   double x;
 190   double y;
 191   double z;
 192
 193   problem.AddParameterBlock(&x, 1);
 194   problem.AddParameterBlock(&y, 1);
 195   problem.AddParameterBlock(&z, 1);
 196   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
 197   problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
 198   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
 199   problem.SetParameterBlockConstant(&x);
 200
 201   vector<double*> removed_parameter_blocks;
 202   double fixed_cost = 0.0;
 203   string message;
 204   scoped_ptr<Program> reduced_program(
 205       CHECK_NOTNULL(problem
 206                     .program()
 207                     .CreateReducedProgram(&removed_parameter_blocks,
 208                                           &fixed_cost,
 209                                           &message)));
 210   EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
 211   EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
 212 }
 213
 214 TEST(Program, RemoveFixedBlocksFixedCost) {
 215   ProblemImpl problem;
 216   double x = 1.23;
 217   double y = 4.56;
 218   double z = 7.89;
 219
 220   problem.AddParameterBlock(&x, 1);
 221   problem.AddParameterBlock(&y, 1);
 222   problem.AddParameterBlock(&z, 1);
 223   problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
 224   problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
 225   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
 226   problem.SetParameterBlockConstant(&x);
 227
 228   ResidualBlock *expected_removed_block =
 229       problem.program().residual_blocks()[0];
 230   scoped_array<double> scratch(
 231       new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
 232   double expected_fixed_cost;
 233   expected_removed_block->Evaluate(true,
 234                                    &expected_fixed_cost,
 235                                    NULL,
 236                                    NULL,
 237                                    scratch.get());
 238
 239
 240   vector<double*> removed_parameter_blocks;
 241   double fixed_cost = 0.0;
 242   string message;
 243   scoped_ptr<Program> reduced_program(
 244       CHECK_NOTNULL(problem
 245                     .program()
 246                     .CreateReducedProgram(&removed_parameter_blocks,
 247                                           &fixed_cost,
 248                                           &message)));
 249
 250   EXPECT_EQ(reduced_program->NumParameterBlocks(), 2);
 251   EXPECT_EQ(reduced_program->NumResidualBlocks(), 2);
 252   EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
 253 }
 254
 255 TEST(Program, CreateJacobianBlockSparsityTranspose) {
 256   ProblemImpl problem;
 257   double x[2];
 258   double y[3];
 259   double z;
 260
 261   problem.AddParameterBlock(x, 2);
 262   problem.AddParameterBlock(y, 3);
 263   problem.AddParameterBlock(&z, 1);
 264
 265   problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
 266   problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
 267   problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
 268   problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
 269   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
 270   problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
 271   problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
 272   problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
 273
 274   TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
 275   {
 276     int* rows = expected_block_sparse_jacobian.mutable_rows();
 277     int* cols = expected_block_sparse_jacobian.mutable_cols();
 278     double* values = expected_block_sparse_jacobian.mutable_values();
 279     rows[0] = 0;
 280     cols[0] = 0;
 281
 282     rows[1] = 2;
 283     cols[1] = 1;
 284     rows[2] = 0;
 285     cols[2] = 1;
 286
 287     rows[3] = 2;
 288     cols[3] = 2;
 289     rows[4] = 1;
 290     cols[4] = 2;
 291
 292     rows[5] = 2;
 293     cols[5] = 3;
 294     rows[6] = 1;
 295     cols[6] = 3;
 296
 297     rows[7] = 0;
 298     cols[7] = 4;
 299     rows[8] = 2;
 300     cols[8] = 4;
 301
 302     rows[9] = 2;
 303     cols[9] = 5;
 304     rows[10] = 1;
 305     cols[10] = 5;
 306
 307     rows[11] = 0;
 308     cols[11] = 6;
 309     rows[12] = 2;
 310     cols[12] = 6;
 311
 312     rows[13] = 1;
 313     cols[13] = 7;
 314     std::fill(values, values + 14, 1.0);
 315     expected_block_sparse_jacobian.set_num_nonzeros(14);
 316   }
 317
 318   Program* program = problem.mutable_program();
 319   program->SetParameterOffsetsAndIndex();
 320
 321   scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
 322       program->CreateJacobianBlockSparsityTranspose());
 323
 324   Matrix expected_dense_jacobian;
 325   expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
 326
 327   Matrix actual_dense_jacobian;
 328   actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
 329   EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
 330 }
 331
 332 template <int kNumResiduals, int kNumParameterBlocks>
 333 class NumParameterBlocksCostFunction : public CostFunction {
 334  public:
 335   NumParameterBlocksCostFunction() {
 336     set_num_residuals(kNumResiduals);
 337     for (int i = 0; i < kNumParameterBlocks; ++i) {
 338       mutable_parameter_block_sizes()->push_back(1);
 339     }
 340   }
 341
 342   virtual ~NumParameterBlocksCostFunction() {
 343   }
 344
 345   virtual bool Evaluate(double const* const* parameters,
 346                         double* residuals,
 347                         double** jacobians) const {
 348     return true;
 349   }
 350 };
 351
 352 TEST(Program, ReallocationInCreateJacobianBlockSparsityTranspose) {
 353   // CreateJacobianBlockSparsityTranspose starts with a conservative
 354   // estimate of the size of the sparsity pattern. This test ensures
 355   // that when those estimates are violated, the reallocation/resizing
 356   // logic works correctly.
 357
 358   ProblemImpl problem;
 359   double x[20];
 360
 361   vector<double*> parameter_blocks;
 362   for (int i = 0; i < 20; ++i) {
 363     problem.AddParameterBlock(x + i, 1);
 364     parameter_blocks.push_back(x + i);
 365   }
 366
 367   problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
 368                            NULL,
 369                            parameter_blocks);
 370
 371   TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
 372   {
 373     int* rows = expected_block_sparse_jacobian.mutable_rows();
 374     int* cols = expected_block_sparse_jacobian.mutable_cols();
 375     for (int i = 0; i < 20; ++i) {
 376       rows[i] = i;
 377       cols[i] = 0;
 378     }
 379
 380     double* values = expected_block_sparse_jacobian.mutable_values();
 381     std::fill(values, values + 20, 1.0);
 382     expected_block_sparse_jacobian.set_num_nonzeros(20);
 383   }
 384
 385   Program* program = problem.mutable_program();
 386   program->SetParameterOffsetsAndIndex();
 387
 388   scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
 389       program->CreateJacobianBlockSparsityTranspose());
 390
 391   Matrix expected_dense_jacobian;
 392   expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
 393
 394   Matrix actual_dense_jacobian;
 395   actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
 396   EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
 397 }
 398
 399 TEST(Program, ProblemHasNanParameterBlocks) {
 400   ProblemImpl problem;
 401   double x[2];
 402   x[0] = 1.0;
 403   x[1] = std::numeric_limits<double>::quiet_NaN();
 404   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
 405   string error;
 406   EXPECT_FALSE(problem.program().ParameterBlocksAreFinite(&error));
 407   EXPECT_NE(error.find("has at least one invalid value"),
 408             string::npos) << error;
 409 }
 410
 411 TEST(Program, InfeasibleParameterBlock) {
 412   ProblemImpl problem;
 413   double x[] = {0.0, 0.0};
 414   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
 415   problem.SetParameterLowerBound(x, 0, 2.0);
 416   problem.SetParameterUpperBound(x, 0, 1.0);
 417   string error;
 418   EXPECT_FALSE(problem.program().IsFeasible(&error));
 419   EXPECT_NE(error.find("infeasible bound"), string::npos) << error;
 420 }
 421
 422 TEST(Program, InfeasibleConstantParameterBlock) {
 423   ProblemImpl problem;
 424   double x[] = {0.0, 0.0};
 425   problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
 426   problem.SetParameterLowerBound(x, 0, 1.0);
 427   problem.SetParameterUpperBound(x, 0, 2.0);
 428   problem.SetParameterBlockConstant(x);
 429   string error;
 430   EXPECT_FALSE(problem.program().IsFeasible(&error));
 431   EXPECT_NE(error.find("infeasible value"), string::npos) << error;
 432 }
 433
 434 }  // namespace internal
 435 }  // namespace ceres