10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: sameeragarwal@google.com (Sameer Agarwal) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// This include must come before any #ifndef check on Ceres compile options. 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/internal/port.h" 3379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_NO_SUITESPARSE 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/suitesparse.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "cholmod.h" 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/compressed_col_sparse_matrix_utils.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/compressed_row_sparse_matrix.h" 4179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/linear_solver.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/triplet_sparse_matrix.h" 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingSuiteSparse::SuiteSparse() { 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_start(&cc_); 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingSuiteSparse::~SuiteSparse() { 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_finish(&cc_); 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongcholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) { 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_triplet triplet; 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.nrow = A->num_rows(); 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.ncol = A->num_cols(); 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.nzmax = A->max_num_nonzeros(); 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.nnz = A->num_nonzeros(); 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.i = reinterpret_cast<void*>(A->mutable_rows()); 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.j = reinterpret_cast<void*>(A->mutable_cols()); 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.x = reinterpret_cast<void*>(A->mutable_values()); 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.stype = 0; // Matrix is not symmetric. 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.itype = CHOLMOD_INT; 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.xtype = CHOLMOD_REAL; 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.dtype = CHOLMOD_DOUBLE; 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongcholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose( 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* A) { 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_triplet triplet; 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.ncol = A->num_rows(); // swap row and columns 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.nrow = A->num_cols(); 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.nzmax = A->max_num_nonzeros(); 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.nnz = A->num_nonzeros(); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // swap rows and columns 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.j = reinterpret_cast<void*>(A->mutable_rows()); 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.i = reinterpret_cast<void*>(A->mutable_cols()); 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.x = reinterpret_cast<void*>(A->mutable_values()); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.stype = 0; // Matrix is not symmetric. 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.itype = CHOLMOD_INT; 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.xtype = CHOLMOD_REAL; 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong triplet.dtype = CHOLMOD_DOUBLE; 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingcholmod_sparse SuiteSparse::CreateSparseMatrixTransposeView( 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CompressedRowSparseMatrix* A) { 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_sparse m; 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.nrow = A->num_cols(); 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.ncol = A->num_rows(); 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.nzmax = A->num_nonzeros(); 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.nz = NULL; 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.p = reinterpret_cast<void*>(A->mutable_rows()); 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.i = reinterpret_cast<void*>(A->mutable_cols()); 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.x = reinterpret_cast<void*>(A->mutable_values()); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.z = NULL; 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.stype = 0; // Matrix is not symmetric. 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.itype = CHOLMOD_INT; 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.xtype = CHOLMOD_REAL; 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.dtype = CHOLMOD_DOUBLE; 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.sorted = 1; 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling m.packed = 1; 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return m; 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongcholmod_dense* SuiteSparse::CreateDenseVector(const double* x, 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int in_size, 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int out_size) { 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_LE(in_size, out_size); 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_); 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (x != NULL) { 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong memcpy(v->x, x, in_size*sizeof(*x)); 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return v; 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 12779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezcholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A, 12879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Cholmod can try multiple re-ordering strategies to find a fill 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // reducing ordering. Here we just tell it use AMD with automatic 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // matrix dependence choice of supernodal versus simplicial 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // factorization. 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cc_.nmethods = 1; 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cc_.method[0].ordering = CHOLMOD_AMD; 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cc_.supernodal = CHOLMOD_AUTO; 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_factor* factor = cholmod_analyze(A, &cc_); 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (VLOG_IS_ON(2)) { 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_); 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (cc_.status != CHOLMOD_OK) { 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = StringPrintf("cholmod_analyze failed. error code: %d", 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cc_.status); 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return NULL; 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return CHECK_NOTNULL(factor); 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongcholmod_factor* SuiteSparse::BlockAnalyzeCholesky( 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_sparse* A, 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& row_blocks, 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int>& col_blocks, 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> ordering; 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!BlockAMDOrdering(A, row_blocks, col_blocks, &ordering)) { 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return NULL; 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 16079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return AnalyzeCholeskyWithUserOrdering(A, ordering, message); 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingcholmod_factor* SuiteSparse::AnalyzeCholeskyWithUserOrdering( 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_sparse* A, 16579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const vector<int>& ordering, 16679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_EQ(ordering.size(), A->nrow); 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cc_.nmethods = 1; 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cc_.method[0].ordering = CHOLMOD_GIVEN; 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_factor* factor = 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_analyze_p(A, const_cast<int*>(&ordering[0]), NULL, 0, &cc_); 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (VLOG_IS_ON(2)) { 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_); 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 17779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (cc_.status != CHOLMOD_OK) { 17879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = StringPrintf("cholmod_analyze failed. error code: %d", 17979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cc_.status); 18079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return NULL; 18179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 18379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return CHECK_NOTNULL(factor); 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingcholmod_factor* SuiteSparse::AnalyzeCholeskyWithNaturalOrdering( 18779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_sparse* A, 18879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cc_.nmethods = 1; 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cc_.method[0].ordering = CHOLMOD_NATURAL; 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cc_.postorder = 0; 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_factor* factor = cholmod_analyze(A, &cc_); 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (VLOG_IS_ON(2)) { 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_print_common(const_cast<char*>("Symbolic Analysis"), &cc_); 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (cc_.status != CHOLMOD_OK) { 19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = StringPrintf("cholmod_analyze failed. error code: %d", 19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cc_.status); 20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return NULL; 20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return CHECK_NOTNULL(factor); 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool SuiteSparse::BlockAMDOrdering(const cholmod_sparse* A, 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& row_blocks, 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const vector<int>& col_blocks, 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int>* ordering) { 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_row_blocks = row_blocks.size(); 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_col_blocks = col_blocks.size(); 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Arrays storing the compressed column structure of the matrix 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // incoding the block sparsity of A. 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> block_cols; 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> block_rows; 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CompressedColumnScalarMatrixToBlockMatrix(reinterpret_cast<const int*>(A->i), 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling reinterpret_cast<const int*>(A->p), 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling row_blocks, 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling col_blocks, 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &block_rows, 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &block_cols); 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cholmod_sparse_struct block_matrix; 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.nrow = num_row_blocks; 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.ncol = num_col_blocks; 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.nzmax = block_rows.size(); 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.p = reinterpret_cast<void*>(&block_cols[0]); 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.i = reinterpret_cast<void*>(&block_rows[0]); 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.x = NULL; 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.stype = A->stype; 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.itype = CHOLMOD_INT; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.xtype = CHOLMOD_PATTERN; 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.dtype = CHOLMOD_DOUBLE; 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.sorted = 1; 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_matrix.packed = 1; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> block_ordering(num_row_blocks); 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!cholmod_amd(&block_matrix, NULL, 0, &block_ordering[0], &cc_)) { 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockOrderingToScalarOrdering(row_blocks, block_ordering, ordering); 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 24879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolverTerminationType SuiteSparse::Cholesky(cholmod_sparse* A, 24979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_factor* L, 25079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(A); 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NOTNULL(L); 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Save the current print level and silence CHOLMOD, otherwise 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // CHOLMOD is prone to dumping stuff to stderr, which can be 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // distracting when the error (matrix is indefinite) is not a fatal 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // failure. 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int old_print_level = cc_.print; 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cc_.print = 0; 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cc_.quick_return_if_not_posdef = 1; 26279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez int cholmod_status = cholmod_factorize(A, L, &cc_); 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cc_.print = old_print_level; 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // TODO(sameeragarwal): This switch statement is not consistent. It 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // treats all kinds of CHOLMOD failures as warnings. Some of these 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // like out of memory are definitely not warnings. The problem is 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // that the return value Cholesky is two valued, but the state of 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // the linear solver is really three valued. SUCCESS, 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // NON_FATAL_FAILURE (e.g., indefinite matrix) and FATAL_FAILURE 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // (e.g. out of memory). 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong switch (cc_.status) { 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_NOT_INSTALLED: 27479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD failure: Method not installed."; 27579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_OUT_OF_MEMORY: 27779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD failure: Out of memory."; 27879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_TOO_LARGE: 28079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD failure: Integer overflow occured."; 28179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_INVALID: 28379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD failure: Invalid input."; 28479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_NOT_POSDEF: 28679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD warning: Matrix not positive definite."; 28779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FAILURE; 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_DSMALL: 28979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD warning: D for LDL' or diag(L) or " 29079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "LL' has tiny absolute value."; 29179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FAILURE; 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CHOLMOD_OK: 29379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (cholmod_status != 0) { 29479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_SUCCESS; 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 29679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 29779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "CHOLMOD failure: cholmod_factorize returned false " 29879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "but cholmod_common::status is CHOLMOD_OK." 29979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Please report this to ceres-solver@googlegroups.com."; 30079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong default: 30279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = 30379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez StringPrintf("Unknown cholmod return code: %d. " 30479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Please report this to ceres-solver@googlegroups.com.", 30579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cc_.status); 30679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 30879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 30979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LINEAR_SOLVER_FATAL_ERROR; 3100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongcholmod_dense* SuiteSparse::Solve(cholmod_factor* L, 31379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_dense* b, 31479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez string* message) { 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cc_.status != CHOLMOD_OK) { 31679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez *message = "cholmod_solve failed. CHOLMOD status is not CHOLMOD_OK"; 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return NULL; 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return cholmod_solve(CHOLMOD_A, L, b, &cc_); 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 32379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool SuiteSparse::ApproximateMinimumDegreeOrdering(cholmod_sparse* matrix, 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* ordering) { 32579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return cholmod_amd(matrix, NULL, 0, ordering, &cc_); 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 32879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandezbool SuiteSparse::ConstrainedApproximateMinimumDegreeOrdering( 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_sparse* matrix, 3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* constraints, 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int* ordering) { 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifndef CERES_NO_CAMD 33379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return cholmod_camd(matrix, NULL, 0, constraints, ordering, &cc_); 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#else 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(FATAL) << "Congratulations you have found a bug in Ceres." 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Ceres Solver was compiled with SuiteSparse " 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "version 4.1.0 or less. Calling this function " 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "in that case is a bug. Please contact the" 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "the Ceres Solver developers."; 34079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return false; 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#endif 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_NO_SUITESPARSE 348