internal/ceres/polynomial.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 <cmath>
  35 #include <cstddef>
  36 #include <vector>
  37
  38 #include "Eigen/Dense"
  39 #include "ceres/function_sample.h"
  40 #include "ceres/internal/port.h"
  41 #include "glog/logging.h"
  42
  43 namespace ceres {
  44 namespace internal {
  45
  46 using std::vector;
  47
  48 namespace {
  49
  50 // Balancing function as described by B. N. Parlett and C. Reinsch,
  51 // "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors".
  52 // In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304,
  53 // Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404
  54 void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) {
  55   CHECK_NOTNULL(companion_matrix_ptr);
  56   Matrix& companion_matrix = *companion_matrix_ptr;
  57   Matrix companion_matrix_offdiagonal = companion_matrix;
  58   companion_matrix_offdiagonal.diagonal().setZero();
  59
  60   const int degree = companion_matrix.rows();
  61
  62   // gamma <= 1 controls how much a change in the scaling has to
  63   // lower the 1-norm of the companion matrix to be accepted.
  64   //
  65   // gamma = 1 seems to lead to cycles (numerical issues?), so
  66   // we set it slightly lower.
  67   const double gamma = 0.9;
  68
  69   // Greedily scale row/column pairs until there is no change.
  70   bool scaling_has_changed;
  71   do {
  72     scaling_has_changed = false;
  73
  74     for (int i = 0; i < degree; ++i) {
  75       const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>();
  76       const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>();
  77
  78       // Decompose row_norm/col_norm into mantissa * 2^exponent,
  79       // where 0.5 <= mantissa < 1. Discard mantissa (return value
  80       // of frexp), as only the exponent is needed.
  81       int exponent = 0;
  82       std::frexp(row_norm / col_norm, &exponent);
  83       exponent /= 2;
  84
  85       if (exponent != 0) {
  86         const double scaled_col_norm = std::ldexp(col_norm, exponent);
  87         const double scaled_row_norm = std::ldexp(row_norm, -exponent);
  88         if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) {
  89           // Accept the new scaling. (Multiplication by powers of 2 should not
  90           // introduce rounding errors (ignoring non-normalized numbers and
  91           // over- or underflow))
  92           scaling_has_changed = true;
  93           companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent);
  94           companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent);
  95         }
  96       }
  97     }
  98   } while (scaling_has_changed);
  99
 100   companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal();
 101   companion_matrix = companion_matrix_offdiagonal;
 102   VLOG(3) << "Balanced companion matrix is\n" << companion_matrix;
 103 }
 104
 105 void BuildCompanionMatrix(const Vector& polynomial,
 106                           Matrix* companion_matrix_ptr) {
 107   CHECK_NOTNULL(companion_matrix_ptr);
 108   Matrix& companion_matrix = *companion_matrix_ptr;
 109
 110   const int degree = polynomial.size() - 1;
 111
 112   companion_matrix.resize(degree, degree);
 113   companion_matrix.setZero();
 114   companion_matrix.diagonal(-1).setOnes();
 115   companion_matrix.col(degree - 1) = -polynomial.reverse().head(degree);
 116 }
 117
 118 // Remove leading terms with zero coefficients.
 119 Vector RemoveLeadingZeros(const Vector& polynomial_in) {
 120   int i = 0;
 121   while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0.0) {
 122     ++i;
 123   }
 124   return polynomial_in.tail(polynomial_in.size() - i);
 125 }
 126
 127 void FindLinearPolynomialRoots(const Vector& polynomial,
 128                                Vector* real,
 129                                Vector* imaginary) {
 130   CHECK_EQ(polynomial.size(), 2);
 131   if (real != NULL) {
 132     real->resize(1);
 133     (*real)(0) = -polynomial(1) / polynomial(0);
 134   }
 135
 136   if (imaginary != NULL) {
 137     imaginary->setZero(1);
 138   }
 139 }
 140
 141 void FindQuadraticPolynomialRoots(const Vector& polynomial,
 142                                   Vector* real,
 143                                   Vector* imaginary) {
 144   CHECK_EQ(polynomial.size(), 3);
 145   const double a = polynomial(0);
 146   const double b = polynomial(1);
 147   const double c = polynomial(2);
 148   const double D = b * b - 4 * a * c;
 149   const double sqrt_D = sqrt(fabs(D));
 150   if (real != NULL) {
 151     real->setZero(2);
 152   }
 153   if (imaginary != NULL) {
 154     imaginary->setZero(2);
 155   }
 156
 157   // Real roots.
 158   if (D >= 0) {
 159     if (real != NULL) {
 160       // Stable quadratic roots according to BKP Horn.
 161       // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
 162       if (b >= 0) {
 163         (*real)(0) = (-b - sqrt_D) / (2.0 * a);
 164         (*real)(1) = (2.0 * c) / (-b - sqrt_D);
 165       } else {
 166         (*real)(0) = (2.0 * c) / (-b + sqrt_D);
 167         (*real)(1) = (-b + sqrt_D) / (2.0 * a);
 168       }
 169     }
 170     return;
 171   }
 172
 173   // Use the normal quadratic formula for the complex case.
 174   if (real != NULL) {
 175     (*real)(0) = -b / (2.0 * a);
 176     (*real)(1) = -b / (2.0 * a);
 177   }
 178   if (imaginary != NULL) {
 179     (*imaginary)(0) = sqrt_D / (2.0 * a);
 180     (*imaginary)(1) = -sqrt_D / (2.0 * a);
 181   }
 182 }
 183 }  // namespace
 184
 185 bool FindPolynomialRoots(const Vector& polynomial_in,
 186                          Vector* real,
 187                          Vector* imaginary) {
 188   if (polynomial_in.size() == 0) {
 189     LOG(ERROR) << "Invalid polynomial of size 0 passed to FindPolynomialRoots";
 190     return false;
 191   }
 192
 193   Vector polynomial = RemoveLeadingZeros(polynomial_in);
 194   const int degree = polynomial.size() - 1;
 195
 196   VLOG(3) << "Input polynomial: " << polynomial_in.transpose();
 197   if (polynomial.size() != polynomial_in.size()) {
 198     VLOG(3) << "Trimmed polynomial: " << polynomial.transpose();
 199   }
 200
 201   // Is the polynomial constant?
 202   if (degree == 0) {
 203     LOG(WARNING) << "Trying to extract roots from a constant "
 204                  << "polynomial in FindPolynomialRoots";
 205     // We return true with no roots, not false, as if the polynomial is constant
 206     // it is correct that there are no roots. It is not the case that they were
 207     // there, but that we have failed to extract them.
 208     return true;
 209   }
 210
 211   // Linear
 212   if (degree == 1) {
 213     FindLinearPolynomialRoots(polynomial, real, imaginary);
 214     return true;
 215   }
 216
 217   // Quadratic
 218   if (degree == 2) {
 219     FindQuadraticPolynomialRoots(polynomial, real, imaginary);
 220     return true;
 221   }
 222
 223   // The degree is now known to be at least 3. For cubic or higher
 224   // roots we use the method of companion matrices.
 225
 226   // Divide by leading term
 227   const double leading_term = polynomial(0);
 228   polynomial /= leading_term;
 229
 230   // Build and balance the companion matrix to the polynomial.
 231   Matrix companion_matrix(degree, degree);
 232   BuildCompanionMatrix(polynomial, &companion_matrix);
 233   BalanceCompanionMatrix(&companion_matrix);
 234
 235   // Find its (complex) eigenvalues.
 236   Eigen::EigenSolver<Matrix> solver(companion_matrix, false);
 237   if (solver.info() != Eigen::Success) {
 238     LOG(ERROR) << "Failed to extract eigenvalues from companion matrix.";
 239     return false;
 240   }
 241
 242   // Output roots
 243   if (real != NULL) {
 244     *real = solver.eigenvalues().real();
 245   } else {
 246     LOG(WARNING) << "NULL pointer passed as real argument to "
 247                  << "FindPolynomialRoots. Real parts of the roots will not "
 248                  << "be returned.";
 249   }
 250   if (imaginary != NULL) {
 251     *imaginary = solver.eigenvalues().imag();
 252   }
 253   return true;
 254 }
 255
 256 Vector DifferentiatePolynomial(const Vector& polynomial) {
 257   const int degree = polynomial.rows() - 1;
 258   CHECK_GE(degree, 0);
 259
 260   // Degree zero polynomials are constants, and their derivative does
 261   // not result in a smaller degree polynomial, just a degree zero
 262   // polynomial with value zero.
 263   if (degree == 0) {
 264     return Eigen::VectorXd::Zero(1);
 265   }
 266
 267   Vector derivative(degree);
 268   for (int i = 0; i < degree; ++i) {
 269     derivative(i) = (degree - i) * polynomial(i);
 270   }
 271
 272   return derivative;
 273 }
 274
 275 void MinimizePolynomial(const Vector& polynomial,
 276                         const double x_min,
 277                         const double x_max,
 278                         double* optimal_x,
 279                         double* optimal_value) {
 280   // Find the minimum of the polynomial at the two ends.
 281   //
 282   // We start by inspecting the middle of the interval. Technically
 283   // this is not needed, but we do this to make this code as close to
 284   // the minFunc package as possible.
 285   *optimal_x = (x_min + x_max) / 2.0;
 286   *optimal_value = EvaluatePolynomial(polynomial, *optimal_x);
 287
 288   const double x_min_value = EvaluatePolynomial(polynomial, x_min);
 289   if (x_min_value < *optimal_value) {
 290     *optimal_value = x_min_value;
 291     *optimal_x = x_min;
 292   }
 293
 294   const double x_max_value = EvaluatePolynomial(polynomial, x_max);
 295   if (x_max_value < *optimal_value) {
 296     *optimal_value = x_max_value;
 297     *optimal_x = x_max;
 298   }
 299
 300   // If the polynomial is linear or constant, we are done.
 301   if (polynomial.rows() <= 2) {
 302     return;
 303   }
 304
 305   const Vector derivative = DifferentiatePolynomial(polynomial);
 306   Vector roots_real;
 307   if (!FindPolynomialRoots(derivative, &roots_real, NULL)) {
 308     LOG(WARNING) << "Unable to find the critical points of "
 309                  << "the interpolating polynomial.";
 310     return;
 311   }
 312
 313   // This is a bit of an overkill, as some of the roots may actually
 314   // have a complex part, but its simpler to just check these values.
 315   for (int i = 0; i < roots_real.rows(); ++i) {
 316     const double root = roots_real(i);
 317     if ((root < x_min) || (root > x_max)) {
 318       continue;
 319     }
 320
 321     const double value = EvaluatePolynomial(polynomial, root);
 322     if (value < *optimal_value) {
 323       *optimal_value = value;
 324       *optimal_x = root;
 325     }
 326   }
 327 }
 328
 329 Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) {
 330   const int num_samples = samples.size();
 331   int num_constraints = 0;
 332   for (int i = 0; i < num_samples; ++i) {
 333     if (samples[i].value_is_valid) {
 334       ++num_constraints;
 335     }
 336     if (samples[i].gradient_is_valid) {
 337       ++num_constraints;
 338     }
 339   }
 340
 341   const int degree = num_constraints - 1;
 342
 343   Matrix lhs = Matrix::Zero(num_constraints, num_constraints);
 344   Vector rhs = Vector::Zero(num_constraints);
 345
 346   int row = 0;
 347   for (int i = 0; i < num_samples; ++i) {
 348     const FunctionSample& sample = samples[i];
 349     if (sample.value_is_valid) {
 350       for (int j = 0; j <= degree; ++j) {
 351         lhs(row, j) = pow(sample.x, degree - j);
 352       }
 353       rhs(row) = sample.value;
 354       ++row;
 355     }
 356
 357     if (sample.gradient_is_valid) {
 358       for (int j = 0; j < degree; ++j) {
 359         lhs(row, j) = (degree - j) * pow(sample.x, degree - j - 1);
 360       }
 361       rhs(row) = sample.gradient;
 362       ++row;
 363     }
 364   }
 365
 366   // TODO(sameeragarwal): This is a hack.
 367   // https://github.com/ceres-solver/ceres-solver/issues/248
 368   Eigen::FullPivLU<Matrix> lu(lhs);
 369   return lu.setThreshold(0.0).solve(rhs);
 370 }
 371
 372 void MinimizeInterpolatingPolynomial(const vector<FunctionSample>& samples,
 373                                      double x_min,
 374                                      double x_max,
 375                                      double* optimal_x,
 376                                      double* optimal_value) {
 377   const Vector polynomial = FindInterpolatingPolynomial(samples);
 378   MinimizePolynomial(polynomial, x_min, x_max, optimal_x, optimal_value);
 379   for (int i = 0; i < samples.size(); ++i) {
 380     const FunctionSample& sample = samples[i];
 381     if ((sample.x < x_min) || (sample.x > x_max)) {
 382       continue;
 383     }
 384
 385     const double value = EvaluatePolynomial(polynomial, sample.x);
 386     if (value < *optimal_value) {
 387       *optimal_x = sample.x;
 388       *optimal_value = value;
 389     }
 390   }
 391 }
 392
 393 }  // namespace internal
 394 }  // namespace ceres