Add a polynomial solver Add a function to find the (complex) roots of a polynomial with real coefficients. The roots are extracted as the eigenvalues of the (balanced) companion matrix. Also adds a test. The polynomial solver will be needed in the Dogleg subspace strategy. Change-Id: Ia6626158819efb858522b7f4998649ca010d6688
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 2914601..0d33a86 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -62,6 +62,7 @@ loss_function.cc normal_prior.cc partitioned_matrix_view.cc + polynomial_solver.cc problem.cc problem_impl.cc program.cc @@ -198,6 +199,7 @@ CERES_TEST(numeric_diff_cost_function) CERES_TEST(parameter_block) CERES_TEST(partitioned_matrix_view) + CERES_TEST(polynomial_solver) CERES_TEST(problem) CERES_TEST(residual_block) CERES_TEST(residual_block_utils)
diff --git a/internal/ceres/polynomial_solver.cc b/internal/ceres/polynomial_solver.cc new file mode 100644 index 0000000..91c6001 --- /dev/null +++ b/internal/ceres/polynomial_solver.cc
@@ -0,0 +1,187 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: moll.markus@arcor.de (Markus Moll) + +#include "ceres/polynomial_solver.h" + +#include <glog/logging.h> +#include <cmath> +#include <cstddef> + +#include "Eigen/Dense" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { +namespace { + +// Balancing function as described by B. N. Parlett and C. Reinsch, +// "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors". +// In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, +// Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 +void BalanceCompanionMatrix(Matrix* companion_matrix_ptr) { + CHECK_NOTNULL(companion_matrix_ptr); + Matrix& companion_matrix = *companion_matrix_ptr; + Matrix companion_matrix_offdiagonal = companion_matrix; + companion_matrix_offdiagonal.diagonal().setZero(); + + const int degree = companion_matrix.rows(); + + // gamma <= 1 controls how much a change in the scaling has to + // lower the 1-norm of the companion matrix to be accepted. + // + // gamma = 1 seems to lead to cycles (numerical issues?), so + // we set it slightly lower. + const double gamma = 0.9; + + // Greedily scale row/column pairs until there is no change. + bool scaling_has_changed; + do { + scaling_has_changed = false; + + for (int i = 0; i < degree; ++i) { + const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); + const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>(); + + // Decompose row_norm/col_norm into mantissa * 2^exponent, + // where 0.5 <= mantissa < 1. Discard mantissa (return value + // of frexp), as only the exponent is needed. + int exponent = 0; + std::frexp(row_norm / col_norm, &exponent); + exponent /= 2; + + if (exponent != 0) { + const double scaled_col_norm = std::ldexp(col_norm, exponent); + const double scaled_row_norm = std::ldexp(row_norm, -exponent); + if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) { + // Accept the new scaling. (Multiplication by powers of 2 should not + // introduce rounding errors (ignoring non-normalized numbers and + // over- or underflow)) + scaling_has_changed = true; + companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); + companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); + } + } + } + } while (scaling_has_changed); + + companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal(); + companion_matrix = companion_matrix_offdiagonal; + VLOG(3) << "Balanced companion matrix is\n" << companion_matrix; +} + +void BuildCompanionMatrix(const Vector& polynomial, + Matrix* companion_matrix_ptr) { + CHECK_NOTNULL(companion_matrix_ptr); + Matrix& companion_matrix = *companion_matrix_ptr; + + const int degree = polynomial.size() - 1; + + companion_matrix.resize(degree, degree); + companion_matrix.setZero(); + companion_matrix.diagonal(-1).setOnes(); + companion_matrix.col(degree-1) = -polynomial.reverse().head(degree-1); +} + +// Remove leading terms with zero coefficients. +Vector RemoveLeadingZeros(const Vector& polynomial_in) { + int i = 0; + while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0.0) { + ++i; + } + return polynomial_in.tail(polynomial_in.size() - i); +} +} // namespace + +bool FindPolynomialRoots(const Vector& polynomial_in, + Vector* real, + Vector* imaginary) { + const double epsilon = std::numeric_limits<double>::epsilon(); + + if (polynomial_in.size() == 0) { + LOG(ERROR) << "Invalid polynomial of size 0 passed to FindPolynomialRoots"; + return false; + } + + Vector polynomial = RemoveLeadingZeros(polynomial_in); + const int degree = polynomial.size() - 1; + + // Is the polynomial constant? + if (degree == 0) { + LOG(WARNING) << "Trying to extract roots from a constant " + << "polynomial in FindPolynomialRoots"; + return true; + } + + // 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); + BalanceCompanionMatrix(&companion_matrix); + + // Find its (complex) eigenvalues. + Eigen::EigenSolver<Matrix> solver(companion_matrix, + Eigen::EigenvaluesOnly); + if (solver.info() != Eigen::Success) { + LOG(ERROR) << "Failed to extract eigenvalues from companion matrix."; + return false; + } + + // Output roots + if (real != NULL) { + *real = solver.eigenvalues().real(); + } else { + LOG(WARNING) << "NULL pointer passed as real argument to " + << "FindPolynomialRoots. Real parts of the roots will not " + << "be returned."; + } + if (imaginary != NULL) { + *imaginary = solver.eigenvalues().imag(); + } + return true; +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/polynomial_solver.h b/internal/ceres/polynomial_solver.h new file mode 100644 index 0000000..1cf07dd --- /dev/null +++ b/internal/ceres/polynomial_solver.h
@@ -0,0 +1,65 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: moll.markus@arcor.de (Markus Moll) + +#ifndef CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ +#define CERES_INTERNAL_POLYNOMIAL_SOLVER_H_ + +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +// Use the companion matrix eigenvalues to determine the roots of the polynomial +// +// sum_{i=0}^N polynomial(i) x^{N-i}. +// +// This function returns true on success, false otherwise. +// Failure indicates that the polynomial is invalid (of size 0) or +// that the eigenvalues of the companion matrix could not be computed. +// On failure, a more detailed message will be written to LOG(ERROR). +// If real is not NULL, the real parts of the roots will be returned in it. +// Likewise, if imaginary is not NULL, imaginary parts will be returned in it. +bool FindPolynomialRoots(const Vector& polynomial, + Vector* real, + Vector* imaginary); + +// Evaluate the polynomial at x using the Horner scheme. +inline double EvaluatePolynomial(const Vector& polynomial, double x) { + double v = 0.0; + for (int i = 0; i < polynomial.size(); ++i) { + v = v * x + polynomial(i); + } + return v; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_POLYNOMIAL_SOLVER_H_
diff --git a/internal/ceres/polynomial_solver_test.cc b/internal/ceres/polynomial_solver_test.cc new file mode 100644 index 0000000..ae347fb --- /dev/null +++ b/internal/ceres/polynomial_solver_test.cc
@@ -0,0 +1,223 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: moll.markus@arcor.de (Markus Moll) + +#include "ceres/polynomial_solver.h" + +#include <limits> +#include <cmath> +#include <cstddef> +#include <algorithm> +#include "gtest/gtest.h" +#include "ceres/test_util.h" + +namespace ceres { +namespace internal { +namespace { + +// For IEEE-754 doubles, machine precision is about 2e-16. +const double kEpsilon = 1e-13; +const double kEpsilonLoose = 1e-9; + +// Return the constant polynomial p(x) = 1.23. +Vector ConstantPolynomial(double value) { + Vector poly(1); + poly(0) = value; + return poly; +} + +// Return the polynomial p(x) = poly(x) * (x - root). +Vector AddRealRoot(const Vector& poly, double root) { + Vector poly2(poly.size() + 1); + poly2.setZero(); + poly2.head(poly.size()) += poly; + poly2.tail(poly.size()) -= root * poly; + return poly2; +} + +// Return the polynomial +// p(x) = poly(x) * (x - real - imag*i) * (x - real + imag*i). +Vector AddComplexRootPair(const Vector& poly, double real, double imag) { + Vector poly2(poly.size() + 2); + poly2.setZero(); + // Multiply poly by x^2 - 2real + abs(real,imag)^2 + poly2.head(poly.size()) += poly; + poly2.segment(1, poly.size()) -= 2 * real * poly; + poly2.tail(poly.size()) += (real*real + imag*imag) * poly; + return poly2; +} + +// Sort the entries in a vector. +// Needed because the roots are not returned in sorted order. +Vector SortVector(const Vector& in) { + Vector out(in); + std::sort(out.data(), out.data() + out.size()); + return out; +} + +// Run a test with the polynomial defined by the N real roots in roots_real. +// If use_real is false, NULL is passed as the real argument to +// FindPolynomialRoots. If use_imaginary is false, NULL is passed as the +// imaginary argument to FindPolynomialRoots. +template<int N> +void RunPolynomialTestRealRoots(const double (&real_roots)[N], + bool use_real, + bool use_imaginary, + double epsilon) { + Vector real; + Vector imaginary; + Vector poly = ConstantPolynomial(1.23); + for (int i = 0; i < N; ++i) { + poly = AddRealRoot(poly, real_roots[i]); + } + Vector* const real_ptr = use_real ? &real : NULL; + Vector* const imaginary_ptr = use_imaginary ? &imaginary : NULL; + bool success = FindPolynomialRoots(poly, real_ptr, imaginary_ptr); + + EXPECT_EQ(success, true); + if (use_real) { + EXPECT_EQ(real.size(), N); + real = SortVector(real); + ExpectArraysClose(N, real.data(), real_roots, epsilon); + } + if (use_imaginary) { + EXPECT_EQ(imaginary.size(), N); + const Vector zeros = Vector::Zero(N); + ExpectArraysClose(N, imaginary.data(), zeros.data(), epsilon); + } +} +} // namespace + +TEST(PolynomialSolver, InvalidPolynomialOfZeroLengthIsRejected) { + // Vector poly(0) is an ambiguous constructor call, so + // use the constructor with explicit column count. + Vector poly(0, 1); + Vector real; + Vector imag; + bool success = FindPolynomialRoots(poly, &real, &imag); + + EXPECT_EQ(success, false); +} + +TEST(PolynomialSolver, ConstantPolynomialReturnsNoRoots) { + Vector poly = ConstantPolynomial(1.23); + Vector real; + Vector imag; + bool success = FindPolynomialRoots(poly, &real, &imag); + + EXPECT_EQ(success, true); + EXPECT_EQ(real.size(), 0); + EXPECT_EQ(imag.size(), 0); +} + +TEST(PolynomialSolver, LinearPolynomialWithPositiveRootWorks) { + const double roots[1] = { 42.42 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, LinearPolynomialWithNegativeRootWorks) { + const double roots[1] = { -42.42 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, QuadraticPolynomialWithPositiveRootsWorks) { + const double roots[2] = { 1.0, 42.42 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, QuadraticPolynomialWithOneNegativeRootWorks) { + const double roots[2] = { -42.42, 1.0 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, QuadraticPolynomialWithTwoNegativeRootsWorks) { + const double roots[2] = { -42.42, -1.0 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, QuadraticPolynomialWithCloseRootsWorks) { + const double roots[2] = { 42.42, 42.43 }; + RunPolynomialTestRealRoots(roots, true, false, kEpsilonLoose); +} + +TEST(PolynomialSolver, QuadraticPolynomialWithComplexRootsWorks) { + Vector real; + Vector imag; + + Vector poly = ConstantPolynomial(1.23); + poly = AddComplexRootPair(poly, 42.42, 4.2); + bool success = FindPolynomialRoots(poly, &real, &imag); + + EXPECT_EQ(success, true); + EXPECT_EQ(real.size(), 2); + EXPECT_EQ(imag.size(), 2); + ExpectClose(real(0), 42.42, kEpsilon); + ExpectClose(real(1), 42.42, kEpsilon); + ExpectClose(std::abs(imag(0)), 4.2, kEpsilon); + ExpectClose(std::abs(imag(1)), 4.2, kEpsilon); + ExpectClose(std::abs(imag(0) + imag(1)), 0.0, kEpsilon); +} + +TEST(PolynomialSolver, QuarticPolynomialWorks) { + const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, QuarticPolynomialWithTwoClustersOfCloseRootsWorks) { + const double roots[4] = { 1.23e-1, 2.46e-1, 1.23e+5, 2.46e+5 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); +} + +TEST(PolynomialSolver, QuarticPolynomialWithTwoZeroRootsWorks) { + const double roots[4] = { -42.42, 0.0, 0.0, 42.42 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilonLoose); +} + +TEST(PolynomialSolver, QuarticMonomialWorks) { + const double roots[4] = { 0.0, 0.0, 0.0, 0.0 }; + RunPolynomialTestRealRoots(roots, true, true, kEpsilon); +} + +TEST(PolynomialSolver, NullPointerAsImaginaryPartWorks) { + const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; + RunPolynomialTestRealRoots(roots, true, false, kEpsilon); +} + +TEST(PolynomialSolver, NullPointerAsRealPartWorks) { + const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; + RunPolynomialTestRealRoots(roots, false, true, kEpsilon); +} + +TEST(PolynomialSolver, BothOutputArgumentsNullWorks) { + const double roots[4] = { 1.23e-4, 1.23e-1, 1.23e+2, 1.23e+5 }; + RunPolynomialTestRealRoots(roots, false, false, kEpsilon); +} + +} // namespace internal +} // namespace ceres