1 files changed, 88 insertions, 13 deletions

diff --git a/internal/ceres/polynomial.cc b/internal/ceres/polynomial.cc
index 3238b89..75f43de 100644
--- a/internal/ceres/polynomial.cc
+++ b/internal/ceres/polynomial.cc
@@ -37,6 +37,7 @@
 
 #include "Eigen/Dense"
 #include "ceres/internal/port.h"
+#include "ceres/stringprintf.h"
 #include "glog/logging.h"
 
 namespace ceres {
@@ -119,6 +120,63 @@ Vector RemoveLeadingZeros(const Vector& polynomial_in) {
   }
   return polynomial_in.tail(polynomial_in.size() - i);
 }
+
+void FindLinearPolynomialRoots(const Vector& polynomial,
+                               Vector* real,
+                               Vector* imaginary) {
+  CHECK_EQ(polynomial.size(), 2);
+  if (real != NULL) {
+    real->resize(1);
+    (*real)(0) = -polynomial(1) / polynomial(0);
+  }
+
+  if (imaginary != NULL) {
+    imaginary->setZero(1);
+  }
+}
+
+void FindQuadraticPolynomialRoots(const Vector& polynomial,
+                                  Vector* real,
+                                  Vector* imaginary) {
+  CHECK_EQ(polynomial.size(), 3);
+  const double a = polynomial(0);
+  const double b = polynomial(1);
+  const double c = polynomial(2);
+  const double D = b * b - 4 * a * c;
+  const double sqrt_D = sqrt(fabs(D));
+  if (real != NULL) {
+    real->setZero(2);
+  }
+  if (imaginary != NULL) {
+    imaginary->setZero(2);
+  }
+
+  // Real roots.
+  if (D >= 0) {
+    if (real != NULL) {
+      // Stable quadratic roots according to BKP Horn.
+      // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
+      if (b >= 0) {
+        (*real)(0) = (-b - sqrt_D) / (2.0 * a);
+        (*real)(1) = (2.0 * c) / (-b - sqrt_D);
+      } else {
+        (*real)(0) = (2.0 * c) / (-b + sqrt_D);
+        (*real)(1) = (-b + sqrt_D) / (2.0 * a);
+      }
+    }
+    return;
+  }
+
+  // Use the normal quadratic formula for the complex case.
+  if (real != NULL) {
+    (*real)(0) = -b / (2.0 * a);
+    (*real)(1) = -b / (2.0 * a);
+  }
+  if (imaginary != NULL) {
+    (*imaginary)(0) = sqrt_D / (2.0 * a);
+    (*imaginary)(1) = -sqrt_D / (2.0 * a);
+  }
+}
 }  // namespace
 
 bool FindPolynomialRoots(const Vector& polynomial_in,
@@ -132,30 +190,40 @@ bool FindPolynomialRoots(const Vector& polynomial_in,
   Vector polynomial = RemoveLeadingZeros(polynomial_in);
   const int degree = polynomial.size() - 1;
 
+  VLOG(3) << "Input polynomial: " << polynomial_in.transpose();
+  if (polynomial.size() != polynomial_in.size()) {
+    VLOG(3) << "Trimmed polynomial: " << polynomial.transpose();
+  }
+
   // Is the polynomial constant?
   if (degree == 0) {
     LOG(WARNING) << "Trying to extract roots from a constant "
                  << "polynomial in FindPolynomialRoots";
+    // We return true with no roots, not false, as if the polynomial is constant
+    // it is correct that there are no roots. It is not the case that they were
+    // there, but that we have failed to extract them.
+    return true;
+  }
+
+  // Linear
+  if (degree == 1) {
+    FindLinearPolynomialRoots(polynomial, real, imaginary);
+    return true;
+  }
+
+  // Quadratic
+  if (degree == 2) {
+    FindQuadraticPolynomialRoots(polynomial, real, imaginary);
     return true;
   }
 
+  // The degree is now known to be at least 3. For cubic or higher
+  // roots we use the method of companion matrices.
+
   // Divide by leading term
   const double leading_term = polynomial(0);
   polynomial /= leading_term;
 
-  // Separately handle linear polynomials.
-  if (degree == 1) {
-    if (real != NULL) {
-      real->resize(1);
-      (*real)(0) = -polynomial(1);
-    }
-    if (imaginary != NULL) {
-      imaginary->resize(1);
-      imaginary->setZero();
-    }
-  }
-
-  // The degree is now known to be at least 2.
   // Build and balance the companion matrix to the polynomial.
   Matrix companion_matrix(degree, degree);
   BuildCompanionMatrix(polynomial, &companion_matrix);
@@ -255,6 +323,12 @@ void MinimizePolynomial(const Vector& polynomial,
   }
 }
 
+string FunctionSample::ToDebugString() const {
+  return StringPrintf("[x: %.8e, value: %.8e, gradient: %.8e, "
+                      "value_is_valid: %d, gradient_is_valid: %d]",
+                      x, value, gradient, value_is_valid, gradient_is_valid);
+}
+
 Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) {
   const int num_samples = samples.size();
   int num_constraints = 0;
@@ -268,6 +342,7 @@ Vector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) {
   }
 
   const int degree = num_constraints - 1;
+
   Matrix lhs = Matrix::Zero(num_constraints, num_constraints);
   Vector rhs = Vector::Zero(num_constraints);