11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2012 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: moll.markus@arcor.de (Markus Moll) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// sameeragarwal@google.com (Sameer Agarwal) 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/polynomial.h" 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cmath> 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cstddef> 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "Eigen/Dense" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/port.h" 4079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/stringprintf.h" 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace { 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Balancing function as described by B. N. Parlett and C. Reinsch, 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors". 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid BalanceCompanionMatrix(Matrix* companion_matrix_ptr) { 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_NOTNULL(companion_matrix_ptr); 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix& companion_matrix = *companion_matrix_ptr; 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix companion_matrix_offdiagonal = companion_matrix; 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix_offdiagonal.diagonal().setZero(); 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int degree = companion_matrix.rows(); 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // gamma <= 1 controls how much a change in the scaling has to 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // lower the 1-norm of the companion matrix to be accepted. 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // gamma = 1 seems to lead to cycles (numerical issues?), so 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // we set it slightly lower. 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double gamma = 0.9; 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Greedily scale row/column pairs until there is no change. 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool scaling_has_changed; 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling do { 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scaling_has_changed = false; 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < degree; ++i) { 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>(); 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Decompose row_norm/col_norm into mantissa * 2^exponent, 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // where 0.5 <= mantissa < 1. Discard mantissa (return value 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // of frexp), as only the exponent is needed. 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int exponent = 0; 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling std::frexp(row_norm / col_norm, &exponent); 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling exponent /= 2; 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (exponent != 0) { 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double scaled_col_norm = std::ldexp(col_norm, exponent); 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double scaled_row_norm = std::ldexp(row_norm, -exponent); 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) { 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Accept the new scaling. (Multiplication by powers of 2 should not 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // introduce rounding errors (ignoring non-normalized numbers and 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // over- or underflow)) 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scaling_has_changed = true; 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } while (scaling_has_changed); 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix_offdiagonal.diagonal() = companion_matrix.diagonal(); 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix = companion_matrix_offdiagonal; 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(3) << "Balanced companion matrix is\n" << companion_matrix; 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid BuildCompanionMatrix(const Vector& polynomial, 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix* companion_matrix_ptr) { 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_NOTNULL(companion_matrix_ptr); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix& companion_matrix = *companion_matrix_ptr; 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int degree = polynomial.size() - 1; 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix.resize(degree, degree); 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix.setZero(); 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix.diagonal(-1).setOnes(); 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling companion_matrix.col(degree - 1) = -polynomial.reverse().head(degree); 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Remove leading terms with zero coefficients. 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingVector RemoveLeadingZeros(const Vector& polynomial_in) { 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int i = 0; 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling while (i < (polynomial_in.size() - 1) && polynomial_in(i) == 0.0) { 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++i; 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return polynomial_in.tail(polynomial_in.size() - i); 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 12379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 12479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid FindLinearPolynomialRoots(const Vector& polynomial, 12579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector* real, 12679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector* imaginary) { 12779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_EQ(polynomial.size(), 2); 12879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (real != NULL) { 12979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez real->resize(1); 13079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(0) = -polynomial(1) / polynomial(0); 13179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 13279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 13379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (imaginary != NULL) { 13479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez imaginary->setZero(1); 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 13679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezvoid FindQuadraticPolynomialRoots(const Vector& polynomial, 13979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector* real, 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Vector* imaginary) { 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CHECK_EQ(polynomial.size(), 3); 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double a = polynomial(0); 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double b = polynomial(1); 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double c = polynomial(2); 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double D = b * b - 4 * a * c; 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double sqrt_D = sqrt(fabs(D)); 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (real != NULL) { 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez real->setZero(2); 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (imaginary != NULL) { 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez imaginary->setZero(2); 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Real roots. 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (D >= 0) { 15679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (real != NULL) { 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Stable quadratic roots according to BKP Horn. 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf 15979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (b >= 0) { 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(0) = (-b - sqrt_D) / (2.0 * a); 16179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(1) = (2.0 * c) / (-b - sqrt_D); 16279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else { 16379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(0) = (2.0 * c) / (-b + sqrt_D); 16479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(1) = (-b + sqrt_D) / (2.0 * a); 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 16779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return; 16879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 16979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 17079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Use the normal quadratic formula for the complex case. 17179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (real != NULL) { 17279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(0) = -b / (2.0 * a); 17379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*real)(1) = -b / (2.0 * a); 17479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 17579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (imaginary != NULL) { 17679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*imaginary)(0) = sqrt_D / (2.0 * a); 17779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (*imaginary)(1) = -sqrt_D / (2.0 * a); 17879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 17979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool FindPolynomialRoots(const Vector& polynomial_in, 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector* real, 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector* imaginary) { 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (polynomial_in.size() == 0) { 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Invalid polynomial of size 0 passed to FindPolynomialRoots"; 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return false; 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector polynomial = RemoveLeadingZeros(polynomial_in); 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int degree = polynomial.size() - 1; 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 19379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VLOG(3) << "Input polynomial: " << polynomial_in.transpose(); 19479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (polynomial.size() != polynomial_in.size()) { 19579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VLOG(3) << "Trimmed polynomial: " << polynomial.transpose(); 19679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Is the polynomial constant? 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (degree == 0) { 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << "Trying to extract roots from a constant " 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "polynomial in FindPolynomialRoots"; 20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // We return true with no roots, not false, as if the polynomial is constant 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // it is correct that there are no roots. It is not the case that they were 20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // there, but that we have failed to extract them. 20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 20679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 20779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 20879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Linear 20979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (degree == 1) { 21079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez FindLinearPolynomialRoots(polynomial, real, imaginary); 21179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return true; 21279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 21379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 21479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Quadratic 21579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (degree == 2) { 21679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez FindQuadraticPolynomialRoots(polynomial, real, imaginary); 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 22079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The degree is now known to be at least 3. For cubic or higher 22179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // roots we use the method of companion matrices. 22279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Divide by leading term 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double leading_term = polynomial(0); 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling polynomial /= leading_term; 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Build and balance the companion matrix to the polynomial. 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix companion_matrix(degree, degree); 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BuildCompanionMatrix(polynomial, &companion_matrix); 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BalanceCompanionMatrix(&companion_matrix); 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Find its (complex) eigenvalues. 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Eigen::EigenSolver<Matrix> solver(companion_matrix, false); 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (solver.info() != Eigen::Success) { 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Failed to extract eigenvalues from companion matrix."; 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return false; 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Output roots 2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (real != NULL) { 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *real = solver.eigenvalues().real(); 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << "NULL pointer passed as real argument to " 2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "FindPolynomialRoots. Real parts of the roots will not " 2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "be returned."; 2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (imaginary != NULL) { 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *imaginary = solver.eigenvalues().imag(); 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return true; 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingVector DifferentiatePolynomial(const Vector& polynomial) { 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int degree = polynomial.rows() - 1; 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_GE(degree, 0); 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Degree zero polynomials are constants, and their derivative does 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // not result in a smaller degree polynomial, just a degree zero 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // polynomial with value zero. 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (degree == 0) { 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return Eigen::VectorXd::Zero(1); 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector derivative(degree); 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < degree; ++i) { 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling derivative(i) = (degree - i) * polynomial(i); 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return derivative; 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid MinimizePolynomial(const Vector& polynomial, 2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double x_min, 2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double x_max, 2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* optimal_x, 2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* optimal_value) { 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Find the minimum of the polynomial at the two ends. 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // We start by inspecting the middle of the interval. Technically 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // this is not needed, but we do this to make this code as close to 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the minFunc package as possible. 2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_x = (x_min + x_max) / 2.0; 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_value = EvaluatePolynomial(polynomial, *optimal_x); 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double x_min_value = EvaluatePolynomial(polynomial, x_min); 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (x_min_value < *optimal_value) { 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_value = x_min_value; 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_x = x_min; 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double x_max_value = EvaluatePolynomial(polynomial, x_max); 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (x_max_value < *optimal_value) { 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_value = x_max_value; 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_x = x_max; 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If the polynomial is linear or constant, we are done. 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (polynomial.rows() <= 2) { 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Vector derivative = DifferentiatePolynomial(polynomial); 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector roots_real; 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!FindPolynomialRoots(derivative, &roots_real, NULL)) { 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << "Unable to find the critical points of " 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "the interpolating polynomial."; 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // This is a bit of an overkill, as some of the roots may actually 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // have a complex part, but its simpler to just check these values. 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < roots_real.rows(); ++i) { 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double root = roots_real(i); 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if ((root < x_min) || (root > x_max)) { 3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double value = EvaluatePolynomial(polynomial, root); 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (value < *optimal_value) { 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_value = value; 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_x = root; 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 32679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezstring FunctionSample::ToDebugString() const { 32779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return StringPrintf("[x: %.8e, value: %.8e, gradient: %.8e, " 32879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "value_is_valid: %d, gradient_is_valid: %d]", 32979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez x, value, gradient, value_is_valid, gradient_is_valid); 33079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 33179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingVector FindInterpolatingPolynomial(const vector<FunctionSample>& samples) { 3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_samples = samples.size(); 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_constraints = 0; 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_samples; ++i) { 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (samples[i].value_is_valid) { 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++num_constraints; 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (samples[i].gradient_is_valid) { 3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++num_constraints; 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int degree = num_constraints - 1; 34579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Matrix lhs = Matrix::Zero(num_constraints, num_constraints); 3471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector rhs = Vector::Zero(num_constraints); 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int row = 0; 3501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < num_samples; ++i) { 3511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const FunctionSample& sample = samples[i]; 3521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (sample.value_is_valid) { 3531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j <= degree; ++j) { 3541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling lhs(row, j) = pow(sample.x, degree - j); 3551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rhs(row) = sample.value; 3571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++row; 3581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (sample.gradient_is_valid) { 3611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < degree; ++j) { 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling lhs(row, j) = (degree - j) * pow(sample.x, degree - j - 1); 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling rhs(row) = sample.gradient; 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++row; 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return lhs.fullPivLu().solve(rhs); 3701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid MinimizeInterpolatingPolynomial(const vector<FunctionSample>& samples, 3731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_min, 3741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double x_max, 3751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* optimal_x, 3761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* optimal_value) { 3771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Vector polynomial = FindInterpolatingPolynomial(samples); 3781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling MinimizePolynomial(polynomial, x_min, x_max, optimal_x, optimal_value); 3791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < samples.size(); ++i) { 3801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const FunctionSample& sample = samples[i]; 3811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if ((sample.x < x_min) || (sample.x > x_max)) { 3821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling continue; 3831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double value = EvaluatePolynomial(polynomial, sample.x); 3861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (value < *optimal_value) { 3871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_x = sample.x; 3881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling *optimal_value = value; 3891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 3941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 395