1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 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: thadh@gmail.com (Thad Hughes) 30// mierle@gmail.com (Keir Mierle) 31// sameeragarwal@google.com (Sameer Agarwal) 32 33#include <cstddef> 34 35#include "ceres/dynamic_autodiff_cost_function.h" 36#include "ceres/internal/scoped_ptr.h" 37#include "gtest/gtest.h" 38 39namespace ceres { 40namespace internal { 41 42// Takes 2 parameter blocks: 43// parameters[0] is size 10. 44// parameters[1] is size 5. 45// Emits 21 residuals: 46// A: i - parameters[0][i], for i in [0,10) -- this is 10 residuals 47// B: parameters[0][i] - i, for i in [0,10) -- this is another 10. 48// C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i]) 49class MyCostFunctor { 50 public: 51 template <typename T> 52 bool operator()(T const* const* parameters, T* residuals) const { 53 const T* params0 = parameters[0]; 54 int r = 0; 55 for (int i = 0; i < 10; ++i) { 56 residuals[r++] = T(i) - params0[i]; 57 residuals[r++] = params0[i] - T(i); 58 } 59 60 T c_residual(0.0); 61 for (int i = 0; i < 10; ++i) { 62 c_residual += pow(params0[i], 2) - T(8) * params0[i]; 63 } 64 65 const T* 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 74TEST(DynamicAutodiffCostFunctionTest, TestResiduals) { 75 vector<double> param_block_0(10, 0.0); 76 vector<double> param_block_1(5, 0.0); 77 DynamicAutoDiffCostFunction<MyCostFunctor, 3> 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 98TEST(DynamicAutodiffCostFunctionTest, TestJacobian) { 99 // Test the residual counting. 100 vector<double> param_block_0(10, 0.0); 101 for (int i = 0; i < 10; ++i) { 102 param_block_0[i] = 2 * i; 103 } 104 vector<double> param_block_1(5, 0.0); 105 DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 106 new MyCostFunctor()); 107 cost_function.AddParameterBlock(param_block_0.size()); 108 cost_function.AddParameterBlock(param_block_1.size()); 109 cost_function.SetNumResiduals(21); 110 111 // Prepare the residuals. 112 vector<double> residuals(21, -100000); 113 114 // Prepare the parameters. 115 vector<double*> parameter_blocks(2); 116 parameter_blocks[0] = ¶m_block_0[0]; 117 parameter_blocks[1] = ¶m_block_1[0]; 118 119 // Prepare the jacobian. 120 vector<vector<double> > jacobian_vect(2); 121 jacobian_vect[0].resize(21 * 10, -100000); 122 jacobian_vect[1].resize(21 * 5, -100000); 123 vector<double*> jacobian; 124 jacobian.push_back(jacobian_vect[0].data()); 125 jacobian.push_back(jacobian_vect[1].data()); 126 127 // Test jacobian computation. 128 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 129 residuals.data(), 130 jacobian.data())); 131 132 for (int r = 0; r < 10; ++r) { 133 EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 134 EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 135 } 136 EXPECT_EQ(420, residuals.at(20)); 137 for (int p = 0; p < 10; ++p) { 138 // Check "A" Jacobian. 139 EXPECT_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]); 140 // Check "B" Jacobian. 141 EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]); 142 jacobian_vect[0][2*p * 10 + p] = 0.0; 143 jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; 144 } 145 146 // Check "C" Jacobian for first parameter block. 147 for (int p = 0; p < 10; ++p) { 148 EXPECT_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]); 149 jacobian_vect[0][20 * 10 + p] = 0.0; 150 } 151 for (int i = 0; i < jacobian_vect[0].size(); ++i) { 152 EXPECT_EQ(0.0, jacobian_vect[0][i]); 153 } 154 155 // Check "C" Jacobian for second parameter block. 156 for (int p = 0; p < 5; ++p) { 157 EXPECT_EQ(1.0, jacobian_vect[1][20 * 5 + p]); 158 jacobian_vect[1][20 * 5 + p] = 0.0; 159 } 160 for (int i = 0; i < jacobian_vect[1].size(); ++i) { 161 EXPECT_EQ(0.0, jacobian_vect[1][i]); 162 } 163} 164 165TEST(DynamicAutodiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) { 166 // Test the residual counting. 167 vector<double> param_block_0(10, 0.0); 168 for (int i = 0; i < 10; ++i) { 169 param_block_0[i] = 2 * i; 170 } 171 vector<double> param_block_1(5, 0.0); 172 DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 173 new MyCostFunctor()); 174 cost_function.AddParameterBlock(param_block_0.size()); 175 cost_function.AddParameterBlock(param_block_1.size()); 176 cost_function.SetNumResiduals(21); 177 178 // Prepare the residuals. 179 vector<double> residuals(21, -100000); 180 181 // Prepare the parameters. 182 vector<double*> parameter_blocks(2); 183 parameter_blocks[0] = ¶m_block_0[0]; 184 parameter_blocks[1] = ¶m_block_1[0]; 185 186 // Prepare the jacobian. 187 vector<vector<double> > jacobian_vect(2); 188 jacobian_vect[0].resize(21 * 10, -100000); 189 jacobian_vect[1].resize(21 * 5, -100000); 190 vector<double*> jacobian; 191 jacobian.push_back(NULL); 192 jacobian.push_back(jacobian_vect[1].data()); 193 194 // Test jacobian computation. 195 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 196 residuals.data(), 197 jacobian.data())); 198 199 for (int r = 0; r < 10; ++r) { 200 EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 201 EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 202 } 203 EXPECT_EQ(420, residuals.at(20)); 204 205 // Check "C" Jacobian for second parameter block. 206 for (int p = 0; p < 5; ++p) { 207 EXPECT_EQ(1.0, jacobian_vect[1][20 * 5 + p]); 208 jacobian_vect[1][20 * 5 + p] = 0.0; 209 } 210 for (int i = 0; i < jacobian_vect[1].size(); ++i) { 211 EXPECT_EQ(0.0, jacobian_vect[1][i]); 212 } 213} 214 215TEST(DynamicAutodiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) { 216 // Test the residual counting. 217 vector<double> param_block_0(10, 0.0); 218 for (int i = 0; i < 10; ++i) { 219 param_block_0[i] = 2 * i; 220 } 221 vector<double> param_block_1(5, 0.0); 222 DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function( 223 new MyCostFunctor()); 224 cost_function.AddParameterBlock(param_block_0.size()); 225 cost_function.AddParameterBlock(param_block_1.size()); 226 cost_function.SetNumResiduals(21); 227 228 // Prepare the residuals. 229 vector<double> residuals(21, -100000); 230 231 // Prepare the parameters. 232 vector<double*> parameter_blocks(2); 233 parameter_blocks[0] = ¶m_block_0[0]; 234 parameter_blocks[1] = ¶m_block_1[0]; 235 236 // Prepare the jacobian. 237 vector<vector<double> > jacobian_vect(2); 238 jacobian_vect[0].resize(21 * 10, -100000); 239 jacobian_vect[1].resize(21 * 5, -100000); 240 vector<double*> jacobian; 241 jacobian.push_back(jacobian_vect[0].data()); 242 jacobian.push_back(NULL); 243 244 // Test jacobian computation. 245 EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(), 246 residuals.data(), 247 jacobian.data())); 248 249 for (int r = 0; r < 10; ++r) { 250 EXPECT_EQ(-1.0 * r, residuals.at(r * 2)); 251 EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1)); 252 } 253 EXPECT_EQ(420, residuals.at(20)); 254 for (int p = 0; p < 10; ++p) { 255 // Check "A" Jacobian. 256 EXPECT_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]); 257 // Check "B" Jacobian. 258 EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]); 259 jacobian_vect[0][2*p * 10 + p] = 0.0; 260 jacobian_vect[0][(2*p+1) * 10 + p] = 0.0; 261 } 262 263 // Check "C" Jacobian for first parameter block. 264 for (int p = 0; p < 10; ++p) { 265 EXPECT_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]); 266 jacobian_vect[0][20 * 10 + p] = 0.0; 267 } 268 for (int i = 0; i < jacobian_vect[0].size(); ++i) { 269 EXPECT_EQ(0.0, jacobian_vect[0][i]); 270 } 271} 272 273// Takes 3 parameter blocks: 274// parameters[0] (x) is size 1. 275// parameters[1] (y) is size 2. 276// parameters[2] (z) is size 3. 277// Emits 7 residuals: 278// A: x[0] (= sum_x) 279// B: y[0] + 2.0 * y[1] (= sum_y) 280// C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z) 281// D: sum_x * sum_y 282// E: sum_y * sum_z 283// F: sum_x * sum_z 284// G: sum_x * sum_y * sum_z 285class MyThreeParameterCostFunctor { 286 public: 287 template <typename T> 288 bool operator()(T const* const* parameters, T* residuals) const { 289 const T* x = parameters[0]; 290 const T* y = parameters[1]; 291 const T* z = parameters[2]; 292 293 T sum_x = x[0]; 294 T sum_y = y[0] + 2.0 * y[1]; 295 T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2]; 296 297 residuals[0] = sum_x; 298 residuals[1] = sum_y; 299 residuals[2] = sum_z; 300 residuals[3] = sum_x * sum_y; 301 residuals[4] = sum_y * sum_z; 302 residuals[5] = sum_x * sum_z; 303 residuals[6] = sum_x * sum_y * sum_z; 304 return true; 305 } 306}; 307 308class ThreeParameterCostFunctorTest : public ::testing::Test { 309 protected: 310 virtual void SetUp() { 311 // Prepare the parameters. 312 x_.resize(1); 313 x_[0] = 0.0; 314 315 y_.resize(2); 316 y_[0] = 1.0; 317 y_[1] = 3.0; 318 319 z_.resize(3); 320 z_[0] = 2.0; 321 z_[1] = 4.0; 322 z_[2] = 6.0; 323 324 parameter_blocks_.resize(3); 325 parameter_blocks_[0] = &x_[0]; 326 parameter_blocks_[1] = &y_[0]; 327 parameter_blocks_[2] = &z_[0]; 328 329 // Prepare the cost function. 330 typedef DynamicAutoDiffCostFunction<MyThreeParameterCostFunctor, 3> 331 DynamicMyThreeParameterCostFunction; 332 DynamicMyThreeParameterCostFunction * cost_function = 333 new DynamicMyThreeParameterCostFunction( 334 new MyThreeParameterCostFunctor()); 335 cost_function->AddParameterBlock(1); 336 cost_function->AddParameterBlock(2); 337 cost_function->AddParameterBlock(3); 338 cost_function->SetNumResiduals(7); 339 340 cost_function_.reset(cost_function); 341 342 // Setup jacobian data. 343 jacobian_vect_.resize(3); 344 jacobian_vect_[0].resize(7 * x_.size(), -100000); 345 jacobian_vect_[1].resize(7 * y_.size(), -100000); 346 jacobian_vect_[2].resize(7 * z_.size(), -100000); 347 348 // Prepare the expected residuals. 349 const double sum_x = x_[0]; 350 const double sum_y = y_[0] + 2.0 * y_[1]; 351 const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2]; 352 353 expected_residuals_.resize(7); 354 expected_residuals_[0] = sum_x; 355 expected_residuals_[1] = sum_y; 356 expected_residuals_[2] = sum_z; 357 expected_residuals_[3] = sum_x * sum_y; 358 expected_residuals_[4] = sum_y * sum_z; 359 expected_residuals_[5] = sum_x * sum_z; 360 expected_residuals_[6] = sum_x * sum_y * sum_z; 361 362 // Prepare the expected jacobian entries. 363 expected_jacobian_x_.resize(7); 364 expected_jacobian_x_[0] = 1.0; 365 expected_jacobian_x_[1] = 0.0; 366 expected_jacobian_x_[2] = 0.0; 367 expected_jacobian_x_[3] = sum_y; 368 expected_jacobian_x_[4] = 0.0; 369 expected_jacobian_x_[5] = sum_z; 370 expected_jacobian_x_[6] = sum_y * sum_z; 371 372 expected_jacobian_y_.resize(14); 373 expected_jacobian_y_[0] = 0.0; 374 expected_jacobian_y_[1] = 0.0; 375 expected_jacobian_y_[2] = 1.0; 376 expected_jacobian_y_[3] = 2.0; 377 expected_jacobian_y_[4] = 0.0; 378 expected_jacobian_y_[5] = 0.0; 379 expected_jacobian_y_[6] = sum_x; 380 expected_jacobian_y_[7] = 2.0 * sum_x; 381 expected_jacobian_y_[8] = sum_z; 382 expected_jacobian_y_[9] = 2.0 * sum_z; 383 expected_jacobian_y_[10] = 0.0; 384 expected_jacobian_y_[11] = 0.0; 385 expected_jacobian_y_[12] = sum_x * sum_z; 386 expected_jacobian_y_[13] = 2.0 * sum_x * sum_z; 387 388 expected_jacobian_z_.resize(21); 389 expected_jacobian_z_[0] = 0.0; 390 expected_jacobian_z_[1] = 0.0; 391 expected_jacobian_z_[2] = 0.0; 392 expected_jacobian_z_[3] = 0.0; 393 expected_jacobian_z_[4] = 0.0; 394 expected_jacobian_z_[5] = 0.0; 395 expected_jacobian_z_[6] = 1.0; 396 expected_jacobian_z_[7] = 3.0; 397 expected_jacobian_z_[8] = 6.0; 398 expected_jacobian_z_[9] = 0.0; 399 expected_jacobian_z_[10] = 0.0; 400 expected_jacobian_z_[11] = 0.0; 401 expected_jacobian_z_[12] = sum_y; 402 expected_jacobian_z_[13] = 3.0 * sum_y; 403 expected_jacobian_z_[14] = 6.0 * sum_y; 404 expected_jacobian_z_[15] = sum_x; 405 expected_jacobian_z_[16] = 3.0 * sum_x; 406 expected_jacobian_z_[17] = 6.0 * sum_x; 407 expected_jacobian_z_[18] = sum_x * sum_y; 408 expected_jacobian_z_[19] = 3.0 * sum_x * sum_y; 409 expected_jacobian_z_[20] = 6.0 * sum_x * sum_y; 410 } 411 412 protected: 413 vector<double> x_; 414 vector<double> y_; 415 vector<double> z_; 416 417 vector<double*> parameter_blocks_; 418 419 scoped_ptr<CostFunction> cost_function_; 420 421 vector<vector<double> > jacobian_vect_; 422 423 vector<double> expected_residuals_; 424 425 vector<double> expected_jacobian_x_; 426 vector<double> expected_jacobian_y_; 427 vector<double> expected_jacobian_z_; 428}; 429 430TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) { 431 vector<double> residuals(7, -100000); 432 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 433 residuals.data(), 434 NULL)); 435 for (int i = 0; i < 7; ++i) { 436 EXPECT_EQ(expected_residuals_[i], residuals[i]); 437 } 438} 439 440TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) { 441 vector<double> residuals(7, -100000); 442 443 vector<double*> jacobian; 444 jacobian.push_back(jacobian_vect_[0].data()); 445 jacobian.push_back(jacobian_vect_[1].data()); 446 jacobian.push_back(jacobian_vect_[2].data()); 447 448 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 449 residuals.data(), 450 jacobian.data())); 451 452 for (int i = 0; i < 7; ++i) { 453 EXPECT_EQ(expected_residuals_[i], residuals[i]); 454 } 455 456 for (int i = 0; i < 7; ++i) { 457 EXPECT_EQ(expected_jacobian_x_[i], jacobian[0][i]); 458 } 459 460 for (int i = 0; i < 14; ++i) { 461 EXPECT_EQ(expected_jacobian_y_[i], jacobian[1][i]); 462 } 463 464 for (int i = 0; i < 21; ++i) { 465 EXPECT_EQ(expected_jacobian_z_[i], jacobian[2][i]); 466 } 467} 468 469TEST_F(ThreeParameterCostFunctorTest, 470 ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) { 471 vector<double> residuals(7, -100000); 472 473 vector<double*> jacobian; 474 jacobian.push_back(NULL); 475 jacobian.push_back(jacobian_vect_[1].data()); 476 jacobian.push_back(NULL); 477 478 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 479 residuals.data(), 480 jacobian.data())); 481 482 for (int i = 0; i < 7; ++i) { 483 EXPECT_EQ(expected_residuals_[i], residuals[i]); 484 } 485 486 for (int i = 0; i < 14; ++i) { 487 EXPECT_EQ(expected_jacobian_y_[i], jacobian[1][i]); 488 } 489} 490 491TEST_F(ThreeParameterCostFunctorTest, 492 ThreeParameterJacobianWithSecondParameterBlockConstant) { 493 vector<double> residuals(7, -100000); 494 495 vector<double*> jacobian; 496 jacobian.push_back(jacobian_vect_[0].data()); 497 jacobian.push_back(NULL); 498 jacobian.push_back(jacobian_vect_[2].data()); 499 500 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 501 residuals.data(), 502 jacobian.data())); 503 504 for (int i = 0; i < 7; ++i) { 505 EXPECT_EQ(expected_residuals_[i], residuals[i]); 506 } 507 508 for (int i = 0; i < 7; ++i) { 509 EXPECT_EQ(expected_jacobian_x_[i], jacobian[0][i]); 510 } 511 512 for (int i = 0; i < 21; ++i) { 513 EXPECT_EQ(expected_jacobian_z_[i], jacobian[2][i]); 514 } 515} 516 517// Takes 6 parameter blocks all of size 1: 518// x0, y0, y1, z0, z1, z2 519// Same 7 residuals as MyThreeParameterCostFunctor. 520// Naming convention for tests is (V)ariable and (C)onstant. 521class MySixParameterCostFunctor { 522 public: 523 template <typename T> 524 bool operator()(T const* const* parameters, T* residuals) const { 525 const T* x0 = parameters[0]; 526 const T* y0 = parameters[1]; 527 const T* y1 = parameters[2]; 528 const T* z0 = parameters[3]; 529 const T* z1 = parameters[4]; 530 const T* z2 = parameters[5]; 531 532 T sum_x = x0[0]; 533 T sum_y = y0[0] + 2.0 * y1[0]; 534 T sum_z = z0[0] + 3.0 * z1[0] + 6.0 * z2[0]; 535 536 residuals[0] = sum_x; 537 residuals[1] = sum_y; 538 residuals[2] = sum_z; 539 residuals[3] = sum_x * sum_y; 540 residuals[4] = sum_y * sum_z; 541 residuals[5] = sum_x * sum_z; 542 residuals[6] = sum_x * sum_y * sum_z; 543 return true; 544 } 545}; 546 547class SixParameterCostFunctorTest : public ::testing::Test { 548 protected: 549 virtual void SetUp() { 550 // Prepare the parameters. 551 x0_ = 0.0; 552 y0_ = 1.0; 553 y1_ = 3.0; 554 z0_ = 2.0; 555 z1_ = 4.0; 556 z2_ = 6.0; 557 558 parameter_blocks_.resize(6); 559 parameter_blocks_[0] = &x0_; 560 parameter_blocks_[1] = &y0_; 561 parameter_blocks_[2] = &y1_; 562 parameter_blocks_[3] = &z0_; 563 parameter_blocks_[4] = &z1_; 564 parameter_blocks_[5] = &z2_; 565 566 // Prepare the cost function. 567 typedef DynamicAutoDiffCostFunction<MySixParameterCostFunctor, 3> 568 DynamicMySixParameterCostFunction; 569 DynamicMySixParameterCostFunction * cost_function = 570 new DynamicMySixParameterCostFunction( 571 new MySixParameterCostFunctor()); 572 for (int i = 0; i < 6; ++i) { 573 cost_function->AddParameterBlock(1); 574 } 575 cost_function->SetNumResiduals(7); 576 577 cost_function_.reset(cost_function); 578 579 // Setup jacobian data. 580 jacobian_vect_.resize(6); 581 for (int i = 0; i < 6; ++i) { 582 jacobian_vect_[i].resize(7, -100000); 583 } 584 585 // Prepare the expected residuals. 586 const double sum_x = x0_; 587 const double sum_y = y0_ + 2.0 * y1_; 588 const double sum_z = z0_ + 3.0 * z1_ + 6.0 * z2_; 589 590 expected_residuals_.resize(7); 591 expected_residuals_[0] = sum_x; 592 expected_residuals_[1] = sum_y; 593 expected_residuals_[2] = sum_z; 594 expected_residuals_[3] = sum_x * sum_y; 595 expected_residuals_[4] = sum_y * sum_z; 596 expected_residuals_[5] = sum_x * sum_z; 597 expected_residuals_[6] = sum_x * sum_y * sum_z; 598 599 // Prepare the expected jacobian entries. 600 expected_jacobians_.resize(6); 601 expected_jacobians_[0].resize(7); 602 expected_jacobians_[0][0] = 1.0; 603 expected_jacobians_[0][1] = 0.0; 604 expected_jacobians_[0][2] = 0.0; 605 expected_jacobians_[0][3] = sum_y; 606 expected_jacobians_[0][4] = 0.0; 607 expected_jacobians_[0][5] = sum_z; 608 expected_jacobians_[0][6] = sum_y * sum_z; 609 610 expected_jacobians_[1].resize(7); 611 expected_jacobians_[1][0] = 0.0; 612 expected_jacobians_[1][1] = 1.0; 613 expected_jacobians_[1][2] = 0.0; 614 expected_jacobians_[1][3] = sum_x; 615 expected_jacobians_[1][4] = sum_z; 616 expected_jacobians_[1][5] = 0.0; 617 expected_jacobians_[1][6] = sum_x * sum_z; 618 619 expected_jacobians_[2].resize(7); 620 expected_jacobians_[2][0] = 0.0; 621 expected_jacobians_[2][1] = 2.0; 622 expected_jacobians_[2][2] = 0.0; 623 expected_jacobians_[2][3] = 2.0 * sum_x; 624 expected_jacobians_[2][4] = 2.0 * sum_z; 625 expected_jacobians_[2][5] = 0.0; 626 expected_jacobians_[2][6] = 2.0 * sum_x * sum_z; 627 628 expected_jacobians_[3].resize(7); 629 expected_jacobians_[3][0] = 0.0; 630 expected_jacobians_[3][1] = 0.0; 631 expected_jacobians_[3][2] = 1.0; 632 expected_jacobians_[3][3] = 0.0; 633 expected_jacobians_[3][4] = sum_y; 634 expected_jacobians_[3][5] = sum_x; 635 expected_jacobians_[3][6] = sum_x * sum_y; 636 637 expected_jacobians_[4].resize(7); 638 expected_jacobians_[4][0] = 0.0; 639 expected_jacobians_[4][1] = 0.0; 640 expected_jacobians_[4][2] = 3.0; 641 expected_jacobians_[4][3] = 0.0; 642 expected_jacobians_[4][4] = 3.0 * sum_y; 643 expected_jacobians_[4][5] = 3.0 * sum_x; 644 expected_jacobians_[4][6] = 3.0 * sum_x * sum_y; 645 646 expected_jacobians_[5].resize(7); 647 expected_jacobians_[5][0] = 0.0; 648 expected_jacobians_[5][1] = 0.0; 649 expected_jacobians_[5][2] = 6.0; 650 expected_jacobians_[5][3] = 0.0; 651 expected_jacobians_[5][4] = 6.0 * sum_y; 652 expected_jacobians_[5][5] = 6.0 * sum_x; 653 expected_jacobians_[5][6] = 6.0 * sum_x * sum_y; 654 } 655 656 protected: 657 double x0_; 658 double y0_; 659 double y1_; 660 double z0_; 661 double z1_; 662 double z2_; 663 664 vector<double*> parameter_blocks_; 665 666 scoped_ptr<CostFunction> cost_function_; 667 668 vector<vector<double> > jacobian_vect_; 669 670 vector<double> expected_residuals_; 671 vector<vector<double> > expected_jacobians_; 672}; 673 674TEST_F(SixParameterCostFunctorTest, TestSixParameterResiduals) { 675 vector<double> residuals(7, -100000); 676 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 677 residuals.data(), 678 NULL)); 679 for (int i = 0; i < 7; ++i) { 680 EXPECT_EQ(expected_residuals_[i], residuals[i]); 681 } 682} 683 684TEST_F(SixParameterCostFunctorTest, TestSixParameterJacobian) { 685 vector<double> residuals(7, -100000); 686 687 vector<double*> jacobian; 688 jacobian.push_back(jacobian_vect_[0].data()); 689 jacobian.push_back(jacobian_vect_[1].data()); 690 jacobian.push_back(jacobian_vect_[2].data()); 691 jacobian.push_back(jacobian_vect_[3].data()); 692 jacobian.push_back(jacobian_vect_[4].data()); 693 jacobian.push_back(jacobian_vect_[5].data()); 694 695 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 696 residuals.data(), 697 jacobian.data())); 698 699 for (int i = 0; i < 7; ++i) { 700 EXPECT_EQ(expected_residuals_[i], residuals[i]); 701 } 702 703 for (int i = 0; i < 6; ++i) { 704 for (int j = 0; j < 7; ++j) { 705 EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); 706 } 707 } 708} 709 710TEST_F(SixParameterCostFunctorTest, TestSixParameterJacobianVVCVVC) { 711 vector<double> residuals(7, -100000); 712 713 vector<double*> jacobian; 714 jacobian.push_back(jacobian_vect_[0].data()); 715 jacobian.push_back(jacobian_vect_[1].data()); 716 jacobian.push_back(NULL); 717 jacobian.push_back(jacobian_vect_[3].data()); 718 jacobian.push_back(jacobian_vect_[4].data()); 719 jacobian.push_back(NULL); 720 721 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 722 residuals.data(), 723 jacobian.data())); 724 725 for (int i = 0; i < 7; ++i) { 726 EXPECT_EQ(expected_residuals_[i], residuals[i]); 727 } 728 729 for (int i = 0; i < 6; ++i) { 730 // Skip the constant variables. 731 if (i == 2 || i == 5) { 732 continue; 733 } 734 735 for (int j = 0; j < 7; ++j) { 736 EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); 737 } 738 } 739} 740 741TEST_F(SixParameterCostFunctorTest, TestSixParameterJacobianVCCVCV) { 742 vector<double> residuals(7, -100000); 743 744 vector<double*> jacobian; 745 jacobian.push_back(jacobian_vect_[0].data()); 746 jacobian.push_back(NULL); 747 jacobian.push_back(NULL); 748 jacobian.push_back(jacobian_vect_[3].data()); 749 jacobian.push_back(NULL); 750 jacobian.push_back(jacobian_vect_[5].data()); 751 752 EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), 753 residuals.data(), 754 jacobian.data())); 755 756 for (int i = 0; i < 7; ++i) { 757 EXPECT_EQ(expected_residuals_[i], residuals[i]); 758 } 759 760 for (int i = 0; i < 6; ++i) { 761 // Skip the constant variables. 762 if (i == 1 || i == 2 || i == 4) { 763 continue; 764 } 765 766 for (int j = 0; j < 7; ++j) { 767 EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); 768 } 769 } 770} 771 772} // namespace internal 773} // namespace ceres 774