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