Add DynamicCostFunctionToFunctor.

This adds a new wrapper class called DynamicCostFunctionToFunctor
that closes a gap in the current API: the existing
CostFunctionToFunctor can only be used with a SizedCostFunction, where
the number and sizes of all parameter vectors are known at compile-time.
The DynamicCostFunctionToFunctor allows you to wrap a generic
CostFunction into a templated functor which can then be used in a
DynamicAutoDiffCostFunction.

Also updates the existing CostFunctionToFunctor class to internally use
DynamicCostFunctionToFunctor.

Change-Id: I088adc3271c58d2519126c27037c3576965a36d6

docs/source/nnls_modeling.rst

@@ -625,7 +625,7 @@ Numeric Differentiation & LocalParameterization
 
      class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {
        public:
-         IntrinsicProjection(const double* observations);
+         IntrinsicProjection(const double* observation);
          virtual bool Evaluate(double const* const* parameters,
                                double* residuals,
                                double** jacobians) const;
@@ -656,7 +656,7 @@ Numeric Differentiation & LocalParameterization
 
     struct CameraProjection {
       CameraProjection(double* observation)
-      : intrinsic_projection_(new IntrinsicProjection(observation_)) {
+      : intrinsic_projection_(new IntrinsicProjection(observation)) {
       }
 
       template <typename T>
@@ -677,6 +677,8 @@ Numeric Differentiation & LocalParameterization
       CostFunctionToFunctor<2,5,3> intrinsic_projection_;
     };
 
+   Note that :class:`CostFunctionToFunctor` takes ownership of the 
+   :class:`CostFunction` that was passed in to the constructor.
 
    In the above example, we assumed that ``IntrinsicProjection`` is a
    ``CostFunction`` capable of evaluating its value and its
@@ -686,9 +688,9 @@ Numeric Differentiation & LocalParameterization
    .. code-block:: c++
 
     struct IntrinsicProjection
-      IntrinsicProjection(const double* observations) {
-        observations_[0] = observations[0];
-        observations_[1] = observations[1];
+      IntrinsicProjection(const double* observation) {
+        observation_[0] = observation[0];
+        observation_[1] = observation[1];
       }
 
       bool operator()(const double* calibration,
@@ -696,11 +698,11 @@ Numeric Differentiation & LocalParameterization
                       double* residuals) {
         double projection[2];
         ThirdPartyProjectionFunction(calibration, point, projection);
-        residuals[0] = observations_[0] - projection[0];
-        residuals[1] = observations_[1] - projection[1];
+        residuals[0] = observation_[0] - projection[0];
+        residuals[1] = observation_[1] - projection[1];
         return true;
       }
-     double observations_[2];
+     double observation_[2];
     };
 
 
@@ -717,7 +719,7 @@ Numeric Differentiation & LocalParameterization
      CameraProjection(double* observation)
        intrinsic_projection_(
          new NumericDiffCostFunction<IntrinsicProjection, CENTRAL, 2, 5, 3>(
-           new IntrinsicProjection(observations)) {
+           new IntrinsicProjection(observation)) {
      }
 
      template <typename T>
@@ -735,6 +737,74 @@ Numeric Differentiation & LocalParameterization
      CostFunctionToFunctor<2,5,3> intrinsic_projection_;
    };
 
+:class:`DynamicCostFunctionToFunctor`
+=====================================
+
+.. class:: DynamicCostFunctionToFunctor
+
+   :class:`DynamicCostFunctionToFunctor` provides the same functionality as
+   :class:`CostFunctionToFunctor` for cases where the number and size of the
+   parameter vectors and residuals are not known at compile-time. The API
+   provided by :class:`DynamicCostFunctionToFunctor` matches what would be
+   expected by :class:`DynamicAutoDiffCostFunction`, i.e. it provides a
+   templated functor of this form:
+
+   .. code-block:: c++
+
+    template<typename T>
+    bool operator()(T const* const* parameters, T* residuals) const;
+
+   Similar to the example given for :class:`CostFunctionToFunctor`, let us
+   assume that
+
+   .. code-block:: c++
+
+     class IntrinsicProjection : public CostFunction {
+       public:
+         IntrinsicProjection(const double* observation);
+         virtual bool Evaluate(double const* const* parameters,
+                               double* residuals,
+                               double** jacobians) const;
+     };
+
+   is a :class:`CostFunction` that projects a point in its local coordinate
+   system onto its image plane and subtracts it from the observed point
+   projection.
+
+   Using this :class:`CostFunction` in a templated functor would then look like
+   this:
+
+   .. code-block:: c++
+
+    struct CameraProjection {
+      CameraProjection(double* observation)
+          : intrinsic_projection_(new IntrinsicProjection(observation)) {
+      }
+
+      template <typename T>
+      bool operator()(T const* const* parameters,
+                      T* residual) const {
+        const T* rotation = parameters[0];
+        const T* translation = parameters[1];
+        const T* intrinsics = parameters[2];
+        const T* point = parameters[3];
+
+        T transformed_point[3];
+        RotateAndTranslatePoint(rotation, translation, point, transformed_point);
+
+        const T* projection_parameters[2];
+        projection_parameters[0] = intrinsics;
+        projection_parameters[1] = transformed_point;
+        return intrinsic_projection_(projection_parameters, residual);
+      }
+
+     private:
+      DynamicCostFunctionToFunctor intrinsic_projection_;
+    };
+
+   Like :class:`CostFunctionToFunctor`, :class:`DynamicCostFunctionToFunctor`
+   takes ownership of the :class:`CostFunction` that was passed in to the
+   constructor.
 
 :class:`ConditionedCostFunction`
 ================================

include/ceres/cost_function_to_functor.h

@@ -29,7 +29,7 @@
 // Author: sameeragarwal@google.com (Sameer Agarwal)
 //
 // CostFunctionToFunctor is an adapter class that allows users to use
-// CostFunction objects in templated functors which are to be used for
+// SizedCostFunction objects in templated functors which are to be used for
 // automatic differentiation.  This allows the user to seamlessly mix
 // analytic, numeric and automatic differentiation.
 //
@@ -37,7 +37,7 @@
 //
 //  class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {
 //    public:
-//      IntrinsicProjection(const double* observations);
+//      IntrinsicProjection(const double* observation);
 //      virtual bool Evaluate(double const* const* parameters,
 //                            double* residuals,
 //                            double** jacobians) const;
@@ -62,10 +62,8 @@
 // Then we can now do the following,
 //
 // struct CameraProjection {
-//   CameraProjection(double* observation) {
-//     intrinsic_projection_.reset(
-//         new CostFunctionToFunctor<2, 5, 3>(
-//             new IntrinsicProjection(observation_)));
+//   CameraProjection(const double* observation)
+//       : intrinsic_projection_(new IntrinsicProjection(observation)) {
 //   }
 //   template <typename T>
 //   bool operator()(const T* rotation,
@@ -79,11 +77,11 @@
 //     // Note that we call intrinsic_projection_, just like it was
 //     // any other templated functor.
 //
-//     return (*intrinsic_projection_)(intrinsics, transformed_point, residual);
+//     return intrinsic_projection_(intrinsics, transformed_point, residual);
 //   }
 //
 //  private:
-//   scoped_ptr<CostFunctionToFunctor<2,5,3> > intrinsic_projection_;
+//   CostFunctionToFunctor<2,5,3> intrinsic_projection_;
 // };
 
 #ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
@@ -93,6 +91,7 @@
 #include <vector>
 
 #include "ceres/cost_function.h"
+#include "ceres/dynamic_cost_function_to_functor.h"
 #include "ceres/internal/fixed_array.h"
 #include "ceres/internal/port.h"
 #include "ceres/internal/scoped_ptr.h"
@@ -104,8 +103,9 @@ template <int kNumResiduals,
           int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
 class CostFunctionToFunctor {
  public:
+  // Takes ownership of cost_function.
   explicit CostFunctionToFunctor(CostFunction* cost_function)
-  : cost_function_(cost_function) {
+      : cost_functor_(cost_function) {
     CHECK_NOTNULL(cost_function);
     CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC);
 
@@ -160,7 +160,7 @@ class CostFunctionToFunctor {
     CHECK_EQ(N8, 0);
     CHECK_EQ(N9, 0);
 
-    return cost_function_->Evaluate(&x0, residuals, NULL);
+    return cost_functor_(&x0, residuals);
   }
 
   bool operator()(const double* x0,
@@ -179,7 +179,7 @@ class CostFunctionToFunctor {
     internal::FixedArray<const double*> parameter_blocks(2);
     parameter_blocks[0] = x0;
     parameter_blocks[1] = x1;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -200,7 +200,7 @@ class CostFunctionToFunctor {
     parameter_blocks[0] = x0;
     parameter_blocks[1] = x1;
     parameter_blocks[2] = x2;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -223,7 +223,7 @@ class CostFunctionToFunctor {
     parameter_blocks[1] = x1;
     parameter_blocks[2] = x2;
     parameter_blocks[3] = x3;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -248,7 +248,7 @@ class CostFunctionToFunctor {
     parameter_blocks[2] = x2;
     parameter_blocks[3] = x3;
     parameter_blocks[4] = x4;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -275,7 +275,7 @@ class CostFunctionToFunctor {
     parameter_blocks[3] = x3;
     parameter_blocks[4] = x4;
     parameter_blocks[5] = x5;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -304,7 +304,7 @@ class CostFunctionToFunctor {
     parameter_blocks[4] = x4;
     parameter_blocks[5] = x5;
     parameter_blocks[6] = x6;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -335,7 +335,7 @@ class CostFunctionToFunctor {
     parameter_blocks[5] = x5;
     parameter_blocks[6] = x6;
     parameter_blocks[7] = x7;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -368,7 +368,7 @@ class CostFunctionToFunctor {
     parameter_blocks[6] = x6;
     parameter_blocks[7] = x7;
     parameter_blocks[8] = x8;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   bool operator()(const double* x0,
@@ -403,7 +403,7 @@ class CostFunctionToFunctor {
     parameter_blocks[7] = x7;
     parameter_blocks[8] = x8;
     parameter_blocks[9] = x9;
-    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
+    return cost_functor_(parameter_blocks.get(), residuals);
   }
 
   template <typename JetT>
@@ -418,7 +418,7 @@ class CostFunctionToFunctor {
     CHECK_EQ(N7, 0);
     CHECK_EQ(N8, 0);
     CHECK_EQ(N9, 0);
-    return EvaluateWithJets(&x0, residuals);
+    return cost_functor_(&x0, residuals);
   }
 
   template <typename JetT>
@@ -438,7 +438,7 @@ class CostFunctionToFunctor {
     internal::FixedArray<const JetT*> jets(2);
     jets[0] = x0;
     jets[1] = x1;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -460,7 +460,7 @@ class CostFunctionToFunctor {
     jets[0] = x0;
     jets[1] = x1;
     jets[2] = x2;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -484,7 +484,7 @@ class CostFunctionToFunctor {
     jets[1] = x1;
     jets[2] = x2;
     jets[3] = x3;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -510,7 +510,7 @@ class CostFunctionToFunctor {
     jets[2] = x2;
     jets[3] = x3;
     jets[4] = x4;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -538,7 +538,7 @@ class CostFunctionToFunctor {
     jets[3] = x3;
     jets[4] = x4;
     jets[5] = x5;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -568,7 +568,7 @@ class CostFunctionToFunctor {
     jets[4] = x4;
     jets[5] = x5;
     jets[6] = x6;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -600,7 +600,7 @@ class CostFunctionToFunctor {
     jets[5] = x5;
     jets[6] = x6;
     jets[7] = x7;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -634,7 +634,7 @@ class CostFunctionToFunctor {
     jets[6] = x6;
     jets[7] = x7;
     jets[8] = x8;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
   template <typename JetT>
@@ -670,79 +670,11 @@ class CostFunctionToFunctor {
     jets[7] = x7;
     jets[8] = x8;
     jets[9] = x9;
-    return EvaluateWithJets(jets.get(), residuals);
+    return cost_functor_(jets.get(), residuals);
   }
 
  private:
-  template <typename JetT>
-  bool EvaluateWithJets(const JetT** inputs, JetT* output) const {
-    const int kNumParameters =  N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
-    const std::vector<int32>& parameter_block_sizes =
-        cost_function_->parameter_block_sizes();
-    const int num_parameter_blocks = parameter_block_sizes.size();
-    const int num_residuals = cost_function_->num_residuals();
-
-    internal::FixedArray<double> parameters(kNumParameters);
-    internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);
-    internal::FixedArray<double> jacobians(num_residuals * kNumParameters);
-    internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);
-    internal::FixedArray<double> residuals(num_residuals);
-
-    // Build a set of arrays to get the residuals and jacobians from
-    // the CostFunction wrapped by this functor.
-    double* parameter_ptr = parameters.get();
-    double* jacobian_ptr = jacobians.get();
-    for (int i = 0; i < num_parameter_blocks; ++i) {
-      parameter_blocks[i] = parameter_ptr;
-      jacobian_blocks[i] = jacobian_ptr;
-      for (int j = 0; j < parameter_block_sizes[i]; ++j) {
-        *parameter_ptr++ = inputs[i][j].a;
-      }
-      jacobian_ptr += num_residuals * parameter_block_sizes[i];
-    }
-
-    if (!cost_function_->Evaluate(parameter_blocks.get(),
-                                  residuals.get(),
-                                  jacobian_blocks.get())) {
-      return false;
-    }
-
-    // Now that we have the incoming Jets, which are carrying the
-    // partial derivatives of each of the inputs w.r.t to some other
-    // underlying parameters. The derivative of the outputs of the
-    // cost function w.r.t to the same underlying parameters can now
-    // be computed by applying the chain rule.
-    //
-    //  d output[i]               d output[i]   d input[j]
-    //  --------------  = sum_j   ----------- * ------------
-    //  d parameter[k]            d input[j]    d parameter[k]
-    //
-    // d input[j]
-    // --------------  = inputs[j], so
-    // d parameter[k]
-    //
-    //  outputJet[i]  = sum_k jacobian[i][k] * inputJet[k]
-    //
-    // The following loop, iterates over the residuals, computing one
-    // output jet at a time.
-    for (int i = 0; i < num_residuals; ++i) {
-      output[i].a = residuals[i];
-      output[i].v.setZero();
-
-      for (int j = 0; j < num_parameter_blocks; ++j) {
-        const int32 block_size = parameter_block_sizes[j];
-        for (int k = 0; k < parameter_block_sizes[j]; ++k) {
-          output[i].v +=
-              jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;
-        }
-      }
-    }
-
-    return true;
-  }
-
- private:
-  internal::scoped_ptr<CostFunction> cost_function_;
+  DynamicCostFunctionToFunctor cost_functor_;
 };
 
 }  // namespace ceres

include/ceres/dynamic_cost_function_to_functor.h

@@ -0,0 +1,190 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+//         dgossow@google.com (David Gossow)
+//
+// DynamicCostFunctionToFunctor allows users to use CostFunction
+// objects in templated functors which are to be used for automatic
+// differentiation. It works similar to CostFunctionToFunctor, with the
+// difference that it allows you to wrap a cost function with dynamic numbers
+// of parameters and residuals.
+//
+// For example, let us assume that
+//
+//  class IntrinsicProjection : public CostFunction {
+//    public:
+//      IntrinsicProjection(const double* observation);
+//      virtual bool Evaluate(double const* const* parameters,
+//                            double* residuals,
+//                            double** jacobians) const;
+//  };
+//
+// is a cost function that implements the projection of a point in its
+// local coordinate system onto its image plane and subtracts it from
+// the observed point projection. It can compute its residual and
+// either via analytic or numerical differentiation can compute its
+// jacobians. The intrinsics are passed in as parameters[0] and the point as
+// parameters[1].
+//
+// Now we would like to compose the action of this CostFunction with
+// the action of camera extrinsics, i.e., rotation and
+// translation. Say we have a templated function
+//
+//   template<typename T>
+//   void RotateAndTranslatePoint(double const* const* parameters,
+//                                double* residuals);
+//
+// Then we can now do the following,
+//
+// struct CameraProjection {
+//   CameraProjection(const double* observation)
+//       : intrinsic_projection_.(new IntrinsicProjection(observation)) {
+//   }
+//   template <typename T>
+//   bool operator()(T const* const* parameters,
+//                   T* residual) const {
+//     const T* rotation = parameters[0];
+//     const T* translation = parameters[1];
+//     const T* intrinsics = parameters[2];
+//     const T* point = parameters[3];
+//     T transformed_point[3];
+//     RotateAndTranslatePoint(rotation, translation, point, transformed_point);
+//
+//     // Note that we call intrinsic_projection_, just like it was
+//     // any other templated functor.
+//     const T* projection_parameters[2];
+//     projection_parameters[0] = intrinsics;
+//     projection_parameters[1] = transformed_point;
+//     return intrinsic_projection_(projection_parameters, residual);
+//   }
+//
+//  private:
+//   DynamicCostFunctionToFunctor intrinsic_projection_;
+// };
+
+#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
+#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
+
+#include <numeric>
+#include <vector>
+
+#include "ceres/cost_function.h"
+#include "ceres/internal/fixed_array.h"
+#include "ceres/internal/port.h"
+#include "ceres/internal/scoped_ptr.h"
+
+namespace ceres {
+
+class DynamicCostFunctionToFunctor {
+ public:
+  // Takes ownership of cost_function.
+  explicit DynamicCostFunctionToFunctor(CostFunction* cost_function)
+      : cost_function_(cost_function) {
+    CHECK_NOTNULL(cost_function);
+  }
+
+  bool operator()(double const* const* parameters, double* residuals) const {
+    return cost_function_->Evaluate(parameters, residuals, NULL);
+  }
+
+  template <typename JetT>
+  bool operator()(JetT const* const* inputs, JetT* output) const {
+    const std::vector<int32>& parameter_block_sizes =
+        cost_function_->parameter_block_sizes();
+    const int num_parameter_blocks = parameter_block_sizes.size();
+    const int num_residuals = cost_function_->num_residuals();
+    const int num_parameters = std::accumulate(parameter_block_sizes.begin(),
+                                               parameter_block_sizes.end(), 0);
+
+    internal::FixedArray<double> parameters(num_parameters);
+    internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);
+    internal::FixedArray<double> jacobians(num_residuals * num_parameters);
+    internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);
+    internal::FixedArray<double> residuals(num_residuals);
+
+    // Build a set of arrays to get the residuals and jacobians from
+    // the CostFunction wrapped by this functor.
+    double* parameter_ptr = parameters.get();
+    double* jacobian_ptr = jacobians.get();
+    for (int i = 0; i < num_parameter_blocks; ++i) {
+      parameter_blocks[i] = parameter_ptr;
+      jacobian_blocks[i] = jacobian_ptr;
+      for (int j = 0; j < parameter_block_sizes[i]; ++j) {
+        *parameter_ptr++ = inputs[i][j].a;
+      }
+      jacobian_ptr += num_residuals * parameter_block_sizes[i];
+    }
+
+    if (!cost_function_->Evaluate(parameter_blocks.get(),
+                                  residuals.get(),
+                                  jacobian_blocks.get())) {
+      return false;
+    }
+
+    // Now that we have the incoming Jets, which are carrying the
+    // partial derivatives of each of the inputs w.r.t to some other
+    // underlying parameters. The derivative of the outputs of the
+    // cost function w.r.t to the same underlying parameters can now
+    // be computed by applying the chain rule.
+    //
+    //  d output[i]               d output[i]   d input[j]
+    //  --------------  = sum_j   ----------- * ------------
+    //  d parameter[k]            d input[j]    d parameter[k]
+    //
+    // d input[j]
+    // --------------  = inputs[j], so
+    // d parameter[k]
+    //
+    //  outputJet[i]  = sum_k jacobian[i][k] * inputJet[k]
+    //
+    // The following loop, iterates over the residuals, computing one
+    // output jet at a time.
+    for (int i = 0; i < num_residuals; ++i) {
+      output[i].a = residuals[i];
+      output[i].v.setZero();
+
+      for (int j = 0; j < num_parameter_blocks; ++j) {
+        const int32 block_size = parameter_block_sizes[j];
+        for (int k = 0; k < parameter_block_sizes[j]; ++k) {
+          output[i].v +=
+              jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;
+        }
+      }
+    }
+
+    return true;
+  }
+
+ private:
+  internal::scoped_ptr<CostFunction> cost_function_;
+};
+
+}  // namespace ceres
+
+#endif  // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_

internal/ceres/cost_function_to_functor_test.cc

@@ -29,6 +29,8 @@
 // Author: sameeragarwal@google.com (Sameer Agarwal)
 
 #include "ceres/cost_function_to_functor.h"
+#include "ceres/dynamic_autodiff_cost_function.h"
+#include "ceres/dynamic_cost_function_to_functor.h"
 #include "ceres/autodiff_cost_function.h"
 #include "gtest/gtest.h"
 
@@ -242,6 +244,18 @@ struct TenParameterBlockFunctor {
   }
 };
 
+class DynamicTwoParameterBlockFunctor {
+ public:
+  template <typename T>
+  bool operator()(T const* const* parameters, T* residuals) const {
+    for (int i = 0; i < 2; ++i) {
+      residuals[0] = parameters[i][0] * parameters[i][0];
+      residuals[1] = parameters[i][1] * parameters[i][1];
+    }
+    return true;
+  }
+};
+
 #define TEST_BODY(NAME)                                                 \
   TEST(CostFunctionToFunctor, NAME) {                                   \
     scoped_ptr<CostFunction> cost_function(                             \
@@ -315,5 +329,23 @@ TEST(CostFunctionToFunctor, DynamicNumberOfResiduals) {
   ExpectCostFunctionsAreEqual(*cost_function, *actual_cost_function);
 }
 
+TEST(CostFunctionToFunctor, DynamicCostFunctionToFunctor) {
+  DynamicAutoDiffCostFunction<DynamicTwoParameterBlockFunctor>*
+      actual_cost_function(
+      new DynamicAutoDiffCostFunction<DynamicTwoParameterBlockFunctor>(
+          new DynamicTwoParameterBlockFunctor));
+  actual_cost_function->AddParameterBlock(2);
+  actual_cost_function->AddParameterBlock(2);
+  actual_cost_function->SetNumResiduals(2);
+
+  DynamicAutoDiffCostFunction<DynamicCostFunctionToFunctor> cost_function(
+      new DynamicCostFunctionToFunctor(actual_cost_function));
+  cost_function.AddParameterBlock(2);
+  cost_function.AddParameterBlock(2);
+  cost_function.SetNumResiduals(2);
+
+  ExpectCostFunctionsAreEqual(cost_function, *actual_cost_function);
+}
+
 }  // namespace internal
 }  // namespace ceres