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
