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 //         sameeragarwal@google.com (Sameer Agarwal)
31 
32 #include "ceres/polynomial.h"
33 
34 #include <limits>
35 #include <cmath>
36 #include <cstddef>
37 #include <algorithm>
38 #include "gtest/gtest.h"
39 #include "ceres/test_util.h"
40 
41 namespace ceres {
42 namespace internal {
43 namespace {
44 
45 // For IEEE-754 doubles, machine precision is about 2e-16.
46 const double kEpsilon = 1e-13;
47 const double kEpsilonLoose = 1e-9;
48 
49 // Return the constant polynomial p(x) = 1.23.
ConstantPolynomial(double value)50 Vector ConstantPolynomial(double value) {
51   Vector poly(1);
52   poly(0) = value;
53   return poly;
54 }
55 
56 // Return the polynomial p(x) = poly(x) * (x - root).
AddRealRoot(const Vector & poly,double root)57 Vector AddRealRoot(const Vector& poly, double root) {
58   Vector poly2(poly.size() + 1);
59   poly2.setZero();
60   poly2.head(poly.size()) += poly;
61   poly2.tail(poly.size()) -= root * poly;
62   return poly2;
63 }
64 
65 // Return the polynomial
66 // p(x) = poly(x) * (x - real - imag*i) * (x - real + imag*i).
AddComplexRootPair(const Vector & poly,double real,double imag)67 Vector AddComplexRootPair(const Vector& poly, double real, double imag) {
68   Vector poly2(poly.size() + 2);
69   poly2.setZero();
70   // Multiply poly by x^2 - 2real + abs(real,imag)^2
71   poly2.head(poly.size()) += poly;
72   poly2.segment(1, poly.size()) -= 2 * real * poly;
73   poly2.tail(poly.size()) += (real*real + imag*imag) * poly;
74   return poly2;
75 }
76 
77 // Sort the entries in a vector.
78 // Needed because the roots are not returned in sorted order.
SortVector(const Vector & in)79 Vector SortVector(const Vector& in) {
80   Vector out(in);
81   std::sort(out.data(), out.data() + out.size());
82   return out;
83 }
84 
85 // Run a test with the polynomial defined by the N real roots in roots_real.
86 // If use_real is false, NULL is passed as the real argument to
87 // FindPolynomialRoots. If use_imaginary is false, NULL is passed as the
88 // imaginary argument to FindPolynomialRoots.
89 template<int N>
RunPolynomialTestRealRoots(const double (& real_roots)[N],bool use_real,bool use_imaginary,double epsilon)90 void RunPolynomialTestRealRoots(const double (&real_roots)[N],
91                                 bool use_real,
92                                 bool use_imaginary,
93                                 double epsilon) {
94   Vector real;
95   Vector imaginary;
96   Vector poly = ConstantPolynomial(1.23);
97   for (int i = 0; i < N; ++i) {
98     poly = AddRealRoot(poly, real_roots[i]);
99   }
100   Vector* const real_ptr = use_real ? &real : NULL;
101   Vector* const imaginary_ptr = use_imaginary ? &imaginary : NULL;
102   bool success = FindPolynomialRoots(poly, real_ptr, imaginary_ptr);
103 
104   EXPECT_EQ(success, true);
105   if (use_real) {
106     EXPECT_EQ(real.size(), N);
107     real = SortVector(real);
108     ExpectArraysClose(N, real.data(), real_roots, epsilon);
109   }
110   if (use_imaginary) {
111     EXPECT_EQ(imaginary.size(), N);
112     const Vector zeros = Vector::Zero(N);
113     ExpectArraysClose(N, imaginary.data(), zeros.data(), epsilon);
114   }
115 }
116 }  // namespace
117 
TEST(Polynomial,InvalidPolynomialOfZeroLengthIsRejected)118 TEST(Polynomial, InvalidPolynomialOfZeroLengthIsRejected) {
119   // Vector poly(0) is an ambiguous constructor call, so
120   // use the constructor with explicit column count.
121   Vector poly(0, 1);
122   Vector real;
123   Vector imag;
124   bool success = FindPolynomialRoots(poly, &real, &imag);
125 
126   EXPECT_EQ(success, false);
127 }
128 
TEST(Polynomial,ConstantPolynomialReturnsNoRoots)129 TEST(Polynomial, ConstantPolynomialReturnsNoRoots) {
130   Vector poly = ConstantPolynomial(1.23);
131   Vector real;
132   Vector imag;
133   bool success = FindPolynomialRoots(poly, &real, &imag);
134 
135   EXPECT_EQ(success, true);
136   EXPECT_EQ(real.size(), 0);
137   EXPECT_EQ(imag.size(), 0);
138 }
139 
TEST(Polynomial,LinearPolynomialWithPositiveRootWorks)140 TEST(Polynomial, LinearPolynomialWithPositiveRootWorks) {
141   const double roots[1] = { 42.42 };
142   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
143 }
144 
TEST(Polynomial,LinearPolynomialWithNegativeRootWorks)145 TEST(Polynomial, LinearPolynomialWithNegativeRootWorks) {
146   const double roots[1] = { -42.42 };
147   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
148 }
149 
TEST(Polynomial,QuadraticPolynomialWithPositiveRootsWorks)150 TEST(Polynomial, QuadraticPolynomialWithPositiveRootsWorks) {
151   const double roots[2] = { 1.0, 42.42 };
152   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
153 }
154 
TEST(Polynomial,QuadraticPolynomialWithOneNegativeRootWorks)155 TEST(Polynomial, QuadraticPolynomialWithOneNegativeRootWorks) {
156   const double roots[2] = { -42.42, 1.0 };
157   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
158 }
159 
TEST(Polynomial,QuadraticPolynomialWithTwoNegativeRootsWorks)160 TEST(Polynomial, QuadraticPolynomialWithTwoNegativeRootsWorks) {
161   const double roots[2] = { -42.42, -1.0 };
162   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
163 }
164 
TEST(Polynomial,QuadraticPolynomialWithCloseRootsWorks)165 TEST(Polynomial, QuadraticPolynomialWithCloseRootsWorks) {
166   const double roots[2] = { 42.42, 42.43 };
167   RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose);
168 }
169 
TEST(Polynomial,QuadraticPolynomialWithComplexRootsWorks)170 TEST(Polynomial, QuadraticPolynomialWithComplexRootsWorks) {
171   Vector real;
172   Vector imag;
173 
174   Vector poly = ConstantPolynomial(1.23);
175   poly = AddComplexRootPair(poly, 42.42, 4.2);
176   bool success = FindPolynomialRoots(poly, &real, &imag);
177 
178   EXPECT_EQ(success, true);
179   EXPECT_EQ(real.size(), 2);
180   EXPECT_EQ(imag.size(), 2);
181   ExpectClose(real(0), 42.42, kEpsilon);
182   ExpectClose(real(1), 42.42, kEpsilon);
183   ExpectClose(std::abs(imag(0)), 4.2, kEpsilon);
184   ExpectClose(std::abs(imag(1)), 4.2, kEpsilon);
185   ExpectClose(std::abs(imag(0) + imag(1)), 0.0, kEpsilon);
186 }
187 
TEST(Polynomial,QuarticPolynomialWorks)188 TEST(Polynomial, QuarticPolynomialWorks) {
189   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
190   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
191 }
192 
TEST(Polynomial,QuarticPolynomialWithTwoClustersOfCloseRootsWorks)193 TEST(Polynomial, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) {
194   const double roots[4] = { 1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5 };
195   RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose);
196 }
197 
TEST(Polynomial,QuarticPolynomialWithTwoZeroRootsWorks)198 TEST(Polynomial, QuarticPolynomialWithTwoZeroRootsWorks) {
199   const double roots[4] = { -42.42, 0.0, 0.0, 42.42 };
200   RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose);
201 }
202 
TEST(Polynomial,QuarticMonomialWorks)203 TEST(Polynomial, QuarticMonomialWorks) {
204   const double roots[4] = { 0.0, 0.0, 0.0, 0.0 };
205   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
206 }
207 
TEST(Polynomial,NullPointerAsImaginaryPartWorks)208 TEST(Polynomial, NullPointerAsImaginaryPartWorks) {
209   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
210   RunPolynomialTestRealRoots(roots, true, false, kEpsilon);
211 }
212 
TEST(Polynomial,NullPointerAsRealPartWorks)213 TEST(Polynomial, NullPointerAsRealPartWorks) {
214   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
215   RunPolynomialTestRealRoots(roots, false, true, kEpsilon);
216 }
217 
TEST(Polynomial,BothOutputArgumentsNullWorks)218 TEST(Polynomial, BothOutputArgumentsNullWorks) {
219   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
220   RunPolynomialTestRealRoots(roots, false, false, kEpsilon);
221 }
222 
TEST(Polynomial,DifferentiateConstantPolynomial)223 TEST(Polynomial, DifferentiateConstantPolynomial) {
224   // p(x) = 1;
225   Vector polynomial(1);
226   polynomial(0) = 1.0;
227   const Vector derivative = DifferentiatePolynomial(polynomial);
228   EXPECT_EQ(derivative.rows(), 1);
229   EXPECT_EQ(derivative(0), 0);
230 }
231 
TEST(Polynomial,DifferentiateQuadraticPolynomial)232 TEST(Polynomial, DifferentiateQuadraticPolynomial) {
233   // p(x) = x^2 + 2x + 3;
234   Vector polynomial(3);
235   polynomial(0) = 1.0;
236   polynomial(1) = 2.0;
237   polynomial(2) = 3.0;
238 
239   const Vector derivative = DifferentiatePolynomial(polynomial);
240   EXPECT_EQ(derivative.rows(), 2);
241   EXPECT_EQ(derivative(0), 2.0);
242   EXPECT_EQ(derivative(1), 2.0);
243 }
244 
TEST(Polynomial,MinimizeConstantPolynomial)245 TEST(Polynomial, MinimizeConstantPolynomial) {
246   // p(x) = 1;
247   Vector polynomial(1);
248   polynomial(0) = 1.0;
249 
250   double optimal_x = 0.0;
251   double optimal_value = 0.0;
252   double min_x = 0.0;
253   double max_x = 1.0;
254   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
255 
256   EXPECT_EQ(optimal_value, 1.0);
257   EXPECT_LE(optimal_x, max_x);
258   EXPECT_GE(optimal_x, min_x);
259 }
260 
TEST(Polynomial,MinimizeLinearPolynomial)261 TEST(Polynomial, MinimizeLinearPolynomial) {
262   // p(x) = x - 2
263   Vector polynomial(2);
264 
265   polynomial(0) = 1.0;
266   polynomial(1) = 2.0;
267 
268   double optimal_x = 0.0;
269   double optimal_value = 0.0;
270   double min_x = 0.0;
271   double max_x = 1.0;
272   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
273 
274   EXPECT_EQ(optimal_x, 0.0);
275   EXPECT_EQ(optimal_value, 2.0);
276 }
277 
278 
TEST(Polynomial,MinimizeQuadraticPolynomial)279 TEST(Polynomial, MinimizeQuadraticPolynomial) {
280   // p(x) = x^2 - 3 x + 2
281   // min_x = 3/2
282   // min_value = -1/4;
283   Vector polynomial(3);
284   polynomial(0) = 1.0;
285   polynomial(1) = -3.0;
286   polynomial(2) = 2.0;
287 
288   double optimal_x = 0.0;
289   double optimal_value = 0.0;
290   double min_x = -2.0;
291   double max_x = 2.0;
292   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
293   EXPECT_EQ(optimal_x, 3.0/2.0);
294   EXPECT_EQ(optimal_value, -1.0/4.0);
295 
296   min_x = -2.0;
297   max_x = 1.0;
298   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
299   EXPECT_EQ(optimal_x, 1.0);
300   EXPECT_EQ(optimal_value, 0.0);
301 
302   min_x = 2.0;
303   max_x = 3.0;
304   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
305   EXPECT_EQ(optimal_x, 2.0);
306   EXPECT_EQ(optimal_value, 0.0);
307 }
308 
TEST(Polymomial,ConstantInterpolatingPolynomial)309 TEST(Polymomial, ConstantInterpolatingPolynomial) {
310   // p(x) = 1.0
311   Vector true_polynomial(1);
312   true_polynomial << 1.0;
313 
314   vector<FunctionSample> samples;
315   FunctionSample sample;
316   sample.x = 1.0;
317   sample.value = 1.0;
318   sample.value_is_valid = true;
319   samples.push_back(sample);
320 
321   const Vector polynomial = FindInterpolatingPolynomial(samples);
322   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15);
323 }
324 
TEST(Polynomial,LinearInterpolatingPolynomial)325 TEST(Polynomial, LinearInterpolatingPolynomial) {
326   // p(x) = 2x - 1
327   Vector true_polynomial(2);
328   true_polynomial << 2.0, -1.0;
329 
330   vector<FunctionSample> samples;
331   FunctionSample sample;
332   sample.x = 1.0;
333   sample.value = 1.0;
334   sample.value_is_valid = true;
335   sample.gradient = 2.0;
336   sample.gradient_is_valid = true;
337   samples.push_back(sample);
338 
339   const Vector polynomial = FindInterpolatingPolynomial(samples);
340   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15);
341 }
342 
TEST(Polynomial,QuadraticInterpolatingPolynomial)343 TEST(Polynomial, QuadraticInterpolatingPolynomial) {
344   // p(x) = 2x^2 + 3x + 2
345   Vector true_polynomial(3);
346   true_polynomial << 2.0, 3.0, 2.0;
347 
348   vector<FunctionSample> samples;
349   {
350     FunctionSample sample;
351     sample.x = 1.0;
352     sample.value = 7.0;
353     sample.value_is_valid = true;
354     sample.gradient = 7.0;
355     sample.gradient_is_valid = true;
356     samples.push_back(sample);
357   }
358 
359   {
360     FunctionSample sample;
361     sample.x = -3.0;
362     sample.value = 11.0;
363     sample.value_is_valid = true;
364     samples.push_back(sample);
365   }
366 
367   Vector polynomial = FindInterpolatingPolynomial(samples);
368   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-15);
369 }
370 
TEST(Polynomial,DeficientCubicInterpolatingPolynomial)371 TEST(Polynomial, DeficientCubicInterpolatingPolynomial) {
372   // p(x) = 2x^2 + 3x + 2
373   Vector true_polynomial(4);
374   true_polynomial << 0.0, 2.0, 3.0, 2.0;
375 
376   vector<FunctionSample> samples;
377   {
378     FunctionSample sample;
379     sample.x = 1.0;
380     sample.value = 7.0;
381     sample.value_is_valid = true;
382     sample.gradient = 7.0;
383     sample.gradient_is_valid = true;
384     samples.push_back(sample);
385   }
386 
387   {
388     FunctionSample sample;
389     sample.x = -3.0;
390     sample.value = 11.0;
391     sample.value_is_valid = true;
392     sample.gradient = -9;
393     sample.gradient_is_valid = true;
394     samples.push_back(sample);
395   }
396 
397   const Vector polynomial = FindInterpolatingPolynomial(samples);
398   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
399 }
400 
401 
TEST(Polynomial,CubicInterpolatingPolynomialFromValues)402 TEST(Polynomial, CubicInterpolatingPolynomialFromValues) {
403   // p(x) = x^3 + 2x^2 + 3x + 2
404   Vector true_polynomial(4);
405   true_polynomial << 1.0, 2.0, 3.0, 2.0;
406 
407   vector<FunctionSample> samples;
408   {
409     FunctionSample sample;
410     sample.x = 1.0;
411     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
412     sample.value_is_valid = true;
413     samples.push_back(sample);
414   }
415 
416   {
417     FunctionSample sample;
418     sample.x = -3.0;
419     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
420     sample.value_is_valid = true;
421     samples.push_back(sample);
422   }
423 
424   {
425     FunctionSample sample;
426     sample.x = 2.0;
427     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
428     sample.value_is_valid = true;
429     samples.push_back(sample);
430   }
431 
432   {
433     FunctionSample sample;
434     sample.x = 0.0;
435     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
436     sample.value_is_valid = true;
437     samples.push_back(sample);
438   }
439 
440   const Vector polynomial = FindInterpolatingPolynomial(samples);
441   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
442 }
443 
TEST(Polynomial,CubicInterpolatingPolynomialFromValuesAndOneGradient)444 TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndOneGradient) {
445   // p(x) = x^3 + 2x^2 + 3x + 2
446   Vector true_polynomial(4);
447   true_polynomial << 1.0, 2.0, 3.0, 2.0;
448   Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial);
449 
450   vector<FunctionSample> samples;
451   {
452     FunctionSample sample;
453     sample.x = 1.0;
454     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
455     sample.value_is_valid = true;
456     samples.push_back(sample);
457   }
458 
459   {
460     FunctionSample sample;
461     sample.x = -3.0;
462     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
463     sample.value_is_valid = true;
464     samples.push_back(sample);
465   }
466 
467   {
468     FunctionSample sample;
469     sample.x = 2.0;
470     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
471     sample.value_is_valid = true;
472     sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x);
473     sample.gradient_is_valid = true;
474     samples.push_back(sample);
475   }
476 
477   const Vector polynomial = FindInterpolatingPolynomial(samples);
478   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
479 }
480 
TEST(Polynomial,CubicInterpolatingPolynomialFromValuesAndGradients)481 TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndGradients) {
482   // p(x) = x^3 + 2x^2 + 3x + 2
483   Vector true_polynomial(4);
484   true_polynomial << 1.0, 2.0, 3.0, 2.0;
485   Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial);
486 
487   vector<FunctionSample> samples;
488   {
489     FunctionSample sample;
490     sample.x = -3.0;
491     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
492     sample.value_is_valid = true;
493     sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x);
494     sample.gradient_is_valid = true;
495     samples.push_back(sample);
496   }
497 
498   {
499     FunctionSample sample;
500     sample.x = 2.0;
501     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
502     sample.value_is_valid = true;
503     sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x);
504     sample.gradient_is_valid = true;
505     samples.push_back(sample);
506   }
507 
508   const Vector polynomial = FindInterpolatingPolynomial(samples);
509   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
510 }
511 
512 }  // namespace internal
513 }  // namespace ceres
514