Refactored DynamicNumericDiffCostFunction to use NumericDiff
Change-Id: I2fc4b203e984beaa7af96fb3cbe8ce14e5bca614
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h
index f700ba2..72fa13b 100644
--- a/include/ceres/dynamic_numeric_diff_cost_function.h
+++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -29,6 +29,7 @@
// Author: mierle@gmail.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
// thadh@gmail.com (Thad Hughes)
+// tbennun@gmail.com (Tal Ben-Nun)
//
// This numeric diff implementation differs from the one found in
// numeric_diff_cost_function.h by supporting numericdiff on cost
@@ -41,7 +42,6 @@
// numeric diff; the expected interface for the cost functors is:
//
// struct MyCostFunctor {
-// template<typename T>
// bool operator()(double const* const* parameters, double* residuals) const {
// // Use parameters[i] to access the i'th parameter block.
// }
@@ -100,6 +100,7 @@
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
+ using internal::NumericDiff;
CHECK_GT(num_residuals(), 0)
<< "You must call DynamicNumericDiffCostFunction::SetNumResiduals() "
<< "before DynamicNumericDiffCostFunction::Evaluate().";
@@ -133,12 +134,18 @@
for (int block = 0; block < block_sizes.size(); ++block) {
if (jacobians[block] != NULL &&
- !EvaluateJacobianForParameterBlock(block_sizes[block],
- block,
- relative_step_size_,
+ !NumericDiff<CostFunctor, method, DYNAMIC,
+ DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
+ DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC, DYNAMIC,
+ DYNAMIC, DYNAMIC>::EvaluateJacobianForParameterBlock(
+ functor_.get(),
residuals,
+ relative_step_size_,
+ this->num_residuals(),
+ block,
+ block_sizes[block],
¶meters_references_copy[0],
- jacobians)) {
+ jacobians[block])) {
return false;
}
}
@@ -146,91 +153,6 @@
}
private:
- bool EvaluateJacobianForParameterBlock(const int parameter_block_size,
- const int parameter_block,
- const double relative_step_size,
- double const* residuals_at_eval_point,
- double** parameters,
- double** jacobians) const {
- using Eigen::Map;
- using Eigen::Matrix;
- using Eigen::Dynamic;
- using Eigen::RowMajor;
-
- typedef Matrix<double, Dynamic, 1> ResidualVector;
- typedef Matrix<double, Dynamic, 1> ParameterVector;
- typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix;
-
- int num_residuals = this->num_residuals();
-
- Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block],
- num_residuals,
- parameter_block_size);
-
- // Mutate one element at a time and then restore.
- Map<ParameterVector> x_plus_delta(parameters[parameter_block],
- parameter_block_size);
- ParameterVector x(x_plus_delta);
- ParameterVector step_size = x.array().abs() * relative_step_size;
-
- // To handle cases where a paremeter is exactly zero, instead use
- // the mean step_size for the other dimensions.
- double fallback_step_size = step_size.sum() / step_size.rows();
- if (fallback_step_size == 0.0) {
- // If all the parameters are zero, there's no good answer. Use the given
- // relative step_size as absolute step_size and hope for the best.
- fallback_step_size = relative_step_size;
- }
-
- // For each parameter in the parameter block, use finite
- // differences to compute the derivative for that parameter.
- for (int j = 0; j < parameter_block_size; ++j) {
- if (step_size(j) == 0.0) {
- // The parameter is exactly zero, so compromise and use the
- // mean step_size from the other parameters. This can break in
- // many cases, but it's hard to pick a good number without
- // problem specific knowledge.
- step_size(j) = fallback_step_size;
- }
- x_plus_delta(j) = x(j) + step_size(j);
-
- ResidualVector residuals(num_residuals);
- if (!EvaluateCostFunctor(parameters, &residuals[0])) {
- // Something went wrong; bail.
- return false;
- }
-
- // Compute this column of the jacobian in 3 steps:
- // 1. Store residuals for the forward part.
- // 2. Subtract residuals for the backward (or 0) part.
- // 3. Divide out the run.
- parameter_jacobian.col(j).matrix() = residuals;
-
- double one_over_h = 1 / step_size(j);
- if (method == CENTRAL) {
- // Compute the function on the other side of x(j).
- x_plus_delta(j) = x(j) - step_size(j);
-
- if (!EvaluateCostFunctor(parameters, &residuals[0])) {
- // Something went wrong; bail.
- return false;
- }
-
- parameter_jacobian.col(j) -= residuals;
- one_over_h /= 2;
- } else {
- // Forward difference only; reuse existing residuals evaluation.
- parameter_jacobian.col(j) -=
- Map<const ResidualVector>(residuals_at_eval_point, num_residuals);
- }
- x_plus_delta(j) = x(j); // Restore x_plus_delta.
-
- // Divide out the run to get slope.
- parameter_jacobian.col(j) *= one_over_h;
- }
- return true;
- }
-
bool EvaluateCostFunctor(double const* const* parameters,
double* residuals) const {
return EvaluateCostFunctorImpl(functor_.get(),
diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h
index c63b9a0..d220c7c 100644
--- a/include/ceres/internal/numeric_diff.h
+++ b/include/ceres/internal/numeric_diff.h
@@ -91,6 +91,8 @@
double const* residuals_at_eval_point,
const double relative_step_size,
int num_residuals,
+ int parameter_block_index,
+ int parameter_block_size,
double **parameters,
double *jacobian) {
using Eigen::Map;
@@ -98,8 +100,14 @@
using Eigen::RowMajor;
using Eigen::ColMajor;
- const int NUM_RESIDUALS =
+ const int num_residuals_internal =
(kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals);
+ const int parameter_block_index_internal =
+ (kParameterBlock != ceres::DYNAMIC ? kParameterBlock :
+ parameter_block_index);
+ const int parameter_block_size_internal =
+ (kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize :
+ parameter_block_size);
typedef Matrix<double, kNumResiduals, 1> ResidualVector;
typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
@@ -115,12 +123,13 @@
JacobianMatrix;
Map<JacobianMatrix> parameter_jacobian(jacobian,
- NUM_RESIDUALS,
- kParameterBlockSize);
+ num_residuals_internal,
+ parameter_block_size_internal);
// Mutate 1 element at a time and then restore.
- Map<ParameterVector> x_plus_delta(parameters[kParameterBlock],
- kParameterBlockSize);
+ Map<ParameterVector> x_plus_delta(
+ parameters[parameter_block_index_internal],
+ parameter_block_size_internal);
ParameterVector x(x_plus_delta);
ParameterVector step_size = x.array().abs() * relative_step_size;
@@ -136,8 +145,8 @@
// For each parameter in the parameter block, use finite differences to
// compute the derivative for that parameter.
- ResidualVector residuals(NUM_RESIDUALS);
- for (int j = 0; j < kParameterBlockSize; ++j) {
+ ResidualVector residuals(num_residuals_internal);
+ for (int j = 0; j < parameter_block_size_internal; ++j) {
const double delta =
(step_size(j) == 0.0) ? fallback_step_size : step_size(j);
@@ -169,7 +178,8 @@
} else {
// Forward difference only; reuse existing residuals evaluation.
parameter_jacobian.col(j) -=
- Map<const ResidualVector>(residuals_at_eval_point, NUM_RESIDUALS);
+ Map<const ResidualVector>(residuals_at_eval_point,
+ num_residuals_internal);
}
x_plus_delta(j) = x(j); // Restore x_plus_delta.
@@ -195,6 +205,8 @@
double const* residuals_at_eval_point,
const double relative_step_size,
const int num_residuals,
+ const int parameter_block_index,
+ const int parameter_block_size,
double **parameters,
double *jacobian) {
LOG(FATAL) << "Control should never reach here.";
diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h
index d439cc1..b3515b9 100644
--- a/include/ceres/internal/variadic_evaluate.h
+++ b/include/ceres/internal/variadic_evaluate.h
@@ -35,6 +35,7 @@
#include <stddef.h>
#include "ceres/jet.h"
+#include "ceres/types.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
#include "glog/logging.h"
@@ -176,6 +177,17 @@
}
};
+// Template instantiation for dynamically-sized functors.
+template<typename Functor, typename T>
+struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC,
+ ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
+ ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
+ ceres::DYNAMIC, ceres::DYNAMIC> {
+ static bool Call(const Functor& functor, T const *const *input, T* output) {
+ return functor(input, output);
+ }
+};
+
} // namespace internal
} // namespace ceres
diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h
index ebbb084..c15e06d 100644
--- a/include/ceres/numeric_diff_cost_function.h
+++ b/include/ceres/numeric_diff_cost_function.h
@@ -282,6 +282,8 @@
SizedCostFunction<kNumResiduals, \
N0, N1, N2, N3, N4, \
N5, N6, N7, N8, N9>::num_residuals(), \
+ block, \
+ N ## block, \
parameters_reference_copy.get(), \
jacobians[block])) { \
return false; \
