1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 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 // mierle (at) gmail.com (Keir Mierle) 31 32 #include <cstddef> 33 34 #include "ceres/dynamic_numeric_diff_cost_function.h" 35 #include "ceres/internal/scoped_ptr.h" 36 #include "gtest/gtest.h" 37 38 namespace ceres { 39 namespace internal { 40 41 const double kTolerance = 1e-6; 42 43 // Takes 2 parameter blocks: 44 // parameters[0] is size 10. 45 // parameters[1] is size 5. 46 // Emits 21 residuals: 47 // A: i - parameters[0][i], for i in [0,10) -- this is 10 residuals 48 // B: parameters[0][i] - i, for i in [0,10) -- this is another 10. 49 // C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i]) 50 class MyCostFunctor { 51 public: 52 bool operator()(double const* const* parameters, double* residuals) const { 53 const double* params0 = parameters[0]; 54 int r = 0; 55 for (int i = 0; i < 10; ++i) { 56 residuals[r++] = i - params0[i]; 57 residuals[r++] = params0[i] - i; 58 } 59 60 double c_residual = 0.0; 61 for (int i = 0; i < 10; ++i) { 62 c_residual += pow(params0[i], 2) - 8.0 * params0[i]; 63 } 64 65 const double* params1 = parameters[1]; 66 for (int i = 0; i < 5; ++i) { 67 c_residual += params1[i]; 68 } 69 residuals[r++] = c_residual; 70 return true; 71 } 72 }; 73 74 TEST(DynamicNumericdiffCostFunctionTest, TestResiduals) { 75 vector<double> param_block_0(10, 0.0); 76 vector<double> param_block_1(5, 0.0); 77 DynamicNumericDiffCostFunction<MyCostFunctor> cost_function( 78 new MyCostFunctor()); 79 cost_function.AddParameterBlock(param_block_0.size()); 80 cost_function.AddParameterBlock(param_block_1.size()); 81 cost_function.SetNumResiduals(21); 82 83 // Test residual computation. 84 vector<double> residuals(21, -100000); 85 vector<double*> parameter_blocks(2); 86 parameter_blocks[0] = ¶m_block_0[0]; 87 parameter_blocks[1] = ¶m_block_1[0]; 88 EXPECT_TRUE(cost_function.Evaluate(¶meter_blocks[0], 89 residuals.data(), 90 NULL)); 91 for (int r = 0; r < 10; ++r) { 92 EXPECT_EQ(1.0 * r, residuals.at(r * 2)); 93 EXPECT_EQ(-1.0 * r, residuals.at(r * 2 + 1)); 94 } 95 EXPECT_EQ(0, residuals.at(20)); 96 } 97 98 99 TEST(DynamicNumericdiffCostFunctionTest, TestJacobian) { 100 // Test the residual counting. 101 vector<double> param_block_0(10, 0.0); 102 for (int i = 0; i < 10; ++i) { 103 param_block_0[i] = 2 * i; 104 } 105 vector<double> param_block_1(5, 0.0); 106 DynamicNumericDiffCostFunction<MyCostFunctor> cost_function( 107 new MyCostFunctor()); 108 cost_function.AddParameterBlock(param_block_0.size()); 109 cost_function.AddParameterBlock(param_block_1.size()); 110 cost_function.SetNumResiduals(21); 111 112 // Prepare the residuals. 113 vector<double> residuals(21, -100000); 114 115 // Prepare the parameters. 116 vector<double*> parameter_blocks(2); 117 parameter_blocks[0] = ¶m_block_0[0]; 118 parameter_blocks[1] = ¶m_block_1[0]; 119 120 // Prepare the jacobian. 121 vector<vector<double> > jacobian_vect(2); 122 jacobian_vect[0].resize(21 * 10, -100000); 123 jacobian_vect[1].resize(21 * 5, -100000); 124 vector<double*> jacobian; 125 jacobian.push_back(jacobian_vect[0].data()); 126 jacobian.push_back(jacobian_vect[1].data()); 127 128 // Test jacobian computation. 129 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 130 residuals.data(), 131 jacobian.data())); 132 133 for (int r = 0; r < 10; ++r) { 134 EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 135 EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 136 } 137 EXPECT_EQ(420, residuals.at(20)); 138 for (int p = 0; p < 10; ++p) { 139 // Check "A" Jacobian. 140 EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance); 141 // Check "B" Jacobian. 142 EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance); 143 jacobian_vect[0][2*p * 10 + p] = 0.0; 144 jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; 145 } 146 147 // Check "C" Jacobian for first parameter block. 148 for (int p = 0; p < 10; ++p) { 149 EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance); 150 jacobian_vect[0][20 * 10 + p] = 0.0; 151 } 152 for (int i = 0; i < jacobian_vect[0].size(); ++i) { 153 EXPECT_NEAR(0.0, jacobian_vect[0][i], kTolerance); 154 } 155 156 // Check "C" Jacobian for second parameter block. 157 for (int p = 0; p < 5; ++p) { 158 EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance); 159 jacobian_vect[1][20 * 5 + p] = 0.0; 160 } 161 for (int i = 0; i < jacobian_vect[1].size(); ++i) { 162 EXPECT_NEAR(0.0, jacobian_vect[1][i], kTolerance); 163 } 164 } 165 166 TEST(DynamicNumericdiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) { // NOLINT 167 // Test the residual counting. 168 vector<double> param_block_0(10, 0.0); 169 for (int i = 0; i < 10; ++i) { 170 param_block_0[i] = 2 * i; 171 } 172 vector<double> param_block_1(5, 0.0); 173 DynamicNumericDiffCostFunction<MyCostFunctor> cost_function( 174 new MyCostFunctor()); 175 cost_function.AddParameterBlock(param_block_0.size()); 176 cost_function.AddParameterBlock(param_block_1.size()); 177 cost_function.SetNumResiduals(21); 178 179 // Prepare the residuals. 180 vector<double> residuals(21, -100000); 181 182 // Prepare the parameters. 183 vector<double*> parameter_blocks(2); 184 parameter_blocks[0] = ¶m_block_0[0]; 185 parameter_blocks[1] = ¶m_block_1[0]; 186 187 // Prepare the jacobian. 188 vector<vector<double> > jacobian_vect(2); 189 jacobian_vect[0].resize(21 * 10, -100000); 190 jacobian_vect[1].resize(21 * 5, -100000); 191 vector<double*> jacobian; 192 jacobian.push_back(NULL); 193 jacobian.push_back(jacobian_vect[1].data()); 194 195 // Test jacobian computation. 196 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 197 residuals.data(), 198 jacobian.data())); 199 200 for (int r = 0; r < 10; ++r) { 201 EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 202 EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 203 } 204 EXPECT_EQ(420, residuals.at(20)); 205 206 // Check "C" Jacobian for second parameter block. 207 for (int p = 0; p < 5; ++p) { 208 EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance); 209 jacobian_vect[1][20 * 5 + p] = 0.0; 210 } 211 for (int i = 0; i < jacobian_vect[1].size(); ++i) { 212 EXPECT_EQ(0.0, jacobian_vect[1][i]); 213 } 214 } 215 216 TEST(DynamicNumericdiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) { // NOLINT 217 // Test the residual counting. 218 vector<double> param_block_0(10, 0.0); 219 for (int i = 0; i < 10; ++i) { 220 param_block_0[i] = 2 * i; 221 } 222 vector<double> param_block_1(5, 0.0); 223 DynamicNumericDiffCostFunction<MyCostFunctor> cost_function( 224 new MyCostFunctor()); 225 cost_function.AddParameterBlock(param_block_0.size()); 226 cost_function.AddParameterBlock(param_block_1.size()); 227 cost_function.SetNumResiduals(21); 228 229 // Prepare the residuals. 230 vector<double> residuals(21, -100000); 231 232 // Prepare the parameters. 233 vector<double*> parameter_blocks(2); 234 parameter_blocks[0] = ¶m_block_0[0]; 235 parameter_blocks[1] = ¶m_block_1[0]; 236 237 // Prepare the jacobian. 238 vector<vector<double> > jacobian_vect(2); 239 jacobian_vect[0].resize(21 * 10, -100000); 240 jacobian_vect[1].resize(21 * 5, -100000); 241 vector<double*> jacobian; 242 jacobian.push_back(jacobian_vect[0].data()); 243 jacobian.push_back(NULL); 244 245 // Test jacobian computation. 246 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 247 residuals.data(), 248 jacobian.data())); 249 250 for (int r = 0; r < 10; ++r) { 251 EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 252 EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 253 } 254 EXPECT_EQ(420, residuals.at(20)); 255 for (int p = 0; p < 10; ++p) { 256 // Check "A" Jacobian. 257 EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance); 258 // Check "B" Jacobian. 259 EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance); 260 jacobian_vect[0][2*p * 10 + p] = 0.0; 261 jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; 262 } 263 264 // Check "C" Jacobian for first parameter block. 265 for (int p = 0; p < 10; ++p) { 266 EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance); 267 jacobian_vect[0][20 * 10 + p] = 0.0; 268 } 269 for (int i = 0; i < jacobian_vect[0].size(); ++i) { 270 EXPECT_EQ(0.0, jacobian_vect[0][i]); 271 } 272 } 273 274 // Takes 3 parameter blocks: 275 // parameters[0] (x) is size 1. 276 // parameters[1] (y) is size 2. 277 // parameters[2] (z) is size 3. 278 // Emits 7 residuals: 279 // A: x[0] (= sum_x) 280 // B: y[0] + 2.0 * y[1] (= sum_y) 281 // C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z) 282 // D: sum_x * sum_y 283 // E: sum_y * sum_z 284 // F: sum_x * sum_z 285 // G: sum_x * sum_y * sum_z 286 class MyThreeParameterCostFunctor { 287 public: 288 template <typename T> 289 bool operator()(T const* const* parameters, T* residuals) const { 290 const T* x = parameters[0]; 291 const T* y = parameters[1]; 292 const T* z = parameters[2]; 293 294 T sum_x = x[0]; 295 T sum_y = y[0] + 2.0 * y[1]; 296 T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2]; 297 298 residuals[0] = sum_x; 299 residuals[1] = sum_y; 300 residuals[2] = sum_z; 301 residuals[3] = sum_x * sum_y; 302 residuals[4] = sum_y * sum_z; 303 residuals[5] = sum_x * sum_z; 304 residuals[6] = sum_x * sum_y * sum_z; 305 return true; 306 } 307 }; 308 309 class ThreeParameterCostFunctorTest : public ::testing::Test { 310 protected: 311 virtual void SetUp() { 312 // Prepare the parameters. 313 x_.resize(1); 314 x_[0] = 0.0; 315 316 y_.resize(2); 317 y_[0] = 1.0; 318 y_[1] = 3.0; 319 320 z_.resize(3); 321 z_[0] = 2.0; 322 z_[1] = 4.0; 323 z_[2] = 6.0; 324 325 parameter_blocks_.resize(3); 326 parameter_blocks_[0] = &x_[0]; 327 parameter_blocks_[1] = &y_[0]; 328 parameter_blocks_[2] = &z_[0]; 329 330 // Prepare the cost function. 331 typedef DynamicNumericDiffCostFunction<MyThreeParameterCostFunctor> 332 DynamicMyThreeParameterCostFunction; 333 DynamicMyThreeParameterCostFunction * cost_function = 334 new DynamicMyThreeParameterCostFunction( 335 new MyThreeParameterCostFunctor()); 336 cost_function->AddParameterBlock(1); 337 cost_function->AddParameterBlock(2); 338 cost_function->AddParameterBlock(3); 339 cost_function->SetNumResiduals(7); 340 341 cost_function_.reset(cost_function); 342 343 // Setup jacobian data. 344 jacobian_vect_.resize(3); 345 jacobian_vect_[0].resize(7 * x_.size(), -100000); 346 jacobian_vect_[1].resize(7 * y_.size(), -100000); 347 jacobian_vect_[2].resize(7 * z_.size(), -100000); 348 349 // Prepare the expected residuals. 350 const double sum_x = x_[0]; 351 const double sum_y = y_[0] + 2.0 * y_[1]; 352 const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2]; 353 354 expected_residuals_.resize(7); 355 expected_residuals_[0] = sum_x; 356 expected_residuals_[1] = sum_y; 357 expected_residuals_[2] = sum_z; 358 expected_residuals_[3] = sum_x * sum_y; 359 expected_residuals_[4] = sum_y * sum_z; 360 expected_residuals_[5] = sum_x * sum_z; 361 expected_residuals_[6] = sum_x * sum_y * sum_z; 362 363 // Prepare the expected jacobian entries. 364 expected_jacobian_x_.resize(7); 365 expected_jacobian_x_[0] = 1.0; 366 expected_jacobian_x_[1] = 0.0; 367 expected_jacobian_x_[2] = 0.0; 368 expected_jacobian_x_[3] = sum_y; 369 expected_jacobian_x_[4] = 0.0; 370 expected_jacobian_x_[5] = sum_z; 371 expected_jacobian_x_[6] = sum_y * sum_z; 372 373 expected_jacobian_y_.resize(14); 374 expected_jacobian_y_[0] = 0.0; 375 expected_jacobian_y_[1] = 0.0; 376 expected_jacobian_y_[2] = 1.0; 377 expected_jacobian_y_[3] = 2.0; 378 expected_jacobian_y_[4] = 0.0; 379 expected_jacobian_y_[5] = 0.0; 380 expected_jacobian_y_[6] = sum_x; 381 expected_jacobian_y_[7] = 2.0 * sum_x; 382 expected_jacobian_y_[8] = sum_z; 383 expected_jacobian_y_[9] = 2.0 * sum_z; 384 expected_jacobian_y_[10] = 0.0; 385 expected_jacobian_y_[11] = 0.0; 386 expected_jacobian_y_[12] = sum_x * sum_z; 387 expected_jacobian_y_[13] = 2.0 * sum_x * sum_z; 388 389 expected_jacobian_z_.resize(21); 390 expected_jacobian_z_[0] = 0.0; 391 expected_jacobian_z_[1] = 0.0; 392 expected_jacobian_z_[2] = 0.0; 393 expected_jacobian_z_[3] = 0.0; 394 expected_jacobian_z_[4] = 0.0; 395 expected_jacobian_z_[5] = 0.0; 396 expected_jacobian_z_[6] = 1.0; 397 expected_jacobian_z_[7] = 3.0; 398 expected_jacobian_z_[8] = 6.0; 399 expected_jacobian_z_[9] = 0.0; 400 expected_jacobian_z_[10] = 0.0; 401 expected_jacobian_z_[11] = 0.0; 402 expected_jacobian_z_[12] = sum_y; 403 expected_jacobian_z_[13] = 3.0 * sum_y; 404 expected_jacobian_z_[14] = 6.0 * sum_y; 405 expected_jacobian_z_[15] = sum_x; 406 expected_jacobian_z_[16] = 3.0 * sum_x; 407 expected_jacobian_z_[17] = 6.0 * sum_x; 408 expected_jacobian_z_[18] = sum_x * sum_y; 409 expected_jacobian_z_[19] = 3.0 * sum_x * sum_y; 410 expected_jacobian_z_[20] = 6.0 * sum_x * sum_y; 411 } 412 413 protected: 414 vector<double> x_; 415 vector<double> y_; 416 vector<double> z_; 417 418 vector<double*> parameter_blocks_; 419 420 scoped_ptr<CostFunction> cost_function_; 421 422 vector<vector<double> > jacobian_vect_; 423 424 vector<double> expected_residuals_; 425 426 vector<double> expected_jacobian_x_; 427 vector<double> expected_jacobian_y_; 428 vector<double> expected_jacobian_z_; 429 }; 430 431 TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) { 432 vector<double> residuals(7, -100000); 433 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 434 residuals.data(), 435 NULL)); 436 for (int i = 0; i < 7; ++i) { 437 EXPECT_EQ(expected_residuals_[i], residuals[i]); 438 } 439 } 440 441 TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) { 442 vector<double> residuals(7, -100000); 443 444 vector<double*> jacobian; 445 jacobian.push_back(jacobian_vect_[0].data()); 446 jacobian.push_back(jacobian_vect_[1].data()); 447 jacobian.push_back(jacobian_vect_[2].data()); 448 449 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 450 residuals.data(), 451 jacobian.data())); 452 453 for (int i = 0; i < 7; ++i) { 454 EXPECT_EQ(expected_residuals_[i], residuals[i]); 455 } 456 457 for (int i = 0; i < 7; ++i) { 458 EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance); 459 } 460 461 for (int i = 0; i < 14; ++i) { 462 EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance); 463 } 464 465 for (int i = 0; i < 21; ++i) { 466 EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance); 467 } 468 } 469 470 TEST_F(ThreeParameterCostFunctorTest, 471 ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) { 472 vector<double> residuals(7, -100000); 473 474 vector<double*> jacobian; 475 jacobian.push_back(NULL); 476 jacobian.push_back(jacobian_vect_[1].data()); 477 jacobian.push_back(NULL); 478 479 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 480 residuals.data(), 481 jacobian.data())); 482 483 for (int i = 0; i < 7; ++i) { 484 EXPECT_EQ(expected_residuals_[i], residuals[i]); 485 } 486 487 for (int i = 0; i < 14; ++i) { 488 EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance); 489 } 490 } 491 492 TEST_F(ThreeParameterCostFunctorTest, 493 ThreeParameterJacobianWithSecondParameterBlockConstant) { 494 vector<double> residuals(7, -100000); 495 496 vector<double*> jacobian; 497 jacobian.push_back(jacobian_vect_[0].data()); 498 jacobian.push_back(NULL); 499 jacobian.push_back(jacobian_vect_[2].data()); 500 501 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 502 residuals.data(), 503 jacobian.data())); 504 505 for (int i = 0; i < 7; ++i) { 506 EXPECT_EQ(expected_residuals_[i], residuals[i]); 507 } 508 509 for (int i = 0; i < 7; ++i) { 510 EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance); 511 } 512 513 for (int i = 0; i < 21; ++i) { 514 EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance); 515 } 516 } 517 518 } // namespace internal 519 } // namespace ceres 520
