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