Fix DynamicAutoDiffCostFunction Changed DynamicAutoDiffCostFunction to handle multiple derivative sections as opposed to just a single contiguous block. In the previous implementation it was assumed that non-constant parameters occur in a single contiguous block so that constant parameters could NOT lie between non-constant parameters. Previously, start_derivative_section was first set as soon as the first non-constant parameter block (marked by jacobians[i] != NULL) was encountered. After this, entries in input_jets[parameter_cursor].v were accessed with `parameter_cursor - start_derivative_section`. For contiguous non-constant parameter blocks this is fine, but if constant parameter blocks fall between then this indexing is incorrect because `parameter_cursor - start_derivative_section` can go out of bounds. For a concrete example, take a cost function with three parameter blocks, each of size 1 and with the center block fixed. Assume that Stride=1 so that two passes are required. On the first pass start_derivative_section=0, and the first variable block is handled correctly. At the end of the first pass end_derivative_section=1, so for the second pass start_derivative_section=1. Now comes the problem. When parameter_cursor=1, parameter_cursor >= start_derivative_section so jacobian[1] is checked to be NULL. Since it is NULL (second parameter block is constant) then nothing is done and active_parameter_count is NOT incremented. Next, when parameter_cursor=2, parameter_cursor >= start_derivative_section and jacobian[2] is checked. Since it is not NULL then input_jets[parameter_cursor].v[parameter_cursor - start_derivative_section] is set to 1.0, BUT parameter_cursor - start_derivative_section = 2 - 1 = 1 which is out of bounds (input_jets[parameter_cursor].v is only of size Stride=1). The proposed solution records the start of each contiguous block of non-constant parameters and indexing into input_jets[parameter_cursor].v is independent of parameter_cursor. Change-Id: I388ab6a0bafa35d317491135ec6fe980453ff888
diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h index 38bdb0a..5d8f188 100644 --- a/include/ceres/dynamic_autodiff_cost_function.h +++ b/include/ceres/dynamic_autodiff_cost_function.h
@@ -126,17 +126,28 @@ vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks, static_cast<Jet<double, Stride>* >(NULL)); int num_active_parameters = 0; - int start_derivative_section = -1; - for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + + // To handle constant parameters between non-constant parameter blocks, the + // start position --- a raw parameter index --- of each contiguous block of + // non-constant parameters is recorded in start_derivative_section. + vector<int> start_derivative_section; + bool in_derivative_section = false; + int parameter_cursor = 0; + + // Discover the derivative sections and set the parameter values. + for (int i = 0; i < num_parameter_blocks; ++i) { jet_parameters[i] = &input_jets[parameter_cursor]; const int parameter_block_size = parameter_block_sizes()[i]; if (jacobians[i] != NULL) { - start_derivative_section = - (start_derivative_section == -1) - ? parameter_cursor - : start_derivative_section; + if (!in_derivative_section) { + start_derivative_section.push_back(parameter_cursor); + in_derivative_section = true; + } + num_active_parameters += parameter_block_size; + } else { + in_derivative_section = false; } for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) { @@ -144,29 +155,54 @@ } } + // When `num_active_parameters % Stride != 0` then it can be the case + // that `active_parameter_count < Stride` while parameter_cursor is less + // than the total number of parameters and with no remaining non-constant + // parameter blocks. Pushing parameter_cursor (the total number of + // parameters) as a final entry to start_derivative_section is required + // because if a constant parameter block is encountered after the + // last non-constant block then current_derivative_section is incremented + // and would otherwise index an invalid position in + // start_derivative_section. Setting the final element to the total number + // of parameters means that this can only happen at most once in the loop + // below. + start_derivative_section.push_back(parameter_cursor); + // Evaluate all of the strides. Each stride is a chunk of the derivative to // evaluate, typically some size proportional to the size of the SIMD // registers of the CPU. int num_strides = static_cast<int>(ceil(num_active_parameters / static_cast<float>(Stride))); + int current_derivative_section = 0; + int current_derivative_section_cursor = 0; + for (int pass = 0; pass < num_strides; ++pass) { // Set most of the jet components to zero, except for // non-constant #Stride parameters. + const int initial_derivative_section = current_derivative_section; + const int initial_derivative_section_cursor = + current_derivative_section_cursor; + int active_parameter_count = 0; - int end_derivative_section = start_derivative_section; - for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + parameter_cursor = 0; + + for (int i = 0; i < num_parameter_blocks; ++i) { for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) { input_jets[parameter_cursor].v.setZero(); - if (parameter_cursor >= start_derivative_section && - active_parameter_count < Stride) { + if (active_parameter_count < Stride && + parameter_cursor >= ( + start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { if (jacobians[i] != NULL) { - input_jets[parameter_cursor] - .v[parameter_cursor - start_derivative_section] = 1.0; + input_jets[parameter_cursor].v[active_parameter_count] = 1.0; ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; } - ++end_derivative_section; } } } @@ -177,18 +213,27 @@ // Copy the pieces of the jacobians into their final place. active_parameter_count = 0; + + current_derivative_section = initial_derivative_section; + current_derivative_section_cursor = initial_derivative_section_cursor; + for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { for (int j = 0; j < parameter_block_sizes()[i]; ++j, parameter_cursor++) { - if (parameter_cursor >= start_derivative_section && - active_parameter_count < Stride) { + if (active_parameter_count < Stride && + parameter_cursor >= ( + start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { if (jacobians[i] != NULL) { for (int k = 0; k < num_residuals(); ++k) { jacobians[i][k * parameter_block_sizes()[i] + j] = - output_jets[k].v[parameter_cursor - - start_derivative_section]; + output_jets[k].v[active_parameter_count]; } ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; } } } @@ -201,8 +246,6 @@ residuals[k] = output_jets[k].a; } } - - start_derivative_section = end_derivative_section; } return true; }
diff --git a/internal/ceres/dynamic_autodiff_cost_function_test.cc b/internal/ceres/dynamic_autodiff_cost_function_test.cc index b62b56a..a42a374 100644 --- a/internal/ceres/dynamic_autodiff_cost_function_test.cc +++ b/internal/ceres/dynamic_autodiff_cost_function_test.cc
@@ -31,6 +31,7 @@ // sameeragarwal@google.com (Sameer Agarwal) #include "ceres/dynamic_autodiff_cost_function.h" +#include "ceres/internal/scoped_ptr.h" #include <cstddef> @@ -270,5 +271,504 @@ } } +// Takes 3 parameter blocks: +// parameters[0] (x) is size 1. +// parameters[1] (y) is size 2. +// parameters[2] (z) is size 3. +// Emits 7 residuals: +// A: x[0] (= sum_x) +// B: y[0] + 2.0 * y[1] (= sum_y) +// C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z) +// D: sum_x * sum_y +// E: sum_y * sum_z +// F: sum_x * sum_z +// G: sum_x * sum_y * sum_z +class MyThreeParameterCostFunctor { + public: + template <typename T> + bool operator()(T const* const* parameters, T* residuals) const { + const T* x = parameters[0]; + const T* y = parameters[1]; + const T* z = parameters[2]; + + T sum_x = x[0]; + T sum_y = y[0] + 2.0 * y[1]; + T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2]; + + residuals[0] = sum_x; + residuals[1] = sum_y; + residuals[2] = sum_z; + residuals[3] = sum_x * sum_y; + residuals[4] = sum_y * sum_z; + residuals[5] = sum_x * sum_z; + residuals[6] = sum_x * sum_y * sum_z; + return true; + } +}; + +class ThreeParameterCostFunctorTest : public ::testing::Test { + protected: + virtual void SetUp() { + // Prepare the parameters. + x_.resize(1); + x_[0] = 0.0; + + y_.resize(2); + y_[0] = 1.0; + y_[1] = 3.0; + + z_.resize(3); + z_[0] = 2.0; + z_[1] = 4.0; + z_[2] = 6.0; + + parameter_blocks_.resize(3); + parameter_blocks_[0] = &x_[0]; + parameter_blocks_[1] = &y_[0]; + parameter_blocks_[2] = &z_[0]; + + // Prepare the cost function. + typedef DynamicAutoDiffCostFunction<MyThreeParameterCostFunctor, 3> + DynamicMyThreeParameterCostFunction; + DynamicMyThreeParameterCostFunction * cost_function = + new DynamicMyThreeParameterCostFunction( + new MyThreeParameterCostFunctor()); + cost_function->AddParameterBlock(1); + cost_function->AddParameterBlock(2); + cost_function->AddParameterBlock(3); + cost_function->SetNumResiduals(7); + + cost_function_.reset(cost_function); + + // Setup jacobian data. + jacobian_vect_.resize(3); + jacobian_vect_[0].resize(7 * x_.size(), -100000); + jacobian_vect_[1].resize(7 * y_.size(), -100000); + jacobian_vect_[2].resize(7 * z_.size(), -100000); + + // Prepare the expected residuals. + const double sum_x = x_[0]; + const double sum_y = y_[0] + 2.0 * y_[1]; + const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2]; + + expected_residuals_.resize(7); + expected_residuals_[0] = sum_x; + expected_residuals_[1] = sum_y; + expected_residuals_[2] = sum_z; + expected_residuals_[3] = sum_x * sum_y; + expected_residuals_[4] = sum_y * sum_z; + expected_residuals_[5] = sum_x * sum_z; + expected_residuals_[6] = sum_x * sum_y * sum_z; + + // Prepare the expected jacobian entries. + expected_jacobian_x_.resize(7); + expected_jacobian_x_[0] = 1.0; + expected_jacobian_x_[1] = 0.0; + expected_jacobian_x_[2] = 0.0; + expected_jacobian_x_[3] = sum_y; + expected_jacobian_x_[4] = 0.0; + expected_jacobian_x_[5] = sum_z; + expected_jacobian_x_[6] = sum_y * sum_z; + + expected_jacobian_y_.resize(14); + expected_jacobian_y_[0] = 0.0; + expected_jacobian_y_[1] = 0.0; + expected_jacobian_y_[2] = 1.0; + expected_jacobian_y_[3] = 2.0; + expected_jacobian_y_[4] = 0.0; + expected_jacobian_y_[5] = 0.0; + expected_jacobian_y_[6] = sum_x; + expected_jacobian_y_[7] = 2.0 * sum_x; + expected_jacobian_y_[8] = sum_z; + expected_jacobian_y_[9] = 2.0 * sum_z; + expected_jacobian_y_[10] = 0.0; + expected_jacobian_y_[11] = 0.0; + expected_jacobian_y_[12] = sum_x * sum_z; + expected_jacobian_y_[13] = 2.0 * sum_x * sum_z; + + expected_jacobian_z_.resize(21); + expected_jacobian_z_[0] = 0.0; + expected_jacobian_z_[1] = 0.0; + expected_jacobian_z_[2] = 0.0; + expected_jacobian_z_[3] = 0.0; + expected_jacobian_z_[4] = 0.0; + expected_jacobian_z_[5] = 0.0; + expected_jacobian_z_[6] = 1.0; + expected_jacobian_z_[7] = 3.0; + expected_jacobian_z_[8] = 6.0; + expected_jacobian_z_[9] = 0.0; + expected_jacobian_z_[10] = 0.0; + expected_jacobian_z_[11] = 0.0; + expected_jacobian_z_[12] = sum_y; + expected_jacobian_z_[13] = 3.0 * sum_y; + expected_jacobian_z_[14] = 6.0 * sum_y; + expected_jacobian_z_[15] = sum_x; + expected_jacobian_z_[16] = 3.0 * sum_x; + expected_jacobian_z_[17] = 6.0 * sum_x; + expected_jacobian_z_[18] = sum_x * sum_y; + expected_jacobian_z_[19] = 3.0 * sum_x * sum_y; + expected_jacobian_z_[20] = 6.0 * sum_x * sum_y; + } + + protected: + vector<double> x_; + vector<double> y_; + vector<double> z_; + + vector<double*> parameter_blocks_; + + scoped_ptr<CostFunction> cost_function_; + + vector<vector<double> > jacobian_vect_; + + vector<double> expected_residuals_; + + vector<double> expected_jacobian_x_; + vector<double> expected_jacobian_y_; + vector<double> expected_jacobian_z_; +}; + +TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) { + vector<double> residuals(7, -100000); + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + NULL)); + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } +} + +TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) { + vector<double> residuals(7, -100000); + + vector<double*> jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(jacobian_vect_[1].data()); + jacobian.push_back(jacobian_vect_[2].data()); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_jacobian_x_[i], jacobian[0][i]); + } + + for (int i = 0; i < 14; ++i) { + EXPECT_EQ(expected_jacobian_y_[i], jacobian[1][i]); + } + + for (int i = 0; i < 21; ++i) { + EXPECT_EQ(expected_jacobian_z_[i], jacobian[2][i]); + } +} + +TEST_F(ThreeParameterCostFunctorTest, + ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) { + vector<double> residuals(7, -100000); + + vector<double*> jacobian; + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[1].data()); + jacobian.push_back(NULL); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 14; ++i) { + EXPECT_EQ(expected_jacobian_y_[i], jacobian[1][i]); + } +} + +TEST_F(ThreeParameterCostFunctorTest, + ThreeParameterJacobianWithSecondParameterBlockConstant) { + vector<double> residuals(7, -100000); + + vector<double*> jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[2].data()); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_jacobian_x_[i], jacobian[0][i]); + } + + for (int i = 0; i < 21; ++i) { + EXPECT_EQ(expected_jacobian_z_[i], jacobian[2][i]); + } +} + +// Takes 6 parameter blocks all of size 1: +// x0, y0, y1, z0, z1, z2 +// Same 7 residuals as MyThreeParameterCostFunctor. +// Naming convention for tests is (V)ariable and (C)onstant. +class MySixParameterCostFunctor { + public: + template <typename T> + bool operator()(T const* const* parameters, T* residuals) const { + const T* x0 = parameters[0]; + const T* y0 = parameters[1]; + const T* y1 = parameters[2]; + const T* z0 = parameters[3]; + const T* z1 = parameters[4]; + const T* z2 = parameters[5]; + + T sum_x = x0[0]; + T sum_y = y0[0] + 2.0 * y1[0]; + T sum_z = z0[0] + 3.0 * z1[0] + 6.0 * z2[0]; + + residuals[0] = sum_x; + residuals[1] = sum_y; + residuals[2] = sum_z; + residuals[3] = sum_x * sum_y; + residuals[4] = sum_y * sum_z; + residuals[5] = sum_x * sum_z; + residuals[6] = sum_x * sum_y * sum_z; + return true; + } +}; + +class SixParameterCostFunctorTest : public ::testing::Test { + protected: + virtual void SetUp() { + // Prepare the parameters. + x0_ = 0.0; + y0_ = 1.0; + y1_ = 3.0; + z0_ = 2.0; + z1_ = 4.0; + z2_ = 6.0; + + parameter_blocks_.resize(6); + parameter_blocks_[0] = &x0_; + parameter_blocks_[1] = &y0_; + parameter_blocks_[2] = &y1_; + parameter_blocks_[3] = &z0_; + parameter_blocks_[4] = &z1_; + parameter_blocks_[5] = &z2_; + + // Prepare the cost function. + typedef DynamicAutoDiffCostFunction<MySixParameterCostFunctor, 3> + DynamicMySixParameterCostFunction; + DynamicMySixParameterCostFunction * cost_function = + new DynamicMySixParameterCostFunction( + new MySixParameterCostFunctor()); + for (int i = 0; i < 6; ++i) { + cost_function->AddParameterBlock(1); + } + cost_function->SetNumResiduals(7); + + cost_function_.reset(cost_function); + + // Setup jacobian data. + jacobian_vect_.resize(6); + for (int i = 0; i < 6; ++i) { + jacobian_vect_[i].resize(7, -100000); + } + + // Prepare the expected residuals. + const double sum_x = x0_; + const double sum_y = y0_ + 2.0 * y1_; + const double sum_z = z0_ + 3.0 * z1_ + 6.0 * z2_; + + expected_residuals_.resize(7); + expected_residuals_[0] = sum_x; + expected_residuals_[1] = sum_y; + expected_residuals_[2] = sum_z; + expected_residuals_[3] = sum_x * sum_y; + expected_residuals_[4] = sum_y * sum_z; + expected_residuals_[5] = sum_x * sum_z; + expected_residuals_[6] = sum_x * sum_y * sum_z; + + // Prepare the expected jacobian entries. + expected_jacobians_.resize(6); + expected_jacobians_[0].resize(7); + expected_jacobians_[0][0] = 1.0; + expected_jacobians_[0][1] = 0.0; + expected_jacobians_[0][2] = 0.0; + expected_jacobians_[0][3] = sum_y; + expected_jacobians_[0][4] = 0.0; + expected_jacobians_[0][5] = sum_z; + expected_jacobians_[0][6] = sum_y * sum_z; + + expected_jacobians_[1].resize(7); + expected_jacobians_[1][0] = 0.0; + expected_jacobians_[1][1] = 1.0; + expected_jacobians_[1][2] = 0.0; + expected_jacobians_[1][3] = sum_x; + expected_jacobians_[1][4] = sum_z; + expected_jacobians_[1][5] = 0.0; + expected_jacobians_[1][6] = sum_x * sum_z; + + expected_jacobians_[2].resize(7); + expected_jacobians_[2][0] = 0.0; + expected_jacobians_[2][1] = 2.0; + expected_jacobians_[2][2] = 0.0; + expected_jacobians_[2][3] = 2.0 * sum_x; + expected_jacobians_[2][4] = 2.0 * sum_z; + expected_jacobians_[2][5] = 0.0; + expected_jacobians_[2][6] = 2.0 * sum_x * sum_z; + + expected_jacobians_[3].resize(7); + expected_jacobians_[3][0] = 0.0; + expected_jacobians_[3][1] = 0.0; + expected_jacobians_[3][2] = 1.0; + expected_jacobians_[3][3] = 0.0; + expected_jacobians_[3][4] = sum_y; + expected_jacobians_[3][5] = sum_x; + expected_jacobians_[3][6] = sum_x * sum_y; + + expected_jacobians_[4].resize(7); + expected_jacobians_[4][0] = 0.0; + expected_jacobians_[4][1] = 0.0; + expected_jacobians_[4][2] = 3.0; + expected_jacobians_[4][3] = 0.0; + expected_jacobians_[4][4] = 3.0 * sum_y; + expected_jacobians_[4][5] = 3.0 * sum_x; + expected_jacobians_[4][6] = 3.0 * sum_x * sum_y; + + expected_jacobians_[5].resize(7); + expected_jacobians_[5][0] = 0.0; + expected_jacobians_[5][1] = 0.0; + expected_jacobians_[5][2] = 6.0; + expected_jacobians_[5][3] = 0.0; + expected_jacobians_[5][4] = 6.0 * sum_y; + expected_jacobians_[5][5] = 6.0 * sum_x; + expected_jacobians_[5][6] = 6.0 * sum_x * sum_y; + } + + protected: + double x0_; + double y0_; + double y1_; + double z0_; + double z1_; + double z2_; + + vector<double*> parameter_blocks_; + + scoped_ptr<CostFunction> cost_function_; + + vector<vector<double> > jacobian_vect_; + + vector<double> expected_residuals_; + vector<vector<double> > expected_jacobians_; +}; + +TEST_F(SixParameterCostFunctorTest, TestSixParameterResiduals) { + vector<double> residuals(7, -100000); + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + NULL)); + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } +} + +TEST_F(SixParameterCostFunctorTest, TestSixParameterJacobian) { + vector<double> residuals(7, -100000); + + vector<double*> jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(jacobian_vect_[1].data()); + jacobian.push_back(jacobian_vect_[2].data()); + jacobian.push_back(jacobian_vect_[3].data()); + jacobian.push_back(jacobian_vect_[4].data()); + jacobian.push_back(jacobian_vect_[5].data()); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 7; ++j) { + EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); + } + } +} + +TEST_F(SixParameterCostFunctorTest, TestSixParameterJacobianVVCVVC) { + vector<double> residuals(7, -100000); + + vector<double*> jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(jacobian_vect_[1].data()); + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[3].data()); + jacobian.push_back(jacobian_vect_[4].data()); + jacobian.push_back(NULL); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 6; ++i) { + // Skip the constant variables. + if (i == 2 || i == 5) { + continue; + } + + for (int j = 0; j < 7; ++j) { + EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); + } + } +} + +TEST_F(SixParameterCostFunctorTest, TestSixParameterJacobianVCCVCV) { + vector<double> residuals(7, -100000); + + vector<double*> jacobian; + jacobian.push_back(jacobian_vect_[0].data()); + jacobian.push_back(NULL); + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[3].data()); + jacobian.push_back(NULL); + jacobian.push_back(jacobian_vect_[5].data()); + + EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(), + residuals.data(), + jacobian.data())); + + for (int i = 0; i < 7; ++i) { + EXPECT_EQ(expected_residuals_[i], residuals[i]); + } + + for (int i = 0; i < 6; ++i) { + // Skip the constant variables. + if (i == 1 || i == 2 || i == 4) { + continue; + } + + for (int j = 0; j < 7; ++j) { + EXPECT_EQ(expected_jacobians_[i][j], jacobian[i][j]); + } + } +} + } // namespace internal } // namespace ceres