| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 2024 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: keir@google.com (Keir Mierle) |
| // tbennun@gmail.com (Tal Ben-Nun) |
| |
| #include "ceres/numeric_diff_cost_function.h" |
| |
| #include <algorithm> |
| #include <array> |
| #include <cmath> |
| #include <memory> |
| #include <random> |
| #include <string> |
| #include <vector> |
| |
| #include "ceres/array_utils.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::internal { |
| |
| TEST(NumericDiffCostFunction, EasyCaseFunctorCentralDifferences) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<EasyFunctor, |
| CENTRAL, |
| 3, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>(new EasyFunctor); |
| EasyFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); |
| } |
| |
| TEST(NumericDiffCostFunction, EasyCaseFunctorForwardDifferences) { |
| auto cost_function = |
| std::make_unique<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, EasyCaseFunctorRidders) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<EasyFunctor, |
| RIDDERS, |
| 3, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>(new EasyFunctor); |
| EasyFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, RIDDERS); |
| } |
| |
| TEST(NumericDiffCostFunction, EasyCaseCostFunctionCentralDifferences) { |
| auto cost_function = |
| std::make_unique<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); |
| } |
| |
| TEST(NumericDiffCostFunction, EasyCaseCostFunctionForwardDifferences) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<EasyCostFunction, |
| FORWARD, |
| 3, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>(new EasyCostFunction, |
| TAKE_OWNERSHIP); |
| EasyFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); |
| } |
| |
| TEST(NumericDiffCostFunction, EasyCaseCostFunctionRidders) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<EasyCostFunction, |
| RIDDERS, |
| 3, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>(new EasyCostFunction, |
| TAKE_OWNERSHIP); |
| |
| EasyFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, RIDDERS); |
| } |
| |
| TEST(NumericDiffCostFunction, TranscendentalCaseFunctorCentralDifferences) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<TranscendentalFunctor, |
| CENTRAL, |
| 2, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>(new TranscendentalFunctor); |
| TranscendentalFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); |
| } |
| |
| TEST(NumericDiffCostFunction, TranscendentalCaseFunctorForwardDifferences) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<TranscendentalFunctor, |
| FORWARD, |
| 2, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>(new TranscendentalFunctor); |
| |
| TranscendentalFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, FORWARD); |
| } |
| |
| TEST(NumericDiffCostFunction, TranscendentalCaseFunctorRidders) { |
| NumericDiffOptions options; |
| |
| // Using a smaller initial step size to overcome oscillatory function |
| // behavior. |
| options.ridders_relative_initial_step_size = 1e-3; |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<TranscendentalFunctor, |
| RIDDERS, |
| 2, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>( |
| new TranscendentalFunctor, TAKE_OWNERSHIP, 2, options); |
| |
| TranscendentalFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, RIDDERS); |
| } |
| |
| TEST(NumericDiffCostFunction, |
| TranscendentalCaseCostFunctionCentralDifferences) { |
| auto cost_function = |
| std::make_unique<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) { |
| auto cost_function = |
| std::make_unique<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); |
| } |
| |
| TEST(NumericDiffCostFunction, TranscendentalCaseCostFunctionRidders) { |
| NumericDiffOptions options; |
| |
| // Using a smaller initial step size to overcome oscillatory function |
| // behavior. |
| options.ridders_relative_initial_step_size = 1e-3; |
| |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<TranscendentalCostFunction, |
| RIDDERS, |
| 2, // number of residuals |
| 5, // size of x1 |
| 5 // size of x2 |
| >>( |
| new TranscendentalCostFunction, TAKE_OWNERSHIP, 2, options); |
| TranscendentalFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, RIDDERS); |
| } |
| |
| template <int num_rows, int num_cols> |
| class SizeTestingCostFunction : public SizedCostFunction<num_rows, num_cols> { |
| public: |
| bool Evaluate(double const* const* parameters, |
| double* residuals, |
| double** jacobians) const final { |
| return true; |
| } |
| }; |
| |
| // As described in |
| // http://forum.kde.org/viewtopic.php?f=74&t=98536#p210774 |
| // Eigen3 has restrictions on the Row/Column major storage of vectors, |
| // depending on their dimensions. This test ensures that the correct |
| // templates are instantiated for various shapes of the Jacobian |
| // matrix. |
| TEST(NumericDiffCostFunction, EigenRowMajorColMajorTest) { |
| std::unique_ptr<CostFunction> cost_function = std::make_unique< |
| NumericDiffCostFunction<SizeTestingCostFunction<1, 1>, CENTRAL, 1, 1>>( |
| new SizeTestingCostFunction<1, 1>, ceres::TAKE_OWNERSHIP); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<SizeTestingCostFunction<2, 1>, CENTRAL, 2, 1>>( |
| new SizeTestingCostFunction<2, 1>, ceres::TAKE_OWNERSHIP); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<SizeTestingCostFunction<1, 2>, CENTRAL, 1, 2>>( |
| new SizeTestingCostFunction<1, 2>, ceres::TAKE_OWNERSHIP); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<SizeTestingCostFunction<2, 2>, CENTRAL, 2, 2>>( |
| new SizeTestingCostFunction<2, 2>, ceres::TAKE_OWNERSHIP); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, ceres::DYNAMIC, 1, 1>>( |
| new EasyFunctor, TAKE_OWNERSHIP, 1); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, ceres::DYNAMIC, 1, 1>>( |
| new EasyFunctor, TAKE_OWNERSHIP, 2); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, ceres::DYNAMIC, 1, 2>>( |
| new EasyFunctor, TAKE_OWNERSHIP, 1); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, ceres::DYNAMIC, 1, 2>>( |
| new EasyFunctor, TAKE_OWNERSHIP, 2); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, ceres::DYNAMIC, 2, 1>>( |
| new EasyFunctor, TAKE_OWNERSHIP, 1); |
| |
| cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, ceres::DYNAMIC, 2, 1>>( |
| new EasyFunctor, TAKE_OWNERSHIP, 2); |
| } |
| |
| TEST(NumericDiffCostFunction, |
| EasyCaseFunctorCentralDifferencesAndDynamicNumResiduals) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<EasyFunctor, |
| CENTRAL, |
| ceres::DYNAMIC, |
| 5, // size of x1 |
| 5 // size of x2 |
| >>( |
| new EasyFunctor, TAKE_OWNERSHIP, 3); |
| EasyFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL); |
| } |
| |
| TEST(NumericDiffCostFunction, ExponentialFunctorRidders) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<ExponentialFunctor, |
| RIDDERS, |
| 1, // number of residuals |
| 1 // size of x1 |
| >>(new ExponentialFunctor); |
| ExponentialFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function); |
| } |
| |
| TEST(NumericDiffCostFunction, ExponentialCostFunctionRidders) { |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<ExponentialCostFunction, |
| RIDDERS, |
| 1, // number of residuals |
| 1 // size of x1 |
| >>(new ExponentialCostFunction); |
| ExponentialFunctor functor; |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function); |
| } |
| |
| TEST(NumericDiffCostFunction, RandomizedFunctorRidders) { |
| std::mt19937 prng; |
| NumericDiffOptions options; |
| // Larger initial step size is chosen to produce robust results in the |
| // presence of random noise. |
| options.ridders_relative_initial_step_size = 10.0; |
| |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<RandomizedFunctor, |
| RIDDERS, |
| 1, // number of residuals |
| 1 // size of x1 |
| >>( |
| new RandomizedFunctor(kNoiseFactor, prng), |
| TAKE_OWNERSHIP, |
| 1, |
| options); |
| RandomizedFunctor functor(kNoiseFactor, prng); |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function); |
| } |
| |
| TEST(NumericDiffCostFunction, RandomizedCostFunctionRidders) { |
| std::mt19937 prng; |
| NumericDiffOptions options; |
| // Larger initial step size is chosen to produce robust results in the |
| // presence of random noise. |
| options.ridders_relative_initial_step_size = 10.0; |
| |
| auto cost_function = |
| std::make_unique<NumericDiffCostFunction<RandomizedCostFunction, |
| RIDDERS, |
| 1, // number of residuals |
| 1 // size of x1 |
| >>( |
| new RandomizedCostFunction(kNoiseFactor, prng), |
| TAKE_OWNERSHIP, |
| 1, |
| options); |
| |
| RandomizedFunctor functor(kNoiseFactor, prng); |
| functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function); |
| } |
| |
| struct OnlyFillsOneOutputFunctor { |
| bool operator()(const double* x, double* output) const { |
| output[0] = x[0]; |
| return true; |
| } |
| }; |
| |
| TEST(NumericDiffCostFunction, PartiallyFilledResidualShouldFailEvaluation) { |
| double parameter = 1.0; |
| double jacobian[2]; |
| double residuals[2]; |
| double* parameters[] = {¶meter}; |
| double* jacobians[] = {jacobian}; |
| |
| auto cost_function = std::make_unique< |
| NumericDiffCostFunction<OnlyFillsOneOutputFunctor, CENTRAL, 2, 1>>( |
| new OnlyFillsOneOutputFunctor); |
| InvalidateArray(2, jacobian); |
| InvalidateArray(2, residuals); |
| EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, jacobians)); |
| EXPECT_FALSE(IsArrayValid(2, residuals)); |
| InvalidateArray(2, residuals); |
| EXPECT_TRUE(cost_function->Evaluate(parameters, residuals, nullptr)); |
| // We are only testing residuals here, because the Jacobians are |
| // computed using finite differencing from the residuals, so unless |
| // we introduce a validation step after every evaluation of |
| // residuals inside NumericDiffCostFunction, there is no way of |
| // ensuring that the Jacobian array is invalid. |
| EXPECT_FALSE(IsArrayValid(2, residuals)); |
| } |
| |
| TEST(NumericDiffCostFunction, ParameterBlockConstant) { |
| constexpr int kNumResiduals = 3; |
| constexpr int kX1 = 5; |
| constexpr int kX2 = 5; |
| |
| auto cost_function = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, kNumResiduals, kX1, kX2>>( |
| new EasyFunctor); |
| |
| // Prepare the parameters and residuals. |
| std::array<double, kX1> x1{1e-64, 2.0, 3.0, 4.0, 5.0}; |
| std::array<double, kX2> x2{9.0, 9.0, 5.0, 5.0, 1.0}; |
| std::array<double*, 2> parameter_blocks{x1.data(), x2.data()}; |
| |
| std::vector<double> residuals(kNumResiduals, -100000); |
| |
| // Evaluate the full jacobian. |
| std::vector<std::vector<double>> jacobian_full_vect(2); |
| jacobian_full_vect[0].resize(kNumResiduals * kX1, -100000); |
| jacobian_full_vect[1].resize(kNumResiduals * kX2, -100000); |
| { |
| std::array<double*, 2> jacobian{jacobian_full_vect[0].data(), |
| jacobian_full_vect[1].data()}; |
| ASSERT_TRUE(cost_function->Evaluate( |
| parameter_blocks.data(), residuals.data(), jacobian.data())); |
| } |
| |
| // Evaluate and check jacobian when first parameter block is constant. |
| { |
| std::vector<double> jacobian_vect(kNumResiduals * kX2, -100000); |
| std::array<double*, 2> jacobian{nullptr, jacobian_vect.data()}; |
| |
| ASSERT_TRUE(cost_function->Evaluate( |
| parameter_blocks.data(), residuals.data(), jacobian.data())); |
| |
| for (int i = 0; i < kNumResiduals * kX2; ++i) { |
| EXPECT_DOUBLE_EQ(jacobian_full_vect[1][i], jacobian_vect[i]); |
| } |
| } |
| |
| // Evaluate and check jacobian when second parameter block is constant. |
| { |
| std::vector<double> jacobian_vect(kNumResiduals * kX1, -100000); |
| std::array<double*, 2> jacobian{jacobian_vect.data(), nullptr}; |
| |
| ASSERT_TRUE(cost_function->Evaluate( |
| parameter_blocks.data(), residuals.data(), jacobian.data())); |
| |
| for (int i = 0; i < kNumResiduals * kX1; ++i) { |
| EXPECT_DOUBLE_EQ(jacobian_full_vect[0][i], jacobian_vect[i]); |
| } |
| } |
| } |
| |
| struct MultiArgFunctor { |
| explicit MultiArgFunctor(int a, double c) {} |
| template <class T> |
| bool operator()(const T* params, T* residuals) const noexcept { |
| return false; |
| } |
| }; |
| |
| TEST(NumericDiffCostFunction, ArgumentForwarding) { |
| auto cost_function1 = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, 3, 5, 5>>(); |
| auto cost_function2 = |
| std::make_unique<NumericDiffCostFunction<MultiArgFunctor, CENTRAL, 1, 1>>( |
| 1, 2); |
| } |
| |
| TEST(NumericDiffCostFunction, UniquePtrCtor) { |
| auto cost_function1 = |
| std::make_unique<NumericDiffCostFunction<EasyFunctor, CENTRAL, 3, 5, 5>>( |
| std::make_unique<EasyFunctor>()); |
| auto cost_function2 = std::make_unique< |
| NumericDiffCostFunction<EasyFunctor, CENTRAL, 3, 5, 5>>(); |
| } |
| |
| } // namespace ceres::internal |
