internal/ceres/polynomial_test.cc

   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 <limits>
  35 #include <cmath>
  36 #include <cstddef>
  37 #include <algorithm>
  38 #include "gtest/gtest.h"
  39 #include "ceres/function_sample.h"
  40 #include "ceres/test_util.h"
  41
  42 namespace ceres {
  43 namespace internal {
  44
  45 using std::vector;
  46
  47 namespace {
  48
  49 // For IEEE-754 doubles, machine precision is about 2e-16.
  50 const double kEpsilon = 1e-13;
  51 const double kEpsilonLoose = 1e-9;
  52
  53 // Return the constant polynomial p(x) = 1.23.
  54 Vector ConstantPolynomial(double value) {
  55   Vector poly(1);
  56   poly(0) = value;
  57   return poly;
  58 }
  59
  60 // Return the polynomial p(x) = poly(x) * (x - root).
  61 Vector AddRealRoot(const Vector& poly, double root) {
  62   Vector poly2(poly.size() + 1);
  63   poly2.setZero();
  64   poly2.head(poly.size()) += poly;
  65   poly2.tail(poly.size()) -= root * poly;
  66   return poly2;
  67 }
  68
  69 // Return the polynomial
  70 // p(x) = poly(x) * (x - real - imag*i) * (x - real + imag*i).
  71 Vector AddComplexRootPair(const Vector& poly, double real, double imag) {
  72   Vector poly2(poly.size() + 2);
  73   poly2.setZero();
  74   // Multiply poly by x^2 - 2real + abs(real,imag)^2
  75   poly2.head(poly.size()) += poly;
  76   poly2.segment(1, poly.size()) -= 2 * real * poly;
  77   poly2.tail(poly.size()) += (real*real + imag*imag) * poly;
  78   return poly2;
  79 }
  80
  81 // Sort the entries in a vector.
  82 // Needed because the roots are not returned in sorted order.
  83 Vector SortVector(const Vector& in) {
  84   Vector out(in);
  85   std::sort(out.data(), out.data() + out.size());
  86   return out;
  87 }
  88
  89 // Run a test with the polynomial defined by the N real roots in roots_real.
  90 // If use_real is false, NULL is passed as the real argument to
  91 // FindPolynomialRoots. If use_imaginary is false, NULL is passed as the
  92 // imaginary argument to FindPolynomialRoots.
  93 template<int N>
  94 void RunPolynomialTestRealRoots(const double (&real_roots)[N],
  95                                 bool use_real,
  96                                 bool use_imaginary,
  97                                 double epsilon) {
  98   Vector real;
  99   Vector imaginary;
 100   Vector poly = ConstantPolynomial(1.23);
 101   for (int i = 0; i < N; ++i) {
 102     poly = AddRealRoot(poly, real_roots[i]);
 103   }
 104   Vector* const real_ptr = use_real ? &real : NULL;
 105   Vector* const imaginary_ptr = use_imaginary ? &imaginary : NULL;
 106   bool success = FindPolynomialRoots(poly, real_ptr, imaginary_ptr);
 107
 108   EXPECT_EQ(success, true);
 109   if (use_real) {
 110     EXPECT_EQ(real.size(), N);
 111     real = SortVector(real);
 112     ExpectArraysClose(N, real.data(), real_roots, epsilon);
 113   }
 114   if (use_imaginary) {
 115     EXPECT_EQ(imaginary.size(), N);
 116     const Vector zeros = Vector::Zero(N);
 117     ExpectArraysClose(N, imaginary.data(), zeros.data(), epsilon);
 118   }
 119 }
 120 }  // namespace
 121
 122 TEST(Polynomial, InvalidPolynomialOfZeroLengthIsRejected) {
 123   // Vector poly(0) is an ambiguous constructor call, so
 124   // use the constructor with explicit column count.
 125   Vector poly(0, 1);
 126   Vector real;
 127   Vector imag;
 128   bool success = FindPolynomialRoots(poly, &real, &imag);
 129
 130   EXPECT_EQ(success, false);
 131 }
 132
 133 TEST(Polynomial, ConstantPolynomialReturnsNoRoots) {
 134   Vector poly = ConstantPolynomial(1.23);
 135   Vector real;
 136   Vector imag;
 137   bool success = FindPolynomialRoots(poly, &real, &imag);
 138
 139   EXPECT_EQ(success, true);
 140   EXPECT_EQ(real.size(), 0);
 141   EXPECT_EQ(imag.size(), 0);
 142 }
 143
 144 TEST(Polynomial, LinearPolynomialWithPositiveRootWorks) {
 145   const double roots[1] = { 42.42 };
 146   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 147 }
 148
 149 TEST(Polynomial, LinearPolynomialWithNegativeRootWorks) {
 150   const double roots[1] = { -42.42 };
 151   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 152 }
 153
 154 TEST(Polynomial, QuadraticPolynomialWithPositiveRootsWorks) {
 155   const double roots[2] = { 1.0, 42.42 };
 156   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 157 }
 158
 159 TEST(Polynomial, QuadraticPolynomialWithOneNegativeRootWorks) {
 160   const double roots[2] = { -42.42, 1.0 };
 161   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 162 }
 163
 164 TEST(Polynomial, QuadraticPolynomialWithTwoNegativeRootsWorks) {
 165   const double roots[2] = { -42.42, -1.0 };
 166   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 167 }
 168
 169 TEST(Polynomial, QuadraticPolynomialWithCloseRootsWorks) {
 170   const double roots[2] = { 42.42, 42.43 };
 171   RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose);
 172 }
 173
 174 TEST(Polynomial, QuadraticPolynomialWithComplexRootsWorks) {
 175   Vector real;
 176   Vector imag;
 177
 178   Vector poly = ConstantPolynomial(1.23);
 179   poly = AddComplexRootPair(poly, 42.42, 4.2);
 180   bool success = FindPolynomialRoots(poly, &real, &imag);
 181
 182   EXPECT_EQ(success, true);
 183   EXPECT_EQ(real.size(), 2);
 184   EXPECT_EQ(imag.size(), 2);
 185   ExpectClose(real(0), 42.42, kEpsilon);
 186   ExpectClose(real(1), 42.42, kEpsilon);
 187   ExpectClose(std::abs(imag(0)), 4.2, kEpsilon);
 188   ExpectClose(std::abs(imag(1)), 4.2, kEpsilon);
 189   ExpectClose(std::abs(imag(0) + imag(1)), 0.0, kEpsilon);
 190 }
 191
 192 TEST(Polynomial, QuarticPolynomialWorks) {
 193   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
 194   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 195 }
 196
 197 TEST(Polynomial, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) {
 198   const double roots[4] = { 1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5 };
 199   RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose);
 200 }
 201
 202 TEST(Polynomial, QuarticPolynomialWithTwoZeroRootsWorks) {
 203   const double roots[4] = { -42.42, 0.0, 0.0, 42.42 };
 204   RunPolynomialTestRealRoots(roots, true, true, 2 * kEpsilonLoose);
 205 }
 206
 207 TEST(Polynomial, QuarticMonomialWorks) {
 208   const double roots[4] = { 0.0, 0.0, 0.0, 0.0 };
 209   RunPolynomialTestRealRoots(roots, true, true, kEpsilon);
 210 }
 211
 212 TEST(Polynomial, NullPointerAsImaginaryPartWorks) {
 213   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
 214   RunPolynomialTestRealRoots(roots, true, false, kEpsilon);
 215 }
 216
 217 TEST(Polynomial, NullPointerAsRealPartWorks) {
 218   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
 219   RunPolynomialTestRealRoots(roots, false, true, kEpsilon);
 220 }
 221
 222 TEST(Polynomial, BothOutputArgumentsNullWorks) {
 223   const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 };
 224   RunPolynomialTestRealRoots(roots, false, false, kEpsilon);
 225 }
 226
 227 TEST(Polynomial, DifferentiateConstantPolynomial) {
 228   // p(x) = 1;
 229   Vector polynomial(1);
 230   polynomial(0) = 1.0;
 231   const Vector derivative = DifferentiatePolynomial(polynomial);
 232   EXPECT_EQ(derivative.rows(), 1);
 233   EXPECT_EQ(derivative(0), 0);
 234 }
 235
 236 TEST(Polynomial, DifferentiateQuadraticPolynomial) {
 237   // p(x) = x^2 + 2x + 3;
 238   Vector polynomial(3);
 239   polynomial(0) = 1.0;
 240   polynomial(1) = 2.0;
 241   polynomial(2) = 3.0;
 242
 243   const Vector derivative = DifferentiatePolynomial(polynomial);
 244   EXPECT_EQ(derivative.rows(), 2);
 245   EXPECT_EQ(derivative(0), 2.0);
 246   EXPECT_EQ(derivative(1), 2.0);
 247 }
 248
 249 TEST(Polynomial, MinimizeConstantPolynomial) {
 250   // p(x) = 1;
 251   Vector polynomial(1);
 252   polynomial(0) = 1.0;
 253
 254   double optimal_x = 0.0;
 255   double optimal_value = 0.0;
 256   double min_x = 0.0;
 257   double max_x = 1.0;
 258   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
 259
 260   EXPECT_EQ(optimal_value, 1.0);
 261   EXPECT_LE(optimal_x, max_x);
 262   EXPECT_GE(optimal_x, min_x);
 263 }
 264
 265 TEST(Polynomial, MinimizeLinearPolynomial) {
 266   // p(x) = x - 2
 267   Vector polynomial(2);
 268
 269   polynomial(0) = 1.0;
 270   polynomial(1) = 2.0;
 271
 272   double optimal_x = 0.0;
 273   double optimal_value = 0.0;
 274   double min_x = 0.0;
 275   double max_x = 1.0;
 276   MinimizePolynomial(polynomial, min_x, max_x, &optimal_x, &optimal_value);
 277
 278   EXPECT_EQ(optimal_x, 0.0);
 279   EXPECT_EQ(optimal_value, 2.0);
 280 }
 281
 282
 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
 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
 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
 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
 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
 405
 406 TEST(Polynomial, CubicInterpolatingPolynomialFromValues) {
 407   // p(x) = x^3 + 2x^2 + 3x + 2
 408   Vector true_polynomial(4);
 409   true_polynomial << 1.0, 2.0, 3.0, 2.0;
 410
 411   vector<FunctionSample> samples;
 412   {
 413     FunctionSample sample;
 414     sample.x = 1.0;
 415     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 416     sample.value_is_valid = true;
 417     samples.push_back(sample);
 418   }
 419
 420   {
 421     FunctionSample sample;
 422     sample.x = -3.0;
 423     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 424     sample.value_is_valid = true;
 425     samples.push_back(sample);
 426   }
 427
 428   {
 429     FunctionSample sample;
 430     sample.x = 2.0;
 431     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 432     sample.value_is_valid = true;
 433     samples.push_back(sample);
 434   }
 435
 436   {
 437     FunctionSample sample;
 438     sample.x = 0.0;
 439     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 440     sample.value_is_valid = true;
 441     samples.push_back(sample);
 442   }
 443
 444   const Vector polynomial = FindInterpolatingPolynomial(samples);
 445   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
 446 }
 447
 448 TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndOneGradient) {
 449   // p(x) = x^3 + 2x^2 + 3x + 2
 450   Vector true_polynomial(4);
 451   true_polynomial << 1.0, 2.0, 3.0, 2.0;
 452   Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial);
 453
 454   vector<FunctionSample> samples;
 455   {
 456     FunctionSample sample;
 457     sample.x = 1.0;
 458     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 459     sample.value_is_valid = true;
 460     samples.push_back(sample);
 461   }
 462
 463   {
 464     FunctionSample sample;
 465     sample.x = -3.0;
 466     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 467     sample.value_is_valid = true;
 468     samples.push_back(sample);
 469   }
 470
 471   {
 472     FunctionSample sample;
 473     sample.x = 2.0;
 474     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 475     sample.value_is_valid = true;
 476     sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x);
 477     sample.gradient_is_valid = true;
 478     samples.push_back(sample);
 479   }
 480
 481   const Vector polynomial = FindInterpolatingPolynomial(samples);
 482   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
 483 }
 484
 485 TEST(Polynomial, CubicInterpolatingPolynomialFromValuesAndGradients) {
 486   // p(x) = x^3 + 2x^2 + 3x + 2
 487   Vector true_polynomial(4);
 488   true_polynomial << 1.0, 2.0, 3.0, 2.0;
 489   Vector true_gradient_polynomial = DifferentiatePolynomial(true_polynomial);
 490
 491   vector<FunctionSample> samples;
 492   {
 493     FunctionSample sample;
 494     sample.x = -3.0;
 495     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 496     sample.value_is_valid = true;
 497     sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x);
 498     sample.gradient_is_valid = true;
 499     samples.push_back(sample);
 500   }
 501
 502   {
 503     FunctionSample sample;
 504     sample.x = 2.0;
 505     sample.value = EvaluatePolynomial(true_polynomial, sample.x);
 506     sample.value_is_valid = true;
 507     sample.gradient = EvaluatePolynomial(true_gradient_polynomial, sample.x);
 508     sample.gradient_is_valid = true;
 509     samples.push_back(sample);
 510   }
 511
 512   const Vector polynomial = FindInterpolatingPolynomial(samples);
 513   EXPECT_NEAR((true_polynomial - polynomial).norm(), 0.0, 1e-14);
 514 }
 515
 516 }  // namespace internal
 517 }  // namespace ceres