Bug fix in DynamicAutoDiffCostFunction
Add handling of constant parameter blocks.
Change-Id: I8b2ea79f47e190604fc4bed27705798240689f71
diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h
index 0bc18eb..4226177 100644
--- a/include/ceres/dynamic_autodiff_cost_function.h
+++ b/include/ceres/dynamic_autodiff_cost_function.h
@@ -121,10 +121,22 @@
vector<Jet<double, Stride> > output_jets(num_residuals());
// Make the parameter pack that is sent to the functor (reused).
- vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks);
+ vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks, NULL);
+ int num_active_parameters = 0;
+ int start_derivative_section = -1;
for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
jet_parameters[i] = &input_jets[parameter_cursor];
- for (int j = 0; j < parameter_block_sizes()[i]; ++j, 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;
+ num_active_parameters += parameter_block_size;
+ }
+
+ for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) {
input_jets[parameter_cursor].a = parameters[i][j];
}
}
@@ -132,21 +144,24 @@
// 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 = int(ceil(num_parameters / float(Stride)));
+ int num_strides = int(ceil(num_active_parameters / float(Stride)));
for (int pass = 0; pass < num_strides; ++pass) {
- const int start_derivative_section = pass * Stride;
- const int end_derivative_section = std::min((pass + 1) * Stride,
- num_parameters);
- // Set most of the jet components to zero, except for the active
- // parameters, which occur in a contiguos block of size Stride.
+ // Set most of the jet components to zero, except for
+ // non-constant #Stride parameters.
+ int active_parameter_count = 0;
+ int end_derivative_section = start_derivative_section;
for (int i = 0, parameter_cursor = 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 &&
- parameter_cursor < end_derivative_section) {
- input_jets[parameter_cursor]
- .v[parameter_cursor - start_derivative_section] = 1.0;
+ active_parameter_count < Stride) {
+ if (jacobians[i] != NULL) {
+ input_jets[parameter_cursor]
+ .v[parameter_cursor - start_derivative_section] = 1.0;
+ ++active_parameter_count;
+ }
+ ++end_derivative_section;
}
}
}
@@ -156,14 +171,18 @@
}
// Copy the pieces of the jacobians into their final place.
+ active_parameter_count = 0;
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 &&
- parameter_cursor < end_derivative_section) {
- 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];
+ active_parameter_count < Stride) {
+ 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];
+ }
+ ++active_parameter_count;
}
}
}
@@ -176,6 +195,8 @@
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 5d743a7..b62b56a 100644
--- a/internal/ceres/dynamic_autodiff_cost_function_test.cc
+++ b/internal/ceres/dynamic_autodiff_cost_function_test.cc
@@ -162,5 +162,113 @@
}
}
+TEST(DynamicAutodiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) {
+ // Test the residual counting.
+ vector<double> param_block_0(10, 0.0);
+ for (int i = 0; i < 10; ++i) {
+ param_block_0[i] = 2 * i;
+ }
+ vector<double> param_block_1(5, 0.0);
+ DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
+ new MyCostFunctor());
+ cost_function.AddParameterBlock(param_block_0.size());
+ cost_function.AddParameterBlock(param_block_1.size());
+ cost_function.SetNumResiduals(21);
+
+ // Prepare the residuals.
+ vector<double> residuals(21, -100000);
+
+ // Prepare the parameters.
+ vector<double*> parameter_blocks(2);
+ parameter_blocks[0] = ¶m_block_0[0];
+ parameter_blocks[1] = ¶m_block_1[0];
+
+ // Prepare the jacobian.
+ vector<vector<double> > jacobian_vect(2);
+ jacobian_vect[0].resize(21 * 10, -100000);
+ jacobian_vect[1].resize(21 * 5, -100000);
+ vector<double*> jacobian;
+ jacobian.push_back(NULL);
+ jacobian.push_back(jacobian_vect[1].data());
+
+ // Test jacobian computation.
+ EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int r = 0; r < 10; ++r) {
+ EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
+ EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
+ }
+ EXPECT_EQ(420, residuals.at(20));
+
+ // Check "C" Jacobian for second parameter block.
+ for (int p = 0; p < 5; ++p) {
+ EXPECT_EQ(1.0, jacobian_vect[1][20 * 5 + p]);
+ jacobian_vect[1][20 * 5 + p] = 0.0;
+ }
+ for (int i = 0; i < jacobian_vect[1].size(); ++i) {
+ EXPECT_EQ(0.0, jacobian_vect[1][i]);
+ }
+}
+
+TEST(DynamicAutodiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) {
+ // Test the residual counting.
+ vector<double> param_block_0(10, 0.0);
+ for (int i = 0; i < 10; ++i) {
+ param_block_0[i] = 2 * i;
+ }
+ vector<double> param_block_1(5, 0.0);
+ DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
+ new MyCostFunctor());
+ cost_function.AddParameterBlock(param_block_0.size());
+ cost_function.AddParameterBlock(param_block_1.size());
+ cost_function.SetNumResiduals(21);
+
+ // Prepare the residuals.
+ vector<double> residuals(21, -100000);
+
+ // Prepare the parameters.
+ vector<double*> parameter_blocks(2);
+ parameter_blocks[0] = ¶m_block_0[0];
+ parameter_blocks[1] = ¶m_block_1[0];
+
+ // Prepare the jacobian.
+ vector<vector<double> > jacobian_vect(2);
+ jacobian_vect[0].resize(21 * 10, -100000);
+ jacobian_vect[1].resize(21 * 5, -100000);
+ vector<double*> jacobian;
+ jacobian.push_back(jacobian_vect[0].data());
+ jacobian.push_back(NULL);
+
+ // Test jacobian computation.
+ EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
+ residuals.data(),
+ jacobian.data()));
+
+ for (int r = 0; r < 10; ++r) {
+ EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
+ EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
+ }
+ EXPECT_EQ(420, residuals.at(20));
+ for (int p = 0; p < 10; ++p) {
+ // Check "A" Jacobian.
+ EXPECT_EQ(-1.0, jacobian_vect[0][2*p * 10 + p]);
+ // Check "B" Jacobian.
+ EXPECT_EQ(+1.0, jacobian_vect[0][(2*p+1) * 10 + p]);
+ jacobian_vect[0][2*p * 10 + p] = 0.0;
+ jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
+ }
+
+ // Check "C" Jacobian for first parameter block.
+ for (int p = 0; p < 10; ++p) {
+ EXPECT_EQ(4 * p - 8, jacobian_vect[0][20 * 10 + p]);
+ jacobian_vect[0][20 * 10 + p] = 0.0;
+ }
+ for (int i = 0; i < jacobian_vect[0].size(); ++i) {
+ EXPECT_EQ(0.0, jacobian_vect[0][i]);
+ }
+}
+
} // namespace internal
} // namespace ceres
