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