Fix Jacobian evaluation for constant parameter This CL fixes a regression bug in numeric differentiation where the differentiation is called even if the parameter block is hold constant. This resulted into a write to a nullptr. A unit test is added for Jacobian evaluation of constant parameters. Change-Id: Ia0f7c6cc7ef18f0f2cd6d758a839729b8ff606a0
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h index f24fdb3..09c94b0 100644 --- a/include/ceres/internal/numeric_diff.h +++ b/include/ceres/internal/numeric_diff.h
@@ -72,6 +72,8 @@ using Eigen::RowMajor; using Eigen::ColMajor; + DCHECK_NOTNULL(jacobian); + const int num_residuals_internal = (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals); const int parameter_block_index_internal = @@ -381,7 +383,7 @@ // each parameter block. // // Example: -// A call to +// A call to // EvaluateJacobianForParameterBlocks<StaticParameterDims<2, 3>>( // functor, // residuals_at_eval_point, @@ -392,29 +394,41 @@ // will result in the following calls to // NumericDiff<...>::EvaluateJacobianForParameterBlock: // -// if (!NumericDiff< -// CostFunctor, method, kNumResiduals, ParameterDims, 0, -// 2>::EvaluateJacobianForParameterBlock(functor, -// residuals_at_eval_point, -// options, -// num_residuals, -// 0, -// 2, -// parameters, -// jacobians[0])) { -// return false; +// if (jacobians[0] != nullptr) { +// if (!NumericDiff< +// CostFunctor, +// method, +// kNumResiduals, +// StaticParameterDims<2, 3>, +// 0, +// 2>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 0, +// 2, +// parameters, +// jacobians[0])) { +// return false; +// } // } -// if (!NumericDiff< -// CostFunctor, method, kNumResiduals, ParameterDims, 1, -// 3>::EvaluateJacobianForParameterBlock(functor, -// residuals_at_eval_point, -// options, -// num_residuals, -// 1, -// 3, -// parameters, -// jacobians[1])) { -// return false; +// if (jacobians[1] != nullptr) { +// if (!NumericDiff< +// CostFunctor, +// method, +// kNumResiduals, +// StaticParameterDims<2, 3>, +// 1, +// 3>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 1, +// 3, +// parameters, +// jacobians[1])) { +// return false; +// } // } template <typename ParameterDims, typename Parameters = typename ParameterDims::Parameters, @@ -422,33 +436,46 @@ struct EvaluateJacobianForParameterBlocks; template <typename ParameterDims, int N, int... Ns, int ParameterIdx> -struct EvaluateJacobianForParameterBlocks< - ParameterDims, integer_sequence<int, N, Ns...>, ParameterIdx> { - template <NumericDiffMethodType method, int kNumResiduals, +struct EvaluateJacobianForParameterBlocks<ParameterDims, + integer_sequence<int, N, Ns...>, + ParameterIdx> { + template <NumericDiffMethodType method, + int kNumResiduals, typename CostFunctor> static bool Apply(const CostFunctor* functor, const double* residuals_at_eval_point, - const NumericDiffOptions& options, int num_residuals, - double** parameters, double** jacobians) { - if (!NumericDiff< - CostFunctor, method, kNumResiduals, ParameterDims, ParameterIdx, - N>::EvaluateJacobianForParameterBlock(functor, - residuals_at_eval_point, - options, - num_residuals, - ParameterIdx, - N, - parameters, - jacobians[ParameterIdx])) { - return false; + const NumericDiffOptions& options, + int num_residuals, + double** parameters, + double** jacobians) { + if (jacobians[ParameterIdx] != nullptr) { + if (!NumericDiff< + CostFunctor, + method, + kNumResiduals, + ParameterDims, + ParameterIdx, + N>::EvaluateJacobianForParameterBlock(functor, + residuals_at_eval_point, + options, + num_residuals, + ParameterIdx, + N, + parameters, + jacobians[ParameterIdx])) { + return false; + } } - return EvaluateJacobianForParameterBlocks< - ParameterDims, integer_sequence<int, Ns...>, ParameterIdx + 1>:: + + return EvaluateJacobianForParameterBlocks<ParameterDims, + integer_sequence<int, Ns...>, + ParameterIdx + 1>:: template Apply<method, kNumResiduals>(functor, residuals_at_eval_point, options, num_residuals, - parameters, jacobians); + parameters, + jacobians); } };
diff --git a/internal/ceres/numeric_diff_cost_function_test.cc b/internal/ceres/numeric_diff_cost_function_test.cc index 79d3e8a..105bef5 100644 --- a/internal/ceres/numeric_diff_cost_function_test.cc +++ b/internal/ceres/numeric_diff_cost_function_test.cc
@@ -32,6 +32,7 @@ #include "ceres/numeric_diff_cost_function.h" #include <algorithm> +#include <array> #include <cmath> #include <memory> #include <string> @@ -385,5 +386,62 @@ EXPECT_FALSE(IsArrayValid(2, residuals)); } +TEST(NumericDiffCostFunction, ParameterBlockConstant) { + constexpr int kNumResiduals = 3; + constexpr int kX1 = 5; + constexpr int kX2 = 5; + + std::unique_ptr<CostFunction> cost_function; + cost_function.reset(new NumericDiffCostFunction<EasyFunctor, + CENTRAL, + kNumResiduals, + kX1, + kX2>(new EasyFunctor)); + + // Prepare the parameters and residuals. + std::array<double, kX1> x1{1e-64, 2.0, 3.0, 4.0, 5.0}; + std::array<double, kX2> x2{9.0, 9.0, 5.0, 5.0, 1.0}; + std::array<double*, 2> parameter_blocks{x1.data(), x2.data()}; + + std::vector<double> residuals(kNumResiduals, -100000); + + // Evaluate the full jacobian. + std::vector<std::vector<double>> jacobian_full_vect(2); + jacobian_full_vect[0].resize(kNumResiduals * kX1, -100000); + jacobian_full_vect[1].resize(kNumResiduals * kX2, -100000); + { + std::array<double*, 2> jacobian{jacobian_full_vect[0].data(), + jacobian_full_vect[1].data()}; + ASSERT_TRUE(cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobian.data())); + } + + // Evaluate and check jacobian when first parameter block is constant. + { + std::vector<double> jacobian_vect(kNumResiduals * kX2, -100000); + std::array<double*, 2> jacobian{nullptr, jacobian_vect.data()}; + + ASSERT_TRUE(cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobian.data())); + + for (int i = 0; i < kNumResiduals * kX2; ++i) { + EXPECT_DOUBLE_EQ(jacobian_full_vect[1][i], jacobian_vect[i]); + } + } + + // Evaluate and check jacobian when second parameter block is constant. + { + std::vector<double> jacobian_vect(kNumResiduals * kX1, -100000); + std::array<double*, 2> jacobian{jacobian_vect.data(), nullptr}; + + ASSERT_TRUE(cost_function->Evaluate( + parameter_blocks.data(), residuals.data(), jacobian.data())); + + for (int i = 0; i < kNumResiduals * kX1; ++i) { + EXPECT_DOUBLE_EQ(jacobian_full_vect[0][i], jacobian_vect[i]); + } + } +} + } // namespace internal } // namespace ceres