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; \