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
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 02418c0..9516795 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -625,7 +625,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;
@@ -656,7 +656,7 @@
struct CameraProjection {
CameraProjection(double* observation)
- : intrinsic_projection_(new IntrinsicProjection(observation_)) {
+ : intrinsic_projection_(new IntrinsicProjection(observation)) {
}
template <typename T>
@@ -677,6 +677,8 @@
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 @@
.. 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 @@
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 @@
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 @@
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`
================================
diff --git a/include/ceres/cost_function_to_functor.h b/include/ceres/cost_function_to_functor.h
index 08b4116..6c67ac0 100644
--- a/include/ceres/cost_function_to_functor.h
+++ b/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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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
diff --git a/include/ceres/dynamic_cost_function_to_functor.h b/include/ceres/dynamic_cost_function_to_functor.h
new file mode 100644
index 0000000..9339a50
--- /dev/null
+++ b/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_
diff --git a/internal/ceres/cost_function_to_functor_test.cc b/internal/ceres/cost_function_to_functor_test.cc
index 73a6892..6c9a940 100644
--- a/internal/ceres/cost_function_to_functor_test.cc
+++ b/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 @@
}
};
+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 @@
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
