Refactored DynamicNumericDiffCostFunction to use NumericDiff

Change-Id: I2fc4b203e984beaa7af96fb3cbe8ce14e5bca614

include/ceres/internal/numeric_diff.h

@@ -91,6 +91,8 @@ struct NumericDiff {
       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 @@ struct NumericDiff {
     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 @@ struct NumericDiff {
         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 @@ struct NumericDiff {
     // 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 @@ struct NumericDiff {
       } 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 @@ struct NumericDiff<CostFunctor, kMethod, kNumResiduals,
       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.";

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 @@ struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
   }
 };
 
+// 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