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
41namespace ceres {
42namespace internal {
43namespace {
44
45// For IEEE-754 doubles, machine precision is about 2e-16.
46const double kEpsilon = 1e-13;
47const double kEpsilonLoose = 1e-9;
48
49// Return the constant polynomial p(x) = 1.23.
50Vector 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).
57Vector 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).
67Vector 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.
79Vector 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.
89template<int N>
90void 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
118TEST(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
129TEST(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
140TEST(Polynomial, LinearPolynomialWithPositiveRootWorks) {
141  const double roots[1] = { 42.42 };
142  RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
143}
144
145TEST(Polynomial, LinearPolynomialWithNegativeRootWorks) {
146  const double roots[1] = { -42.42 };
147  RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
148}
149
150TEST(Polynomial, QuadraticPolynomialWithPositiveRootsWorks) {
151  const double roots[2] = { 1.0, 42.42 };
152  RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
153}
154
155TEST(Polynomial, QuadraticPolynomialWithOneNegativeRootWorks) {
156  const double roots[2] = { -42.42, 1.0 };
157  RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
158}
159
160TEST(Polynomial, QuadraticPolynomialWithTwoNegativeRootsWorks) {
161  const double roots[2] = { -42.42, -1.0 };
162  RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
163}
164
165TEST(Polynomial, QuadraticPolynomialWithCloseRootsWorks) {
166  const double roots[2] = { 42.42, 42.43 };
167  RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose);
168}
169
170TEST(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
188TEST(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
193TEST(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
198TEST(Polynomial, QuarticPolynomialWithTwoZeroRootsWorks) {
199  const double roots[4] = { -42.42, 0.0, 0.0, 42.42 };
200  RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose);
201}
202
203TEST(Polynomial, QuarticMonomialWorks) {
204  const double roots[4] = { 0.0, 0.0, 0.0, 0.0 };
205  RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
206}
207
208TEST(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
213TEST(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
218TEST(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
223TEST(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
232TEST(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
245TEST(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
261TEST(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
279TEST(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
309TEST(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
325TEST(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
343TEST(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
371TEST(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
402TEST(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
444TEST(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
481TEST(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