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
