| // 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: moll.markus@arcor.de (Markus Moll) |
| // sameeragarwal@google.com (Sameer Agarwal) |
| |
| #include "ceres/polynomial.h" |
| |
| #include <algorithm> |
| #include <cmath> |
| #include <cstddef> |
| #include <limits> |
| |
| #include "ceres/function_sample.h" |
| #include "ceres/test_util.h" |
| #include "gtest/gtest.h" |
| |
| namespace ceres::internal { |
| |
| using std::vector; |
| |
| namespace { |
| |
| // For IEEE-754 doubles, machine precision is about 2e-16. |
| const double kEpsilon = 1e-13; |
| const double kEpsilonLoose = 1e-9; |
| |
| // Return the constant polynomial p(x) = 1.23. |
| Vector ConstantPolynomial(double value) { |
| Vector poly(1); |
| poly(0) = value; |
| return poly; |
| } |
| |
| // Return the polynomial p(x) = poly(x) * (x - root). |
| Vector AddRealRoot(const Vector& poly, double root) { |
| Vector poly2(poly.size() + 1); |
| poly2.setZero(); |
| poly2.head(poly.size()) += poly; |
| poly2.tail(poly.size()) -= root * poly; |
| return poly2; |
| } |
| |
| // Return the polynomial |
| // p(x) = poly(x) * (x - real - imag*i) * (x - real + imag*i). |
| Vector AddComplexRootPair(const Vector& poly, double real, double imag) { |
| Vector poly2(poly.size() + 2); |
| poly2.setZero(); |
| // Multiply poly by x^2 - 2real + abs(real,imag)^2 |
| poly2.head(poly.size()) += poly; |
| poly2.segment(1, poly.size()) -= 2 * real * poly; |
| poly2.tail(poly.size()) += (real * real + imag * imag) * poly; |
| return poly2; |
| } |
| |
| // Sort the entries in a vector. |
| // Needed because the roots are not returned in sorted order. |
| Vector SortVector(const Vector& in) { |
| Vector out(in); |
| std::sort(out.data(), out.data() + out.size()); |
| return out; |
| } |
| |
| // Run a test with the polynomial defined by the N real roots in roots_real. |
| // If use_real is false, nullptr is passed as the real argument to |
| // FindPolynomialRoots. If use_imaginary is false, nullptr is passed as the |
| // imaginary argument to FindPolynomialRoots. |
| template <int N> |
| void RunPolynomialTestRealRoots(const double (&real_roots)[N], |
| bool use_real, |
| bool use_imaginary, |
| double epsilon) { |
| Vector real; |
| Vector imaginary; |
| Vector poly = ConstantPolynomial(1.23); |
| for (int i = 0; i < N; ++i) { |
| poly = AddRealRoot(poly, real_roots[i]); |
| } |
| Vector* const real_ptr = use_real ? &real : nullptr; |
| Vector* const imaginary_ptr = use_imaginary ? &imaginary : nullptr; |
| bool success = FindPolynomialRoots(poly, real_ptr, imaginary_ptr); |
| |
| EXPECT_EQ(success, true); |
| if (use_real) { |
| EXPECT_EQ(real.size(), N); |
| real = SortVector(real); |
| ExpectArraysClose(N, real.data(), real_roots, epsilon); |
| } |
| if (use_imaginary) { |
| EXPECT_EQ(imaginary.size(), N); |
| const Vector zeros = Vector::Zero(N); |
| ExpectArraysClose(N, imaginary.data(), zeros.data(), epsilon); |
| } |
| } |
| } // namespace |
| |
| TEST(Polynomial, InvalidPolynomialOfZeroLengthIsRejected) { |
| // Vector poly(0) is an ambiguous constructor call, so |
| // use the constructor with explicit column count. |
| Vector poly(0, 1); |
| Vector real; |
| Vector imag; |
| bool success = FindPolynomialRoots(poly, &real, &imag); |
| |
| EXPECT_EQ(success, false); |
| } |
| |
| TEST(Polynomial, ConstantPolynomialReturnsNoRoots) { |
| Vector poly = ConstantPolynomial(1.23); |
| Vector real; |
| Vector imag; |
| bool success = FindPolynomialRoots(poly, &real, &imag); |
| |
| EXPECT_EQ(success, true); |
| EXPECT_EQ(real.size(), 0); |
| EXPECT_EQ(imag.size(), 0); |
| } |
| |
| TEST(Polynomial, LinearPolynomialWithPositiveRootWorks) { |
| const double roots[1] = {42.42}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, LinearPolynomialWithNegativeRootWorks) { |
| const double roots[1] = {-42.42}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, QuadraticPolynomialWithPositiveRootsWorks) { |
| const double roots[2] = {1.0, 42.42}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, QuadraticPolynomialWithOneNegativeRootWorks) { |
| const double roots[2] = {-42.42, 1.0}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, QuadraticPolynomialWithTwoNegativeRootsWorks) { |
| const double roots[2] = {-42.42, -1.0}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, QuadraticPolynomialWithCloseRootsWorks) { |
| const double roots[2] = {42.42, 42.43}; |
| RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose); |
| } |
| |
| TEST(Polynomial, QuadraticPolynomialWithComplexRootsWorks) { |
| Vector real; |
| Vector imag; |
| |
| Vector poly = ConstantPolynomial(1.23); |
| poly = AddComplexRootPair(poly, 42.42, 4.2); |
| bool success = FindPolynomialRoots(poly, &real, &imag); |
| |
| EXPECT_EQ(success, true); |
| EXPECT_EQ(real.size(), 2); |
| EXPECT_EQ(imag.size(), 2); |
| ExpectClose(real(0), 42.42, kEpsilon); |
| ExpectClose(real(1), 42.42, kEpsilon); |
| ExpectClose(std::abs(imag(0)), 4.2, kEpsilon); |
| ExpectClose(std::abs(imag(1)), 4.2, kEpsilon); |
| ExpectClose(std::abs(imag(0) + imag(1)), 0.0, kEpsilon); |
| } |
| |
| TEST(Polynomial, QuarticPolynomialWorks) { |
| const double roots[4] = {1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) { |
| const double roots[4] = {1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); |
| } |
| |
| TEST(Polynomial, QuarticPolynomialWithTwoZeroRootsWorks) { |
| const double roots[4] = {-42.42, 0.0, 0.0, 42.42}; |
| RunPolynomialTestRealRoots(roots, true, true, 2 * kEpsilonLoose); |
| } |
| |
| TEST(Polynomial, QuarticMonomialWorks) { |
| const double roots[4] = {0.0, 0.0, 0.0, 0.0}; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, NullPointerAsImaginaryPartWorks) { |
| const double roots[4] = {1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5}; |
| RunPolynomialTestRealRoots(roots, true, false, kEpsilon); |
| } |
| |
| TEST(Polynomial, NullPointerAsRealPartWorks) { |
| const double roots[4] = {1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5}; |
| RunPolynomialTestRealRoots(roots, false, true, kEpsilon); |
| } |
| |
| TEST(Polynomial, BothOutputArgumentsNullWorks) { |
| const double roots[4] = {1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5}; |
| RunPolynomialTestRealRoots(roots, false, false, kEpsilon); |
| } |
| |
| TEST(Polynomial, DifferentiateConstantPolynomial) { |
| // p(x) = 1; |
| Vector polynomial(1); |
| polynomial(0) = 1.0; |
| const Vector derivative = DifferentiatePolynomial(polynomial); |
| EXPECT_EQ(derivative.rows(), 1); |
| EXPECT_EQ(derivative(0), 0); |
| } |
| |
| TEST(Polynomial, DifferentiateQuadraticPolynomial) { |
| // p(x) = x^2 + 2x + 3; |
| Vector polynomial(3); |
| polynomial(0) = 1.0; |
| polynomial(1) = 2.0; |
| polynomial(2) = 3.0; |
| |
| const Vector derivative = DifferentiatePolynomial(polynomial); |
| EXPECT_EQ(derivative.rows(), 2); |
| EXPECT_EQ(derivative(0), 2.0); |
| EXPECT_EQ(derivative(1), 2.0); |
| } |
| |
| TEST(Polynomial, MinimizeConstantPolynomial) { |
| // p(x) = 1; |
| Vector polynomial(1); |
| polynomial(0) = 1.0; |
| |
| double optimal_x = 0.0; |
| double optimal_value = 0.0; |
| double min_x = 0.0; |
| double max_x = 1.0; |
| MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); |
| |
| EXPECT_EQ(optimal_value, 1.0); |
| EXPECT_LE(optimal_x, max_x); |
| EXPECT_GE(optimal_x, min_x); |
| } |
| |
| TEST(Polynomial, MinimizeLinearPolynomial) { |
| // p(x) = x - 2 |
| Vector polynomial(2); |
| |
| polynomial(0) = 1.0; |
| polynomial(1) = 2.0; |
| |
| double optimal_x = 0.0; |
| double optimal_value = 0.0; |
| double min_x = 0.0; |
| double max_x = 1.0; |
| MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); |
| |
| EXPECT_EQ(optimal_x, 0.0); |
| EXPECT_EQ(optimal_value, 2.0); |
| } |
| |
| TEST(Polynomial, MinimizeQuadraticPolynomial) { |
| // p(x) = x^2 - 3 x + 2 |
| // min_x = 3/2 |
| // min_value = -1/4; |
| Vector polynomial(3); |
| polynomial(0) = 1.0; |
| polynomial(1) = -3.0; |
| polynomial(2) = 2.0; |
| |
| double optimal_x = 0.0; |
| double optimal_value = 0.0; |
| double min_x = -2.0; |
| double max_x = 2.0; |
| MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); |
| EXPECT_EQ(optimal_x, 3.0 / 2.0); |
| EXPECT_EQ(optimal_value, -1.0 / 4.0); |
| |
| min_x = -2.0; |
| max_x = 1.0; |
| MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); |
| EXPECT_EQ(optimal_x, 1.0); |
| EXPECT_EQ(optimal_value, 0.0); |
| |
| min_x = 2.0; |
| max_x = 3.0; |
| MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value); |
| EXPECT_EQ(optimal_x, 2.0); |
| EXPECT_EQ(optimal_value, 0.0); |
| } |
| |
| TEST(Polymomial, ConstantInterpolatingPolynomial) { |
| // p(x) = 1.0 |
| Vector true_polynomial(1); |
| true_polynomial << 1.0; |
| |
| vector<FunctionSample> samples; |
| FunctionSample sample; |
| sample.x = 1.0; |
| sample.value = 1.0; |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| |
| const Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15); |
| } |
| |
| TEST(Polynomial, LinearInterpolatingPolynomial) { |
| // p(x) = 2x - 1 |
| Vector true_polynomial(2); |
| true_polynomial << 2.0, -1.0; |
| |
| vector<FunctionSample> samples; |
| FunctionSample sample; |
| sample.x = 1.0; |
| sample.value = 1.0; |
| sample.value_is_valid = true; |
| sample.gradient = 2.0; |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| |
| const Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15); |
| } |
| |
| TEST(Polynomial, QuadraticInterpolatingPolynomial) { |
| // p(x) = 2x^2 + 3x + 2 |
| Vector true_polynomial(3); |
| true_polynomial << 2.0, 3.0, 2.0; |
| |
| vector<FunctionSample> samples; |
| { |
| FunctionSample sample; |
| sample.x = 1.0; |
| sample.value = 7.0; |
| sample.value_is_valid = true; |
| sample.gradient = 7.0; |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = -3.0; |
| sample.value = 11.0; |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15); |
| } |
| |
| TEST(Polynomial, DeficientCubicInterpolatingPolynomial) { |
| // p(x) = 2x^2 + 3x + 2 |
| Vector true_polynomial(4); |
| true_polynomial << 0.0, 2.0, 3.0, 2.0; |
| |
| vector<FunctionSample> samples; |
| { |
| FunctionSample sample; |
| sample.x = 1.0; |
| sample.value = 7.0; |
| sample.value_is_valid = true; |
| sample.gradient = 7.0; |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = -3.0; |
| sample.value = 11.0; |
| sample.value_is_valid = true; |
| sample.gradient = -9; |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| const Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); |
| } |
| |
| TEST(Polynomial, CubicInterpolatingPolynomialFromValues) { |
| // p(x) = x^3 + 2x^2 + 3x + 2 |
| Vector true_polynomial(4); |
| true_polynomial << 1.0, 2.0, 3.0, 2.0; |
| |
| vector<FunctionSample> samples; |
| { |
| FunctionSample sample; |
| sample.x = 1.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = -3.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = 2.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = 0.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| const Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); |
| } |
| |
| TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndOneGradient) { |
| // p(x) = x^3 + 2x^2 + 3x + 2 |
| Vector true_polynomial(4); |
| true_polynomial << 1.0, 2.0, 3.0, 2.0; |
| Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial); |
| |
| vector<FunctionSample> samples; |
| { |
| FunctionSample sample; |
| sample.x = 1.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = -3.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = 2.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x); |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| const Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); |
| } |
| |
| TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndGradients) { |
| // p(x) = x^3 + 2x^2 + 3x + 2 |
| Vector true_polynomial(4); |
| true_polynomial << 1.0, 2.0, 3.0, 2.0; |
| Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial); |
| |
| vector<FunctionSample> samples; |
| { |
| FunctionSample sample; |
| sample.x = -3.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x); |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| { |
| FunctionSample sample; |
| sample.x = 2.0; |
| sample.value = EvaluatePolynomial(true_polynomial, sample.x); |
| sample.value_is_valid = true; |
| sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x); |
| sample.gradient_is_valid = true; |
| samples.push_back(sample); |
| } |
| |
| const Vector polynomial = FindInterpolatingPolynomial(samples); |
| EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14); |
| } |
| |
| } // namespace ceres::internal |