1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: moll.markus (at) arcor.de (Markus Moll) 30 31 #include "ceres/polynomial_solver.h" 32 33 #include <limits> 34 #include <cmath> 35 #include <cstddef> 36 #include <algorithm> 37 #include "gtest/gtest.h" 38 #include "ceres/test_util.h" 39 40 namespace ceres { 41 namespace internal { 42 namespace { 43 44 // For IEEE-754 doubles, machine precision is about 2e-16. 45 const double kEpsilon = 1e-13; 46 const double kEpsilonLoose = 1e-9; 47 48 // Return the constant polynomial p(x) = 1.23. 49 Vector ConstantPolynomial(double value) { 50 Vector poly(1); 51 poly(0) = value; 52 return poly; 53 } 54 55 // Return the polynomial p(x) = poly(x) * (x - root). 56 Vector AddRealRoot(const Vector& poly, double root) { 57 Vector poly2(poly.size() + 1); 58 poly2.setZero(); 59 poly2.head(poly.size()) += poly; 60 poly2.tail(poly.size()) -= root * poly; 61 return poly2; 62 } 63 64 // Return the polynomial 65 // p(x) = poly(x) * (x - real - imag*i) * (x - real + imag*i). 66 Vector AddComplexRootPair(const Vector& poly, double real, double imag) { 67 Vector poly2(poly.size() + 2); 68 poly2.setZero(); 69 // Multiply poly by x^2 - 2real + abs(real,imag)^2 70 poly2.head(poly.size()) += poly; 71 poly2.segment(1, poly.size()) -= 2 * real * poly; 72 poly2.tail(poly.size()) += (real*real + imag*imag) * poly; 73 return poly2; 74 } 75 76 // Sort the entries in a vector. 77 // Needed because the roots are not returned in sorted order. 78 Vector SortVector(const Vector& in) { 79 Vector out(in); 80 std::sort(out.data(), out.data() + out.size()); 81 return out; 82 } 83 84 // Run a test with the polynomial defined by the N real roots in roots_real. 85 // If use_real is false, NULL is passed as the real argument to 86 // FindPolynomialRoots. If use_imaginary is false, NULL is passed as the 87 // imaginary argument to FindPolynomialRoots. 88 template<int N> 89 void RunPolynomialTestRealRoots(const double (&real_roots)[N], 90 bool use_real, 91 bool use_imaginary, 92 double epsilon) { 93 Vector real; 94 Vector imaginary; 95 Vector poly = ConstantPolynomial(1.23); 96 for (int i = 0; i < N; ++i) { 97 poly = AddRealRoot(poly, real_roots[i]); 98 } 99 Vector* const real_ptr = use_real ? &real : NULL; 100 Vector* const imaginary_ptr = use_imaginary ? &imaginary : NULL; 101 bool success = FindPolynomialRoots(poly, real_ptr, imaginary_ptr); 102 103 EXPECT_EQ(success, true); 104 if (use_real) { 105 EXPECT_EQ(real.size(), N); 106 real = SortVector(real); 107 ExpectArraysClose(N, real.data(), real_roots, epsilon); 108 } 109 if (use_imaginary) { 110 EXPECT_EQ(imaginary.size(), N); 111 const Vector zeros = Vector::Zero(N); 112 ExpectArraysClose(N, imaginary.data(), zeros.data(), epsilon); 113 } 114 } 115 } // namespace 116 117 TEST(PolynomialSolver, InvalidPolynomialOfZeroLengthIsRejected) { 118 // Vector poly(0) is an ambiguous constructor call, so 119 // use the constructor with explicit column count. 120 Vector poly(0, 1); 121 Vector real; 122 Vector imag; 123 bool success = FindPolynomialRoots(poly, &real, &imag); 124 125 EXPECT_EQ(success, false); 126 } 127 128 TEST(PolynomialSolver, ConstantPolynomialReturnsNoRoots) { 129 Vector poly = ConstantPolynomial(1.23); 130 Vector real; 131 Vector imag; 132 bool success = FindPolynomialRoots(poly, &real, &imag); 133 134 EXPECT_EQ(success, true); 135 EXPECT_EQ(real.size(), 0); 136 EXPECT_EQ(imag.size(), 0); 137 } 138 139 TEST(PolynomialSolver, LinearPolynomialWithPositiveRootWorks) { 140 const double roots[1] = { 42.42 }; 141 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 142 } 143 144 TEST(PolynomialSolver, LinearPolynomialWithNegativeRootWorks) { 145 const double roots[1] = { -42.42 }; 146 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 147 } 148 149 TEST(PolynomialSolver, QuadraticPolynomialWithPositiveRootsWorks) { 150 const double roots[2] = { 1.0, 42.42 }; 151 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 152 } 153 154 TEST(PolynomialSolver, QuadraticPolynomialWithOneNegativeRootWorks) { 155 const double roots[2] = { -42.42, 1.0 }; 156 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 157 } 158 159 TEST(PolynomialSolver, QuadraticPolynomialWithTwoNegativeRootsWorks) { 160 const double roots[2] = { -42.42, -1.0 }; 161 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 162 } 163 164 TEST(PolynomialSolver, QuadraticPolynomialWithCloseRootsWorks) { 165 const double roots[2] = { 42.42, 42.43 }; 166 RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose); 167 } 168 169 TEST(PolynomialSolver, QuadraticPolynomialWithComplexRootsWorks) { 170 Vector real; 171 Vector imag; 172 173 Vector poly = ConstantPolynomial(1.23); 174 poly = AddComplexRootPair(poly, 42.42, 4.2); 175 bool success = FindPolynomialRoots(poly, &real, &imag); 176 177 EXPECT_EQ(success, true); 178 EXPECT_EQ(real.size(), 2); 179 EXPECT_EQ(imag.size(), 2); 180 ExpectClose(real(0), 42.42, kEpsilon); 181 ExpectClose(real(1), 42.42, kEpsilon); 182 ExpectClose(std::abs(imag(0)), 4.2, kEpsilon); 183 ExpectClose(std::abs(imag(1)), 4.2, kEpsilon); 184 ExpectClose(std::abs(imag(0) + imag(1)), 0.0, kEpsilon); 185 } 186 187 TEST(PolynomialSolver, QuarticPolynomialWorks) { 188 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 189 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 190 } 191 192 TEST(PolynomialSolver, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) { 193 const double roots[4] = { 1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5 }; 194 RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); 195 } 196 197 TEST(PolynomialSolver, QuarticPolynomialWithTwoZeroRootsWorks) { 198 const double roots[4] = { -42.42, 0.0, 0.0, 42.42 }; 199 RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); 200 } 201 202 TEST(PolynomialSolver, QuarticMonomialWorks) { 203 const double roots[4] = { 0.0, 0.0, 0.0, 0.0 }; 204 RunPolynomialTestRealRoots(roots, true, true, kEpsilon); 205 } 206 207 TEST(PolynomialSolver, NullPointerAsImaginaryPartWorks) { 208 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 209 RunPolynomialTestRealRoots(roots, true, false, kEpsilon); 210 } 211 212 TEST(PolynomialSolver, NullPointerAsRealPartWorks) { 213 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 214 RunPolynomialTestRealRoots(roots, false, true, kEpsilon); 215 } 216 217 TEST(PolynomialSolver, BothOutputArgumentsNullWorks) { 218 const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; 219 RunPolynomialTestRealRoots(roots, false, false, kEpsilon); 220 } 221 222 } // namespace internal 223 } // namespace ceres 224