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@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.
ConstantPolynomial(double value)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).
AddRealRoot(const Vector & poly,double 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).
AddComplexRootPair(const Vector & poly,double real,double imag)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.
SortVector(const Vector & in)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>
RunPolynomialTestRealRoots(const double (& real_roots)[N],bool use_real,bool use_imaginary,double epsilon)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
TEST(PolynomialSolver,InvalidPolynomialOfZeroLengthIsRejected)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
TEST(PolynomialSolver,ConstantPolynomialReturnsNoRoots)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
TEST(PolynomialSolver,LinearPolynomialWithPositiveRootWorks)139 TEST(PolynomialSolver, LinearPolynomialWithPositiveRootWorks) {
140 const double roots[1] = { 42.42 };
141 RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
142 }
143
TEST(PolynomialSolver,LinearPolynomialWithNegativeRootWorks)144 TEST(PolynomialSolver, LinearPolynomialWithNegativeRootWorks) {
145 const double roots[1] = { -42.42 };
146 RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
147 }
148
TEST(PolynomialSolver,QuadraticPolynomialWithPositiveRootsWorks)149 TEST(PolynomialSolver, QuadraticPolynomialWithPositiveRootsWorks) {
150 const double roots[2] = { 1.0, 42.42 };
151 RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
152 }
153
TEST(PolynomialSolver,QuadraticPolynomialWithOneNegativeRootWorks)154 TEST(PolynomialSolver, QuadraticPolynomialWithOneNegativeRootWorks) {
155 const double roots[2] = { -42.42, 1.0 };
156 RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
157 }
158
TEST(PolynomialSolver,QuadraticPolynomialWithTwoNegativeRootsWorks)159 TEST(PolynomialSolver, QuadraticPolynomialWithTwoNegativeRootsWorks) {
160 const double roots[2] = { -42.42, -1.0 };
161 RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
162 }
163
TEST(PolynomialSolver,QuadraticPolynomialWithCloseRootsWorks)164 TEST(PolynomialSolver, QuadraticPolynomialWithCloseRootsWorks) {
165 const double roots[2] = { 42.42, 42.43 };
166 RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose);
167 }
168
TEST(PolynomialSolver,QuadraticPolynomialWithComplexRootsWorks)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
TEST(PolynomialSolver,QuarticPolynomialWorks)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
TEST(PolynomialSolver,QuarticPolynomialWithTwoClustersOfCloseRootsWorks)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
TEST(PolynomialSolver,QuarticPolynomialWithTwoZeroRootsWorks)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
TEST(PolynomialSolver,QuarticMonomialWorks)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
TEST(PolynomialSolver,NullPointerAsImaginaryPartWorks)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
TEST(PolynomialSolver,NullPointerAsRealPartWorks)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
TEST(PolynomialSolver,BothOutputArgumentsNullWorks)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