Home | History | Annotate | Download | only in ceres 
      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 (at) arcor.de (Markus Moll)
     30 
     31 #include "ceres/polynomial_solver.h"
     32 
     33 #include <cmath>
     34 #include <cstddef>
     35 #include "Eigen/Dense"
     36 #include "ceres/internal/port.h"
     37 #include "glog/logging.h"
     38 
     39 namespace ceres {
     40 namespace internal {
     41 namespace {
     42 
     43 // Balancing function as described by B. N. Parlett and C. Reinsch,
     44 // "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors".
     45 // In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304,
     46 // Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404
     47 void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) {
     48   CHECK_NOTNULL(companion_matrix_ptr);
     49   Matrix& companion_matrix = *companion_matrix_ptr;
     50   Matrix companion_matrix_offdiagonal = companion_matrix;
     51   companion_matrix_offdiagonal.diagonal().setZero();
     52 
     53   const int degree = companion_matrix.rows();
     54 
     55   // gamma <= 1 controls how much a change in the scaling has to
     56   // lower the 1-norm of the companion matrix to be accepted.
     57   //
     58   // gamma = 1 seems to lead to cycles (numerical issues?), so
     59   // we set it slightly lower.
     60   const double gamma = 0.9;
     61 
     62   // Greedily scale row/column pairs until there is no change.
     63   bool scaling_has_changed;
     64   do {
     65     scaling_has_changed = false;
     66 
     67     for (int i = 0; i < degree; ++i) {
     68       const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>();
     69       const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>();
     70 
     71       // Decompose row_norm/col_norm into mantissa * 2^exponent,
     72       // where 0.5 <= mantissa < 1. Discard mantissa (return value
     73       // of frexp), as only the exponent is needed.
     74       int exponent = 0;
     75       std::frexp(row_norm / col_norm, &exponent);
     76       exponent /= 2;
     77 
     78       if (exponent != 0) {
     79         const double scaled_col_norm = std::ldexp(col_norm, exponent);
     80         const double scaled_row_norm = std::ldexp(row_norm, -exponent);
     81         if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) {
     82           // Accept the new scaling. (Multiplication by powers of 2 should not
     83           // introduce rounding errors (ignoring non-normalized numbers and
     84           // over- or underflow))
     85           scaling_has_changed = true;
     86           companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent);
     87           companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent);
     88         }
     89       }
     90     }
     91   } while (scaling_has_changed);
     92 
     93   companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal();
     94   companion_matrix = companion_matrix_offdiagonal;
     95   VLOG(3) << "Balanced companion matrix is\n" << companion_matrix;
     96 }
     97 
     98 void BuildCompanionMatrix(const Vector& polynomial,
     99                           Matrix* companion_matrix_ptr) {
    100   CHECK_NOTNULL(companion_matrix_ptr);
    101   Matrix& companion_matrix = *companion_matrix_ptr;
    102 
    103   const int degree = polynomial.size() - 1;
    104 
    105   companion_matrix.resize(degree, degree);
    106   companion_matrix.setZero();
    107   companion_matrix.diagonal(-1).setOnes();
    108   companion_matrix.col(degree - 1) = -polynomial.reverse().head(degree);
    109 }
    110 
    111 // Remove leading terms with zero coefficients.
    112 Vector RemoveLeadingZeros(const Vector& polynomial_in) {
    113   int i = 0;
    114   while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0.0) {
    115     ++i;
    116   }
    117   return polynomial_in.tail(polynomial_in.size() - i);
    118 }
    119 }  // namespace
    120 
    121 bool FindPolynomialRoots(const Vector& polynomial_in,
    122                          Vector* real,
    123                          Vector* imaginary) {
    124   if (polynomial_in.size() == 0) {
    125     LOG(ERROR) << "Invalid polynomial of size 0 passed to FindPolynomialRoots";
    126     return false;
    127   }
    128 
    129   Vector polynomial = RemoveLeadingZeros(polynomial_in);
    130   const int degree = polynomial.size() - 1;
    131 
    132   // Is the polynomial constant?
    133   if (degree == 0) {
    134     LOG(WARNING) << "Trying to extract roots from a constant "
    135                  << "polynomial in FindPolynomialRoots";
    136     return true;
    137   }
    138 
    139   // Divide by leading term
    140   const double leading_term = polynomial(0);
    141   polynomial /= leading_term;
    142 
    143   // Separately handle linear polynomials.
    144   if (degree == 1) {
    145     if (real != NULL) {
    146       real->resize(1);
    147       (*real)(0) = -polynomial(1);
    148     }
    149     if (imaginary != NULL) {
    150       imaginary->resize(1);
    151       imaginary->setZero();
    152     }
    153   }
    154 
    155   // The degree is now known to be at least 2.
    156   // Build and balance the companion matrix to the polynomial.
    157   Matrix companion_matrix(degree, degree);
    158   BuildCompanionMatrix(polynomial, &companion_matrix);
    159   BalanceCompanionMatrix(&companion_matrix);
    160 
    161   // Find its (complex) eigenvalues.
    162   Eigen::EigenSolver<Matrix> solver(companion_matrix, false);
    163   if (solver.info() != Eigen::Success) {
    164     LOG(ERROR) << "Failed to extract eigenvalues from companion matrix.";
    165     return false;
    166   }
    167 
    168   // Output roots
    169   if (real != NULL) {
    170     *real = solver.eigenvalues().real();
    171   } else {
    172     LOG(WARNING) << "NULL pointer passed as real argument to "
    173                  << "FindPolynomialRoots. Real parts of the roots will not "
    174                  << "be returned.";
    175   }
    176   if (imaginary != NULL) {
    177     *imaginary = solver.eigenvalues().imag();
    178   }
    179   return true;
    180 }
    181 
    182 }  // namespace internal
    183 }  // namespace ceres
    184