| // Ceres Solver - A fast non-linear least squares minimizer |
| // Copyright 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: moll.markus@arcor.de (Markus Moll) |
| |
| #include "ceres/polynomial_solver.h" |
| |
| #include <limits> |
| #include <cmath> |
| #include <cstddef> |
| #include <algorithm> |
| #include "gtest/gtest.h" |
| #include "ceres/test_util.h" |
| |
| namespace ceres { |
| namespace internal { |
| 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, NULL is passed as the real argument to |
| // FindPolynomialRoots. If use_imaginary is false, NULL 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 : NULL; |
| Vector* const imaginary_ptr = use_imaginary ? &imaginary : NULL; |
| 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(PolynomialSolver, 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(PolynomialSolver, 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(PolynomialSolver, LinearPolynomialWithPositiveRootWorks) { |
| const double roots[1] = { 42.42 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, LinearPolynomialWithNegativeRootWorks) { |
| const double roots[1] = { -42.42 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, QuadraticPolynomialWithPositiveRootsWorks) { |
| const double roots[2] = { 1.0, 42.42 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, QuadraticPolynomialWithOneNegativeRootWorks) { |
| const double roots[2] = { -42.42, 1.0 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, QuadraticPolynomialWithTwoNegativeRootsWorks) { |
| const double roots[2] = { -42.42, -1.0 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, QuadraticPolynomialWithCloseRootsWorks) { |
| const double roots[2] = { 42.42, 42.43 }; |
| RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose); |
| } |
| |
| TEST(PolynomialSolver, 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(PolynomialSolver, QuarticPolynomialWorks) { |
| const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) { |
| const double roots[4] = { 1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); |
| } |
| |
| TEST(PolynomialSolver, QuarticPolynomialWithTwoZeroRootsWorks) { |
| const double roots[4] = { -42.42, 0.0, 0.0, 42.42 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); |
| } |
| |
| TEST(PolynomialSolver, QuarticMonomialWorks) { |
| const double roots[4] = { 0.0, 0.0, 0.0, 0.0 }; |
| RunPolynomialTestRealRoots(roots, true, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, NullPointerAsImaginaryPartWorks) { |
| const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; |
| RunPolynomialTestRealRoots(roots, true, false, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, NullPointerAsRealPartWorks) { |
| const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; |
| RunPolynomialTestRealRoots(roots, false, true, kEpsilon); |
| } |
| |
| TEST(PolynomialSolver, BothOutputArgumentsNullWorks) { |
| const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; |
| RunPolynomialTestRealRoots(roots, false, false, kEpsilon); |
| } |
| |
| } // namespace internal |
| } // namespace ceres |