1399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Ceres Solver - A fast non-linear least squares minimizer 2399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Copyright 2013 Google Inc. All rights reserved. 3399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// http://code.google.com/p/ceres-solver/ 4399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 5399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Redistribution and use in source and binary forms, with or without 6399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// modification, are permitted provided that the following conditions are met: 7399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 8399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// * Redistributions of source code must retain the above copyright notice, 9399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// this list of conditions and the following disclaimer. 10399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// * Redistributions in binary form must reproduce the above copyright notice, 11399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// this list of conditions and the following disclaimer in the documentation 12399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// and/or other materials provided with the distribution. 13399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// * Neither the name of Google Inc. nor the names of its contributors may be 14399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// used to endorse or promote products derived from this software without 15399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// specific prior written permission. 16399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 17399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// POSSIBILITY OF SUCH DAMAGE. 28399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// 29399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// Author: sameeragarwal@google.com (Sameer Agarwal) 30399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 31399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#include "ceres/lapack.h" 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/internal/port.h" 3479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/linear_solver.h" 35399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#include "glog/logging.h" 36399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 37399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger// C interface to the LAPACK Cholesky factorization and triangular solve. 38399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingerextern "C" void dpotrf_(char* uplo, 39399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* n, 40399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* a, 41399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* lda, 42399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* info); 43399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 44399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingerextern "C" void dpotrs_(char* uplo, 45399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* n, 46399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* nrhs, 47399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* a, 48399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* lda, 49399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* b, 50399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* ldb, 51399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* info); 52399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 53399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingerextern "C" void dgels_(char* uplo, 54399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* m, 55399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* n, 56399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* nrhs, 57399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* a, 58399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* lda, 59399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* b, 60399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* ldb, 61399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* work, 62399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* lwork, 63399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int* info); 64399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 65399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 66399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingernamespace ceres { 67399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingernamespace internal { 68399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 6979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolverTerminationType LAPACK::SolveInPlaceUsingCholesky( 7079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_rows, 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double* in_lhs, 7279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* rhs_and_solution, 7379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 74399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#ifdef CERES_NO_LAPACK 75399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger LOG(FATAL) << "Ceres was built without a BLAS library."; 7679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 77399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#else 78399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger char uplo = 'L'; 79399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int n = num_rows; 80399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int info = 0; 81399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int nrhs = 1; 82399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* lhs = const_cast<double*>(in_lhs); 83399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 84399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger dpotrf_(&uplo, &n, lhs, &n, &info); 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (info < 0) { 8679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(FATAL) << "Congratulations, you found a bug in Ceres." 8779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Please report it." 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "LAPACK::dpotrf fatal error." 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Argument: " << -info << " is invalid."; 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 9279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 9379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (info > 0) { 9479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = 9579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez StringPrintf( 9679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "LAPACK::dpotrf numerical failure. " 9779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "The leading minor of order %d is not positive definite.", info); 9879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FAILURE; 99399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 100399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 101399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info); 10279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (info < 0) { 10379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(FATAL) << "Congratulations, you found a bug in Ceres." 10479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Please report it." 10579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "LAPACK::dpotrs fatal error." 10679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Argument: " << -info << " is invalid."; 10779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 108399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 109399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "Success"; 11179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_SUCCESS; 112399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#endif 113399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger}; 114399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 115399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettingerint LAPACK::EstimateWorkSizeForQR(int num_rows, int num_cols) { 116399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#ifdef CERES_NO_LAPACK 117399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger LOG(FATAL) << "Ceres was built without a LAPACK library."; 118399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger return -1; 119399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#else 120399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger char trans = 'N'; 121399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int nrhs = 1; 122399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int lwork = -1; 123399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double work; 124399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int info = 0; 125399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger dgels_(&trans, 126399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &num_rows, 127399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &num_cols, 128399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &nrhs, 129399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger NULL, 130399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &num_rows, 131399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger NULL, 132399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &num_rows, 133399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &work, 134399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &lwork, 135399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &info); 136399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (info < 0) { 13879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(FATAL) << "Congratulations, you found a bug in Ceres." 13979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Please report it." 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "LAPACK::dgels fatal error." 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Argument: " << -info << " is invalid."; 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return static_cast<int>(work); 144399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#endif 145399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} 146399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolverTerminationType LAPACK::SolveInPlaceUsingQR( 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_rows, 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int num_cols, 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const double* in_lhs, 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int work_size, 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* work, 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez double* rhs_and_solution, 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 155399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#ifdef CERES_NO_LAPACK 156399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger LOG(FATAL) << "Ceres was built without a LAPACK library."; 15779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 158399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#else 159399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger char trans = 'N'; 160399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int m = num_rows; 161399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int n = num_cols; 162399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int nrhs = 1; 163399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int lda = num_rows; 164399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int ldb = num_rows; 165399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger int info = 0; 166399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger double* lhs = const_cast<double*>(in_lhs); 167399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 168399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger dgels_(&trans, 169399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &m, 170399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &n, 171399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &nrhs, 172399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger lhs, 173399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &lda, 174399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger rhs_and_solution, 175399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &ldb, 176399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger work, 177399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &work_size, 178399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger &info); 179399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 18079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (info < 0) { 18179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LOG(FATAL) << "Congratulations, you found a bug in Ceres." 18279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Please report it." 18379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "LAPACK::dgels fatal error." 18479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez << "Argument: " << -info << " is invalid."; 18579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 18679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "Success."; 18879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_SUCCESS; 189399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#endif 190399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} 191399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger 192399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} // namespace internal 193399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger} // namespace ceres 194