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