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