Remove RuntimeNumericDiffCostFunction. Move the GradientCheckingCostFunction to DynamicNumericDiffCostFunction. Also fix a const correctness issue with DynamicNumericDiffCostFunction. Change-Id: Id446810f43374e7b7db7fe4dd01a891e3c54abb9
diff --git a/CMakeLists.txt b/CMakeLists.txt index f451588..982453b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt
@@ -263,7 +263,7 @@ ELSE (CXSPARSE) MESSAGE("-- Building without CXSparse.") ADD_DEFINITIONS(-DCERES_NO_CXSPARSE) -ENDIF (CXSPARSE) +ENDIF (CXSPARSE) # GFlags. IF (GFLAGS)
diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h index 7216c8e..c2bfb32 100644 --- a/include/ceres/dynamic_numeric_diff_cost_function.h +++ b/include/ceres/dynamic_numeric_diff_cost_function.h
@@ -67,6 +67,7 @@ #include "ceres/cost_function.h" #include "ceres/internal/scoped_ptr.h" #include "ceres/internal/eigen.h" +#include "ceres/internal/numeric_diff.h" #include "glog/logging.h" namespace ceres { @@ -74,7 +75,7 @@ template <typename CostFunctor, NumericDiffMethod method = CENTRAL> class DynamicNumericDiffCostFunction : public CostFunction { public: - explicit DynamicNumericDiffCostFunction(CostFunctor* functor, + explicit DynamicNumericDiffCostFunction(const CostFunctor* functor, Ownership ownership = TAKE_OWNERSHIP, double relative_step_size = 1e-6) : functor_(functor), @@ -108,7 +109,7 @@ << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " << "before DynamicNumericDiffCostFunction::Evaluate()."; - const bool status = (*functor_)(parameters, residuals); + const bool status = EvaluateCostFunctor(parameters, residuals); if (jacobians == NULL || !status) { return status; } @@ -194,7 +195,7 @@ x_plus_delta(j) = x(j) + step_size(j); ResidualVector residuals(num_residuals); - if (!(*functor_)(parameters, &residuals[0])) { + if (!EvaluateCostFunctor(parameters, &residuals[0])) { // Something went wrong; bail. return false; } @@ -210,7 +211,7 @@ // Compute the function on the other side of x(j). x_plus_delta(j) = x(j) - step_size(j); - if (!(*functor_)(parameters, &residuals[0])) { + if (!EvaluateCostFunctor(parameters, &residuals[0])) { // Something went wrong; bail. return false; } @@ -230,7 +231,31 @@ return true; } - internal::scoped_ptr<CostFunctor> functor_; + bool EvaluateCostFunctor(double const* const* parameters, + double* residuals) const { + return EvaluateCostFunctorImpl(functor_.get(), + parameters, + residuals, + functor_.get()); + } + + // Helper templates to allow evaluation of a functor or a + // CostFunction. + bool EvaluateCostFunctorImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const void* /* NOT USED */) const { + return (*functor)(parameters, residuals); + } + + bool EvaluateCostFunctorImpl(const CostFunctor* functor, + double const* const* parameters, + double* residuals, + const CostFunction* /* NOT USED */) const { + return functor->Evaluate(parameters, residuals, NULL); + } + + internal::scoped_ptr<const CostFunctor> functor_; Ownership ownership_; const double relative_step_size_; };
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 31d737d..cefe809 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -86,7 +86,6 @@ program.cc residual_block.cc residual_block_utils.cc - runtime_numeric_diff_cost_function.cc schur_complement_solver.cc schur_eliminator.cc schur_jacobi_preconditioner.cc @@ -247,7 +246,6 @@ CERES_TEST(residual_block) CERES_TEST(residual_block_utils) CERES_TEST(rotation) - CERES_TEST(runtime_numeric_diff_cost_function) CERES_TEST(schur_complement_solver) CERES_TEST(schur_eliminator) CERES_TEST(small_blas)
diff --git a/internal/ceres/gradient_checking_cost_function.cc b/internal/ceres/gradient_checking_cost_function.cc index 3edf95d..5503013 100644 --- a/internal/ceres/gradient_checking_cost_function.cc +++ b/internal/ceres/gradient_checking_cost_function.cc
@@ -44,7 +44,7 @@ #include "ceres/problem_impl.h" #include "ceres/program.h" #include "ceres/residual_block.h" -#include "ceres/runtime_numeric_diff_cost_function.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" #include "ceres/stringprintf.h" #include "ceres/types.h" #include "glog/logging.h" @@ -84,14 +84,24 @@ double relative_precision, const string& extra_info) : function_(function), - finite_diff_cost_function_( - CreateRuntimeNumericDiffCostFunction(function, - CENTRAL, - relative_step_size)), relative_precision_(relative_precision), extra_info_(extra_info) { - *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + DynamicNumericDiffCostFunction<CostFunction, CENTRAL>* + finite_diff_cost_function = + new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>( + function, + DO_NOT_TAKE_OWNERSHIP, + relative_step_size); + + const vector<int16>& parameter_block_sizes = + function->parameter_block_sizes(); + for (int i = 0; i < parameter_block_sizes.size(); ++i) { + finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]); + } + *mutable_parameter_block_sizes() = parameter_block_sizes; set_num_residuals(function->num_residuals()); + finite_diff_cost_function->SetNumResiduals(num_residuals()); + finite_diff_cost_function_.reset(finite_diff_cost_function); } virtual ~GradientCheckingCostFunction() { }
diff --git a/internal/ceres/runtime_numeric_diff_cost_function.cc b/internal/ceres/runtime_numeric_diff_cost_function.cc deleted file mode 100644 index 7af275c..0000000 --- a/internal/ceres/runtime_numeric_diff_cost_function.cc +++ /dev/null
@@ -1,217 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) -// -// Based on the templated version in public/numeric_diff_cost_function.h. - -#include "ceres/runtime_numeric_diff_cost_function.h" - -#include <algorithm> -#include <numeric> -#include <vector> -#include "Eigen/Dense" -#include "ceres/cost_function.h" -#include "ceres/internal/scoped_ptr.h" -#include "glog/logging.h" - -namespace ceres { -namespace internal { -namespace { - -bool EvaluateJacobianForParameterBlock(const CostFunction* function, - int parameter_block_size, - int parameter_block, - RuntimeNumericDiffMethod method, - double relative_step_size, - double const* residuals_at_eval_point, - double** parameters, - double** jacobians) { - using Eigen::Map; - using Eigen::Matrix; - using Eigen::Dynamic; - using Eigen::RowMajor; - - typedef Matrix<double, Dynamic, 1> ResidualVector; - typedef Matrix<double, Dynamic, 1> ParameterVector; - typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix; - - int num_residuals = function->num_residuals(); - - Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], - num_residuals, - parameter_block_size); - - // Mutate one element at a time and then restore. - Map<ParameterVector> x_plus_delta(parameters[parameter_block], - parameter_block_size); - ParameterVector x(x_plus_delta); - ParameterVector step_size = x.array().abs() * relative_step_size; - - // To handle cases where a paremeter is exactly zero, instead use the mean - // step_size for the other dimensions. - double fallback_step_size = step_size.sum() / step_size.rows(); - if (fallback_step_size == 0.0) { - // If all the parameters are zero, there's no good answer. Use the given - // relative step_size as absolute step_size and hope for the best. - fallback_step_size = relative_step_size; - } - - // For each parameter in the parameter block, use finite differences to - // compute the derivative for that parameter. - for (int j = 0; j < parameter_block_size; ++j) { - if (step_size(j) == 0.0) { - // The parameter is exactly zero, so compromise and use the mean step_size - // from the other parameters. This can break in many cases, but it's hard - // to pick a good number without problem specific knowledge. - step_size(j) = fallback_step_size; - } - x_plus_delta(j) = x(j) + step_size(j); - - ResidualVector residuals(num_residuals); - if (!function->Evaluate(parameters, &residuals[0], NULL)) { - // Something went wrong; bail. - return false; - } - - // Compute this column of the jacobian in 3 steps: - // 1. Store residuals for the forward part. - // 2. Subtract residuals for the backward (or 0) part. - // 3. Divide out the run. - parameter_jacobian.col(j) = residuals; - - double one_over_h = 1 / step_size(j); - if (method == CENTRAL) { - // Compute the function on the other side of x(j). - x_plus_delta(j) = x(j) - step_size(j); - - if (!function->Evaluate(parameters, &residuals[0], NULL)) { - // Something went wrong; bail. - return false; - } - parameter_jacobian.col(j) -= residuals; - one_over_h /= 2; - } else { - // Forward difference only; reuse existing residuals evaluation. - parameter_jacobian.col(j) -= - Map<const ResidualVector>(residuals_at_eval_point, num_residuals); - } - x_plus_delta(j) = x(j); // Restore x_plus_delta. - - // Divide out the run to get slope. - parameter_jacobian.col(j) *= one_over_h; - } - return true; -} - -class RuntimeNumericDiffCostFunction : public CostFunction { - public: - RuntimeNumericDiffCostFunction(const CostFunction* function, - RuntimeNumericDiffMethod method, - double relative_step_size) - : function_(function), - method_(method), - relative_step_size_(relative_step_size) { - *mutable_parameter_block_sizes() = function->parameter_block_sizes(); - set_num_residuals(function->num_residuals()); - } - - virtual ~RuntimeNumericDiffCostFunction() { } - - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - // Get the function value (residuals) at the the point to evaluate. - bool success = function_->Evaluate(parameters, residuals, NULL); - if (!success) { - // Something went wrong; ignore the jacobian. - return false; - } - if (!jacobians) { - // Nothing to do; just forward. - return true; - } - - const vector<int16>& block_sizes = function_->parameter_block_sizes(); - CHECK(!block_sizes.empty()); - - // Create local space for a copy of the parameters which will get mutated. - int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); - vector<double> parameters_copy(parameters_size); - vector<double*> parameters_references_copy(block_sizes.size()); - parameters_references_copy[0] = ¶meters_copy[0]; - for (int block = 1; block < block_sizes.size(); ++block) { - parameters_references_copy[block] = parameters_references_copy[block - 1] - + block_sizes[block - 1]; - } - - // Copy the parameters into the local temp space. - for (int block = 0; block < block_sizes.size(); ++block) { - memcpy(parameters_references_copy[block], - parameters[block], - block_sizes[block] * sizeof(*parameters[block])); - } - - for (int block = 0; block < block_sizes.size(); ++block) { - if (!jacobians[block]) { - // No jacobian requested for this parameter / residual pair. - continue; - } - if (!EvaluateJacobianForParameterBlock(function_, - block_sizes[block], - block, - method_, - relative_step_size_, - residuals, - ¶meters_references_copy[0], - jacobians)) { - return false; - } - } - return true; - } - - private: - const CostFunction* function_; - RuntimeNumericDiffMethod method_; - double relative_step_size_; -}; - -} // namespace - -CostFunction* CreateRuntimeNumericDiffCostFunction( - const CostFunction* cost_function, - RuntimeNumericDiffMethod method, - double relative_step_size) { - return new RuntimeNumericDiffCostFunction(cost_function, - method, - relative_step_size); -} - -} // namespace internal -} // namespace ceres
diff --git a/internal/ceres/runtime_numeric_diff_cost_function.h b/internal/ceres/runtime_numeric_diff_cost_function.h deleted file mode 100644 index 01b57f9..0000000 --- a/internal/ceres/runtime_numeric_diff_cost_function.h +++ /dev/null
@@ -1,87 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) -// -// Create CostFunctions as needed by the least squares framework with jacobians -// computed via numeric differentiation. -// -// To get a numerically differentiated cost function, define a subclass of -// CostFunction such that the Evaluate() function ignores the jacobian -// parameter. The numeric differentiation wrapper will fill in the jacobian -// parameter if nececssary by repeatedly calling the Evaluate() function with -// small changes to the appropriate parameters, and computing the slope. This -// implementation is not templated (hence the "Runtime" prefix), which is a bit -// slower than but is more convenient than the templated version in -// numeric_diff_cost_function.h -// -// The numerically differentiated version of a cost function for a cost function -// can be constructed as follows: -// -// CostFunction* cost_function = -// CreateRuntimeNumericDiffCostFunction(new MyCostFunction(...), -// CENTRAL, -// TAKE_OWNERSHIP); -// -// The central difference method is considerably more accurate; consider using -// to start and only after that works, trying forward difference. -// -// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. - -#ifndef CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ -#define CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ - -#include "ceres/cost_function.h" - -namespace ceres { -namespace internal { - -enum RuntimeNumericDiffMethod { - CENTRAL, - FORWARD, -}; - -// Create a cost function that evaluates the derivative with finite differences. -// The base cost_function's implementation of Evaluate() only needs to fill in -// the "residuals" argument and not the "jacobians". Any data written to the -// jacobians by the base cost_function is overwritten. -// -// Forward difference or central difference is selected with CENTRAL or FORWARD. -// The relative eps, which determines the step size for forward and central -// differencing, is set with relative eps. Caller owns the resulting cost -// function, and the resulting cost function does not own the base cost -// function. -CostFunction *CreateRuntimeNumericDiffCostFunction( - const CostFunction *cost_function, - RuntimeNumericDiffMethod method, - double relative_eps); - -} // namespace internal -} // namespace ceres - -#endif // CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_
diff --git a/internal/ceres/runtime_numeric_diff_cost_function_test.cc b/internal/ceres/runtime_numeric_diff_cost_function_test.cc deleted file mode 100644 index 71469ea..0000000 --- a/internal/ceres/runtime_numeric_diff_cost_function_test.cc +++ /dev/null
@@ -1,222 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 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: keir@google.com (Keir Mierle) -// -// Based on the tests in numeric_diff_cost_function.cc. -// -// TODO(keir): See about code duplication. - -#include "ceres/runtime_numeric_diff_cost_function.h" - -#include <algorithm> -#include <cmath> -#include <string> -#include <vector> -#include "ceres/cost_function.h" -#include "ceres/internal/macros.h" -#include "ceres/internal/scoped_ptr.h" -#include "ceres/stringprintf.h" -#include "ceres/test_util.h" -#include "glog/logging.h" -#include "gtest/gtest.h" - -namespace ceres { -namespace internal { - -const double kRelativeEps = 1e-6; - -// 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 TestCostFunction : public CostFunction { - public: - TestCostFunction() { - set_num_residuals(3); - mutable_parameter_block_sizes()->push_back(5); // x1. - mutable_parameter_block_sizes()->push_back(5); // x2. - } - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - (void) jacobians; // Ignored. - - residuals[0] = residuals[1] = residuals[2] = 0; - for (int i = 0; i < 5; ++i) { - residuals[0] += parameters[0][i] * parameters[1][i]; - residuals[2] += parameters[1][i] * parameters[1][i]; - } - residuals[1] = residuals[0] * residuals[0]; - return true; - } -}; - -TEST(NumericDiffCostFunction, EasyCase) { - // Try both central and forward difference. - TestCostFunction term; - scoped_ptr<CostFunction> cfs[2]; - cfs[0].reset( - CreateRuntimeNumericDiffCostFunction(&term, CENTRAL, kRelativeEps)); - - cfs[1].reset( - CreateRuntimeNumericDiffCostFunction(&term, FORWARD, kRelativeEps)); - - - for (int c = 0; c < 2; ++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); - } - } -} - -// 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 TranscendentalTestCostFunction : public CostFunction { - public: - TranscendentalTestCostFunction() { - set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(5); // x1. - mutable_parameter_block_sizes()->push_back(5); // x2. - } - virtual bool Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { - (void) jacobians; // Ignored. - - double x1x2 = 0; - for (int i = 0; i < 5; ++i) { - x1x2 += parameters[0][i] * parameters[1][i]; - } - residuals[0] = sin(x1x2); - residuals[1] = exp(-x1x2 / 10); - return true; - } -}; - -TEST(NumericDiffCostFunction, TransendentalOperationsInCostFunction) { - // Try both central and forward difference. - TranscendentalTestCostFunction term; - scoped_ptr<CostFunction> cfs[2]; - cfs[0].reset( - CreateRuntimeNumericDiffCostFunction(&term, CENTRAL, kRelativeEps)); - - cfs[1].reset( - CreateRuntimeNumericDiffCostFunction(&term, FORWARD, kRelativeEps)); - - for (int c = 0; c < 2; ++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); // NOLINT - ExpectClose( x1[i] * cos(x1x2), dydx2[5 * 0 + i], kEps); // NOLINT - ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], kEps); - ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], kEps); - } - } - } -} - -} // namespace internal -} // namespace ceres