NumericDiffFunctor. A wrapper class that takes a variadic functor evaluating a function, numerically differentiates it and makes it available as a templated functor so that it can be easily used as part of Ceres' automatic differentiation framework. The tests for NumericDiffCostFunction and NumericDiffFunctor have a lot of stuff that is common, so refactor them to reduce code. Change-Id: I83b01e58b05e575fb2530d15cbd611928298646a
diff --git a/include/ceres/numeric_diff_functor.h b/include/ceres/numeric_diff_functor.h new file mode 100644 index 0000000..14adbed --- /dev/null +++ b/include/ceres/numeric_diff_functor.h
@@ -0,0 +1,341 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) +// +// A wrapper class that takes a variadic functor evaluating a +// function, numerically differentiates it and makes it available as a +// templated functor so that it can be easily used as part of Ceres' +// automatic differentiation framework. +// +// For example: +// +// For example, let us assume that +// +// struct IntrinsicProjection +// IntrinsicProjection(const double* observations); +// bool operator()(const double* calibration, +// const double* point, +// double* residuals); +// }; +// +// is a functor that implements the projection of a point in its local +// coordinate system onto its image plane and subtracts it from the +// observed point projection. +// +// Now we would like to compose the action of this functor with the +// action of camera extrinsics, i.e., rotation and translation, which +// is given by the following templated function +// +// template<typename T> +// void RotateAndTranslatePoint(const T* rotation, +// const T* translation, +// const T* point, +// T* result); +// +// To compose the extrinsics and intrinsics, we can construct a +// CameraProjection functor as follows. +// +// struct CameraProjection { +// typedef NumericDiffFunctor<IntrinsicProjection, CENTRAL, 2, 5, 3> +// IntrinsicProjectionFunctor; +// +// CameraProjection(double* observation) { +// intrinsic_projection_.reset( +// new IntrinsicProjectionFunctor(observation)) { +// } +// +// template <typename T> +// bool operator(const T* rotation, +// const T* translation, +// const T* intrinsics, +// const T* point, +// T* residuals) const { +// T transformed_point[3]; +// RotateAndTranslatePoint(rotation, translation, point, transformed_point); +// return (*intrinsic_projection_)(intrinsics, transformed_point, residual); +// } +// +// private: +// scoped_ptr<IntrinsicProjectionFunctor> intrinsic_projection_; +// }; +// +// Here, we made the choice of using CENTRAL differences to compute +// the jacobian of IntrinsicProjection. +// +// Now, we are ready to construct an automatically differentiated cost +// function as +// +// CostFunction* cost_function = +// new AutoDiffCostFunction<CameraProjection, 2, 3, 3, 5>( +// new CameraProjection(observations)); +// +// cost_function now seamlessly integrates automatic differentiation +// of RotateAndTranslatePoint with a numerically differentiated +// version of IntrinsicProjection. + +#include "ceres/numeric_diff_cost_function.h" +#include "ceres/types.h" +#include "ceres/cost_function_to_functor.h" + +namespace ceres { + +template<typename Functor, + NumericDiffMethod kMethod = CENTRAL, + int kNumResiduals = 0, + int N0 = 0, int N1 = 0 , int N2 = 0, int N3 = 0, int N4 = 0, + int N5 = 0, int N6 = 0 , int N7 = 0, int N8 = 0, int N9 = 0> +class NumericDiffFunctor { + public: + // relative_step_size controls the step size used by the numeric + // differentiation process. + NumericDiffFunctor(double relative_step_size = 1e-6) + : functor_(new NumericDiffCostFunction<Functor, + kMethod, + kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9>( + new Functor, relative_step_size)) { + } + + NumericDiffFunctor(Functor* functor, double relative_step_size = 1e-6) + : functor_(new NumericDiffCostFunction<Functor, + kMethod, + kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9>( + functor, relative_step_size)) { + } + + bool operator()(const double* x0, double* residuals) const { + functor_(x0, residuals); + } + + bool operator()(const double* x0, + const double* x1, + double* residuals) const { + return functor_(x0, x1, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + double* residuals) const { + return functor_(x0, x1, x2, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + double* residuals) const { + return functor_(x0, x1, x2, x3, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + double* residuals) const { + return functor_(x0, x1, x2, x3, x4, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + double* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + double* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + const double* x7, + double* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + const double* x7, + const double* x8, + double* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals); + } + + bool operator()(const double* x0, + const double* x1, + const double* x2, + const double* x3, + const double* x4, + const double* x5, + const double* x6, + const double* x7, + const double* x8, + const double* x9, + double* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals); + } + + template <typename T> + bool operator()(const T* x0, T* residuals) const { + functor_(x0, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + T* residuals) const { + return functor_(x0, x1, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + T* residuals) const { + return functor_(x0, x1, x2, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + T* residuals) const { + return functor_(x0, x1, x2, x3, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + const T* x4, + T* residuals) const { + return functor_(x0, x1, x2, x3, x4, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + const T* x4, + const T* x5, + T* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + const T* x4, + const T* x5, + const T* x6, + T* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + const T* x4, + const T* x5, + const T* x6, + const T* x7, + T* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, x7, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + const T* x4, + const T* x5, + const T* x6, + const T* x7, + const T* x8, + T* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, residuals); + } + + template <typename T> + bool operator()(const T* x0, + const T* x1, + const T* x2, + const T* x3, + const T* x4, + const T* x5, + const T* x6, + const T* x7, + const T* x8, + const T* x9, + T* residuals) const { + return functor_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, residuals); + } + + + private: + CostFunctionToFunctor<kNumResiduals, + N0, N1, N2, N3, N4, + N5, N6, N7, N8, N9> functor_; + +}; + +} // namespace ceres
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 193dd63..f9329e9 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -201,7 +201,7 @@ IF (${BUILD_TESTING} AND ${GFLAGS}) ADD_LIBRARY(gtest gmock_gtest_all.cc gmock_main.cc) - ADD_LIBRARY(test_util test_util.cc) + ADD_LIBRARY(test_util test_util.cc numeric_diff_test_utils.cc) TARGET_LINK_LIBRARIES(gtest ${GFLAGS_LIB} ${GLOG_LIB}) MACRO (CERES_TEST NAME) @@ -240,6 +240,7 @@ CERES_TEST(minimizer) CERES_TEST(normal_prior) CERES_TEST(numeric_diff_cost_function) + CERES_TEST(numeric_diff_functor) CERES_TEST(ordered_groups) CERES_TEST(parameter_block) CERES_TEST(parameter_block_ordering)
diff --git a/internal/ceres/numeric_diff_cost_function_test.cc b/internal/ceres/numeric_diff_cost_function_test.cc index 296cc22..3953ded 100644 --- a/internal/ceres/numeric_diff_cost_function_test.cc +++ b/internal/ceres/numeric_diff_cost_function_test.cc
@@ -36,8 +36,7 @@ #include <vector> #include "ceres/internal/macros.h" #include "ceres/internal/scoped_ptr.h" -#include "ceres/sized_cost_function.h" -#include "ceres/stringprintf.h" +#include "ceres/numeric_diff_test_utils.h" #include "ceres/test_util.h" #include "ceres/types.h" #include "glog/logging.h" @@ -46,223 +45,109 @@ namespace ceres { namespace internal { -// y1 = x1'x2 -> dy1/dx1 = x2, dy1/dx2 = x1 -// y2 = (x1'x2)^2 -> dy2/dx1 = 2 * x2 * (x1'x2), dy2/dx2 = 2 * x1 * (x1'x2) -// y3 = x2'x2 -> dy3/dx1 = 0, dy3/dx2 = 2 * x2 -struct EasyFunctor { - bool operator()(const double* x1, const double* x2, double* residuals) const { - residuals[0] = residuals[1] = residuals[2] = 0; - for (int i = 0; i < 5; ++i) { - residuals[0] += x1[i] * x2[i]; - residuals[2] += x2[i] * x2[i]; - } - residuals[1] = residuals[0] * residuals[0]; - return true; - } -}; +TEST(NumericDiffCostFunction, EasyCaseFunctorCentralDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( + new NumericDiffCostFunction<EasyFunctor, + CENTRAL, + 3, /* number of residuals */ + 5, /* size of x1 */ + 5 /* size of x2 */>( + new EasyFunctor)); + EasyFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} -class EasyCostFunction : public SizedCostFunction<3, 5, 5> { - public: - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - (void) jacobians; // Ignored. - return EasyFunctor()(parameters[0], parameters[1], residuals); - } -}; +TEST(NumericDiffCostFunction, EasyCaseFunctorForwardDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( + new NumericDiffCostFunction<EasyFunctor, + FORWARD, + 3, /* number of residuals */ + 5, /* size of x1 */ + 5 /* size of x2 */>( + new EasyFunctor)); + EasyFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); +} -TEST(NumericDiffCostFunction, EasyCase) { - // Try both central and forward difference. - internal::scoped_ptr<CostFunction> cfs[4]; - cfs[0].reset( +TEST(NumericDiffCostFunction, EasyCaseCostFunctionCentralDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( new NumericDiffCostFunction<EasyCostFunction, CENTRAL, 3, /* number of residuals */ 5, /* size of x1 */ 5 /* size of x2 */>( new EasyCostFunction, TAKE_OWNERSHIP)); + EasyFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} - cfs[1].reset( +TEST(NumericDiffCostFunction, EasyCaseCostFunctionForwardDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( new NumericDiffCostFunction<EasyCostFunction, FORWARD, 3, /* number of residuals */ 5, /* size of x1 */ 5 /* size of x2 */>( new EasyCostFunction, TAKE_OWNERSHIP)); - - cfs[2].reset( - new NumericDiffCostFunction< EasyFunctor, - CENTRAL, - 3, /* number of residuals */ - 5, /* size of x1 */ - 5 /* size of x2 */>( - new EasyFunctor)); - - cfs[3].reset( - new NumericDiffCostFunction< EasyFunctor, - FORWARD, - 3, /* number of residuals */ - 5, /* size of x1 */ - 5 /* size of x2 */>( - new EasyFunctor)); - - - for (int c = 0; c < 4; ++c) { - CostFunction *cost_function = cfs[c].get(); - - double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 }; - double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 }; - double *parameters[] = { &x1[0], &x2[0] }; - - double dydx1[15]; // 3 x 5, row major. - double dydx2[15]; // 3 x 5, row major. - double *jacobians[2] = { &dydx1[0], &dydx2[0] }; - - double residuals[3] = {-1e-100, -2e-100, -3e-100 }; - - ASSERT_TRUE(cost_function->Evaluate(¶meters[0], - &residuals[0], - &jacobians[0])); - - EXPECT_EQ(residuals[0], 67); - EXPECT_EQ(residuals[1], 4489); - EXPECT_EQ(residuals[2], 213); - - for (int i = 0; i < 5; ++i) { - LOG(INFO) << "c = " << c << " i = " << i; - const double kEps = c == 0 ? /* central */ 3e-9 : /* forward */ 2e-5; - - ExpectClose(x2[i], dydx1[5 * 0 + i], kEps); // y1 - ExpectClose(x1[i], dydx2[5 * 0 + i], kEps); - ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], kEps); // y2 - ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], kEps); - ExpectClose(0.0, dydx1[5 * 2 + i], kEps); // y3 - ExpectClose(2 * x2[i], dydx2[5 * 2 + i], kEps); - } - } + EasyFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); } -// y1 = sin(x1'x2) -// y2 = exp(-x1'x2 / 10) -// -// dy1/dx1 = x2 * cos(x1'x2), dy1/dx2 = x1 * cos(x1'x2) -// dy2/dx1 = -x2 * exp(-x1'x2 / 10) / 10, dy2/dx2 = -x2 * exp(-x1'x2 / 10) / 10 -struct TranscendentalFunctor { - bool operator()(const double* x1, const double* x2, double* residuals) const { - double x1x2 = 0; - for (int i = 0; i < 5; ++i) { - x1x2 += x1[i] * x2[i]; - } - residuals[0] = sin(x1x2); - residuals[1] = exp(-x1x2 / 10); - return true; - } -}; - -class TranscendentalTestCostFunction : public SizedCostFunction<2, 5, 5> { - public: - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - (void) jacobians; // Ignored. - return TranscendentalFunctor()(parameters[0], parameters[1], residuals); - } -}; - -TEST(NumericDiffCostFunction, TransendentalOperationsInCostFunction) { - // Try both central and forward difference. - internal::scoped_ptr<CostFunction> cfs[4]; - cfs[0].reset( - new NumericDiffCostFunction<TranscendentalTestCostFunction, - CENTRAL, - 2, /* number of residuals */ - 5, /* size of x1 */ - 5 /* size of x2 */>( - new TranscendentalTestCostFunction, TAKE_OWNERSHIP)); - - cfs[1].reset( - new NumericDiffCostFunction<TranscendentalTestCostFunction, - FORWARD, - 2, /* number of residuals */ - 5, /* size of x1 */ - 5 /* size of x2 */>( - new TranscendentalTestCostFunction, TAKE_OWNERSHIP)); - - cfs[2].reset( +TEST(NumericDiffCostFunction, TranscendentalCaseFunctorCentralDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( new NumericDiffCostFunction<TranscendentalFunctor, CENTRAL, 2, /* number of residuals */ 5, /* size of x1 */ 5 /* size of x2 */>( - new TranscendentalFunctor)); + new TranscendentalFunctor)); + TranscendentalFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} - cfs[3].reset( +TEST(NumericDiffCostFunction, TranscendentalCaseFunctorForwardDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( new NumericDiffCostFunction<TranscendentalFunctor, FORWARD, 2, /* number of residuals */ 5, /* size of x1 */ 5 /* size of x2 */>( - new TranscendentalFunctor)); - - for (int c = 0; c < 4; ++c) { - CostFunction *cost_function = cfs[c].get(); - - struct { - double x1[5]; - double x2[5]; - } kTests[] = { - { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // No zeros. - { 9.0, 9.0, 5.0, 5.0, 1.0 }, - }, - { { 0.0, 2.0, 3.0, 0.0, 5.0 }, // Some zeros x1. - { 9.0, 9.0, 5.0, 5.0, 1.0 }, - }, - { { 1.0, 2.0, 3.0, 1.0, 5.0 }, // Some zeros x2. - { 0.0, 9.0, 0.0, 5.0, 0.0 }, - }, - { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros x1. - { 9.0, 9.0, 5.0, 5.0, 1.0 }, - }, - { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // All zeros x2. - { 0.0, 0.0, 0.0, 0.0, 0.0 }, - }, - { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros. - { 0.0, 0.0, 0.0, 0.0, 0.0 }, - }, - }; - for (int k = 0; k < CERES_ARRAYSIZE(kTests); ++k) { - double *x1 = &(kTests[k].x1[0]); - double *x2 = &(kTests[k].x2[0]); - double *parameters[] = { x1, x2 }; - - double dydx1[10]; - double dydx2[10]; - double *jacobians[2] = { &dydx1[0], &dydx2[0] }; - - double residuals[2]; - - ASSERT_TRUE(cost_function->Evaluate(¶meters[0], - &residuals[0], - &jacobians[0])); - LOG(INFO) << "Ran evaluate for test k=" << k << " c=" << c; - - double x1x2 = 0; - for (int i = 0; i < 5; ++i) { - x1x2 += x1[i] * x2[i]; - } - - for (int i = 0; i < 5; ++i) { - const double kEps = c == 0 ? /* central */ 3e-9 : /* forward */ 2e-5; - - ExpectClose( x2[i] * cos(x1x2), dydx1[5 * 0 + i], kEps); - ExpectClose( x1[i] * cos(x1x2), dydx2[5 * 0 + i], kEps); - ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], kEps); - ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], kEps); - } - } - } + new TranscendentalFunctor)); + TranscendentalFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); } +TEST(NumericDiffCostFunction, TranscendentalCaseCostFunctionCentralDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( + new NumericDiffCostFunction<TranscendentalCostFunction, + CENTRAL, + 2, /* number of residuals */ + 5, /* size of x1 */ + 5 /* size of x2 */>( + new TranscendentalCostFunction, TAKE_OWNERSHIP)); + TranscendentalFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} + +TEST(NumericDiffCostFunction, TranscendentalCaseCostFunctionForwardDifferences) { + internal::scoped_ptr<CostFunction> cost_function; + cost_function.reset( + new NumericDiffCostFunction<TranscendentalCostFunction, + FORWARD, + 2, /* number of residuals */ + 5, /* size of x1 */ + 5 /* size of x2 */>( + new TranscendentalCostFunction, TAKE_OWNERSHIP)); + TranscendentalFunctor functor; + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); +} template<int num_rows, int num_cols> class SizeTestingCostFunction : public SizedCostFunction<num_rows, num_cols> {
diff --git a/internal/ceres/numeric_diff_functor_test.cc b/internal/ceres/numeric_diff_functor_test.cc new file mode 100644 index 0000000..a37ceaf --- /dev/null +++ b/internal/ceres/numeric_diff_functor_test.cc
@@ -0,0 +1,123 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2013 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) + +#include "ceres/numeric_diff_functor.h" + +#include <algorithm> +#include <cmath> +#include <string> +#include <vector> +#include "ceres/autodiff_cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/numeric_diff_test_utils.h" +#include "ceres/test_util.h" +#include "ceres/types.h" +#include "glog/logging.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +TEST(NumericDiffCostFunction, EasyCaseCentralDifferences) { + typedef NumericDiffFunctor<EasyFunctor, CENTRAL, 3, 5, 5> NumericDiffEasyFunctor; + + internal::scoped_ptr<CostFunction> cost_function; + EasyFunctor functor; + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>( + new NumericDiffEasyFunctor)); + + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>( + new NumericDiffEasyFunctor(new EasyFunctor))); + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} + +TEST(NumericDiffCostFunction, EasyCaseForwardDifferences) { + typedef NumericDiffFunctor<EasyFunctor, FORWARD, 3, 5, 5> NumericDiffEasyFunctor; + + internal::scoped_ptr<CostFunction> cost_function; + EasyFunctor functor; + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>( + new NumericDiffEasyFunctor)); + + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffEasyFunctor, 3, 5, 5>( + new NumericDiffEasyFunctor(new EasyFunctor))); + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); +} + +TEST(NumericDiffCostFunction, TranscendentalCaseCentralDifferences) { + typedef NumericDiffFunctor<TranscendentalFunctor, CENTRAL, 2, 5, 5> + NumericDiffTranscendentalFunctor; + + internal::scoped_ptr<CostFunction> cost_function; + TranscendentalFunctor functor; + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>( + new NumericDiffTranscendentalFunctor)); + + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>( + new NumericDiffTranscendentalFunctor(new TranscendentalFunctor))); + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); +} + +TEST(NumericDiffCostFunction, TranscendentalCaseForwardDifferences) { + typedef NumericDiffFunctor<TranscendentalFunctor, FORWARD, 2, 5, 5> + NumericDiffTranscendentalFunctor; + + internal::scoped_ptr<CostFunction> cost_function; + TranscendentalFunctor functor; + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>( + new NumericDiffTranscendentalFunctor)); + + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); + + cost_function.reset( + new AutoDiffCostFunction<NumericDiffTranscendentalFunctor, 2, 5, 5>( + new NumericDiffTranscendentalFunctor(new TranscendentalFunctor))); + functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/numeric_diff_test_utils.cc b/internal/ceres/numeric_diff_test_utils.cc new file mode 100644 index 0000000..6786ac9 --- /dev/null +++ b/internal/ceres/numeric_diff_test_utils.cc
@@ -0,0 +1,130 @@ +#include "ceres/numeric_diff_test_utils.h" + +#include <algorithm> +#include <cmath> +#include "ceres/cost_function.h" +#include "ceres/internal/macros.h" +#include "ceres/test_util.h" +#include "ceres/types.h" +#include "gtest/gtest.h" + + +namespace ceres { +namespace internal { + +bool EasyFunctor::operator()(const double* x1, + const double* x2, + double* residuals) const { + residuals[0] = residuals[1] = residuals[2] = 0; + for (int i = 0; i < 5; ++i) { + residuals[0] += x1[i] * x2[i]; + residuals[2] += x2[i] * x2[i]; + } + residuals[1] = residuals[0] * residuals[0]; + return true; +} + +void EasyFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect( + const CostFunction& cost_function, + NumericDiffMethod method) const { + double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 }; + double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 }; + double *parameters[] = { &x1[0], &x2[0] }; + + double dydx1[15]; // 3 x 5, row major. + double dydx2[15]; // 3 x 5, row major. + double *jacobians[2] = { &dydx1[0], &dydx2[0] }; + + double residuals[3] = {-1e-100, -2e-100, -3e-100 }; + + ASSERT_TRUE(cost_function.Evaluate(¶meters[0], + &residuals[0], + &jacobians[0])); + + EXPECT_EQ(residuals[0], 67); + EXPECT_EQ(residuals[1], 4489); + EXPECT_EQ(residuals[2], 213); + + const double tolerance = (method == CENTRAL)? 3e-9 : 2e-5; + + for (int i = 0; i < 5; ++i) { + ExpectClose(x2[i], dydx1[5 * 0 + i], tolerance); // y1 + ExpectClose(x1[i], dydx2[5 * 0 + i], tolerance); + ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], tolerance); // y2 + ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], tolerance); + ExpectClose(0.0, dydx1[5 * 2 + i], tolerance); // y3 + ExpectClose(2 * x2[i], dydx2[5 * 2 + i], tolerance); + } +} + +bool TranscendentalFunctor::operator()(const double* x1, + const double* x2, + double* residuals) const { + double x1x2 = 0; + for (int i = 0; i < 5; ++i) { + x1x2 += x1[i] * x2[i]; + } + residuals[0] = sin(x1x2); + residuals[1] = exp(-x1x2 / 10); + return true; +} + +void TranscendentalFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect( + const CostFunction& cost_function, + NumericDiffMethod method) const { + struct { + double x1[5]; + double x2[5]; + } kTests[] = { + { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // No zeros. + { 9.0, 9.0, 5.0, 5.0, 1.0 }, + }, + { { 0.0, 2.0, 3.0, 0.0, 5.0 }, // Some zeros x1. + { 9.0, 9.0, 5.0, 5.0, 1.0 }, + }, + { { 1.0, 2.0, 3.0, 1.0, 5.0 }, // Some zeros x2. + { 0.0, 9.0, 0.0, 5.0, 0.0 }, + }, + { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros x1. + { 9.0, 9.0, 5.0, 5.0, 1.0 }, + }, + { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // All zeros x2. + { 0.0, 0.0, 0.0, 0.0, 0.0 }, + }, + { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros. + { 0.0, 0.0, 0.0, 0.0, 0.0 }, + }, + }; + + for (int k = 0; k < CERES_ARRAYSIZE(kTests); ++k) { + double *x1 = &(kTests[k].x1[0]); + double *x2 = &(kTests[k].x2[0]); + double *parameters[] = { x1, x2 }; + + double dydx1[10]; + double dydx2[10]; + double *jacobians[2] = { &dydx1[0], &dydx2[0] }; + + double residuals[2]; + + ASSERT_TRUE(cost_function.Evaluate(¶meters[0], + &residuals[0], + &jacobians[0])); + double x1x2 = 0; + for (int i = 0; i < 5; ++i) { + x1x2 += x1[i] * x2[i]; + } + + const double tolerance = (method == CENTRAL)? 3e-9 : 2e-5; + + for (int i = 0; i < 5; ++i) { + ExpectClose( x2[i] * cos(x1x2), dydx1[5 * 0 + i], tolerance); + ExpectClose( x1[i] * cos(x1x2), dydx2[5 * 0 + i], tolerance); + ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], tolerance); + ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], tolerance); + } + } +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/numeric_diff_test_utils.h b/internal/ceres/numeric_diff_test_utils.h new file mode 100644 index 0000000..1a43df2 --- /dev/null +++ b/internal/ceres/numeric_diff_test_utils.h
@@ -0,0 +1,56 @@ +#include "ceres/cost_function.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// y1 = x1'x2 -> dy1/dx1 = x2, dy1/dx2 = x1 +// y2 = (x1'x2)^2 -> dy2/dx1 = 2 * x2 * (x1'x2), dy2/dx2 = 2 * x1 * (x1'x2) +// y3 = x2'x2 -> dy3/dx1 = 0, dy3/dx2 = 2 * x2 +class EasyFunctor { + public: + bool operator()(const double* x1, const double* x2, double* residuals) const; + void ExpectCostFunctionEvaluationIsNearlyCorrect( + const CostFunction& cost_function, + NumericDiffMethod method) const; +}; + +class EasyCostFunction : public SizedCostFunction<3, 5, 5> { + public: + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** /* not used */) const { + return functor_(parameters[0], parameters[1], residuals); + } + + private: + EasyFunctor functor_; +}; + +// y1 = sin(x1'x2) +// y2 = exp(-x1'x2 / 10) +// +// dy1/dx1 = x2 * cos(x1'x2), dy1/dx2 = x1 * cos(x1'x2) +// dy2/dx1 = -x2 * exp(-x1'x2 / 10) / 10, dy2/dx2 = -x2 * exp(-x1'x2 / 10) / 10 +class TranscendentalFunctor { + public: + bool operator()(const double* x1, const double* x2, double* residuals) const; + void ExpectCostFunctionEvaluationIsNearlyCorrect( + const CostFunction& cost_function, + NumericDiffMethod method) const; +}; + +class TranscendentalCostFunction : public SizedCostFunction<2, 5, 5> { + public: + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** /* not used */) const { + return functor_(parameters[0], parameters[1], residuals); + } + private: + TranscendentalFunctor functor_; +}; + +} // namespace internal +} // namespace ceres