10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Copyright 2014 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#include "ceres/internal/port.h" 3279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <ctime> 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <set> 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <vector> 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_dense_matrix.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_matrix.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_random_access_sparse_matrix.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_structure.h" 43399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#include "ceres/cxsparse.h" 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/detect_structure.h" 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/eigen.h" 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/scoped_ptr.h" 47399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger#include "ceres/lapack.h" 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/linear_solver.h" 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/schur_complement_solver.h" 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/suitesparse.h" 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/triplet_sparse_matrix.h" 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/wall_time.h" 5479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "Eigen/Dense" 5579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "Eigen/SparseCore" 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongLinearSolver::Summary SchurComplementSolver::SolveImpl( 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling BlockSparseMatrix* A, 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b, 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LinearSolver::PerSolveOptions& per_solve_options, 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* x) { 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EventLogger event_logger("SchurComplementSolver::Solve"); 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (eliminator_.get() == NULL) { 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong InitStorage(A->block_structure()); 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong DetectStructure(*A->block_structure(), 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong options_.elimination_groups[0], 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong &options_.row_block_size, 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong &options_.e_block_size, 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong &options_.f_block_size); 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_->Init(options_.elimination_groups[0], A->block_structure()); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong fill(x, x + A->num_cols(), 0.0); 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling event_logger.AddEvent("Setup"); 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get()); 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling event_logger.AddEvent("Eliminate"); 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); 8479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const LinearSolver::Summary summary = 8579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez SolveReducedLinearSystem(reduced_solution); 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling event_logger.AddEvent("ReducedSolve"); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 8879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (summary.termination_type == LINEAR_SOLVER_SUCCESS) { 8979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x); 9079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez event_logger.AddEvent("BackSubstitute"); 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return summary; 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Initialize a BlockRandomAccessDenseMatrix to store the Schur 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// complement. 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid DenseSchurComplementSolver::InitStorage( 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs) { 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_eliminate_blocks = options().elimination_groups[0]; 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_col_blocks = bs->cols.size(); 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = num_eliminate_blocks, j = 0; 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong i < num_col_blocks; 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ++i, ++j) { 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong blocks[j] = bs->cols[i].size; 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_lhs(new BlockRandomAccessDenseMatrix(blocks)); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_rhs(new double[lhs()->num_rows()]); 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Solve the system Sx = r, assuming that the matrix S is stored in a 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// BlockRandomAccessDenseMatrix. The linear system is solved using 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Eigen's Cholesky factorization. 11779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolver::Summary 11879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezDenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { 11979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 12079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 12179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_SUCCESS; 12279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Success."; 12379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const BlockRandomAccessDenseMatrix* m = 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong down_cast<const BlockRandomAccessDenseMatrix*>(lhs()); 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = m->num_rows(); 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The case where there are no f blocks, and the system is block 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // diagonal. 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (num_rows == 0) { 13179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 13479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 1; 13579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 136399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger if (options().dense_linear_algebra_library_type == EIGEN) { 13779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Eigen::LLT<Matrix, Eigen::Upper> llt = 138399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger ConstMatrixRef(m->values(), num_rows, num_rows) 139399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger .selfadjointView<Eigen::Upper>() 14079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez .llt(); 14179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (llt.info() != Eigen::Success) { 14279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FAILURE; 14379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = 14479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Eigen failure. Unable to perform dense Cholesky factorization."; 14579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 14679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 14779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 14879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows)); 14979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else { 15079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); 15179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = 15279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LAPACK::SolveInPlaceUsingCholesky(num_rows, 15379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez m->values(), 15479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez solution, 15579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &summary.message); 156399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger } 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 15879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongSparseSchurComplementSolver::SparseSchurComplementSolver( 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const LinearSolver::Options& options) 163399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger : SchurComplementSolver(options), 164399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger factor_(NULL), 165399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger cxsparse_factor_(NULL) { 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongSparseSchurComplementSolver::~SparseSchurComplementSolver() { 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (factor_ != NULL) { 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(factor_); 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong factor_ = NULL; 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cxsparse_factor_ != NULL) { 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cxsparse_.Free(cxsparse_factor_); 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cxsparse_factor_ = NULL; 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Determine the non-zero blocks in the Schur Complement matrix, and 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// initialize a BlockRandomAccessSparseMatrix object. 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid SparseSchurComplementSolver::InitStorage( 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* bs) { 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_eliminate_blocks = options().elimination_groups[0]; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_col_blocks = bs->cols.size(); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_row_blocks = bs->rows.size(); 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong blocks_.resize(num_col_blocks - num_eliminate_blocks, 0); 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) { 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong blocks_[i - num_eliminate_blocks] = bs->cols[i].size; 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set<pair<int, int> > block_pairs; 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < blocks_.size(); ++i) { 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs.insert(make_pair(i, i)); 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int r = 0; 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong while (r < num_row_blocks) { 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int e_block_id = bs->rows[r].cells.front().block_id; 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (e_block_id >= num_eliminate_blocks) { 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong vector<int> f_blocks; 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Add to the chunk until the first block in the row is 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // different than the one in the first row for the chunk. 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (; r < num_row_blocks; ++r) { 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRow& row = bs->rows[r]; 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (row.cells.front().block_id != e_block_id) { 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong break; 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Iterate over the blocks in the row, ignoring the first 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // block since it is the one to be eliminated. 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int c = 1; c < row.cells.size(); ++c) { 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Cell& cell = row.cells[c]; 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong f_blocks.push_back(cell.block_id - num_eliminate_blocks); 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong sort(f_blocks.begin(), f_blocks.end()); 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end()); 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < f_blocks.size(); ++i) { 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = i + 1; j < f_blocks.size(); ++j) { 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs.insert(make_pair(f_blocks[i], f_blocks[j])); 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Remaing rows do not contribute to the chunks and directly go 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // into the schur complement via an outer product. 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (; r < num_row_blocks; ++r) { 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRow& row = bs->rows[r]; 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < row.cells.size(); ++i) { 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int r_block1_id = row.cells[i].block_id - num_eliminate_blocks; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int j = 0; j < row.cells.size(); ++j) { 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong int r_block2_id = row.cells[j].block_id - num_eliminate_blocks; 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (r_block1_id <= r_block2_id) { 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_pairs.insert(make_pair(r_block1_id, r_block2_id)); 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_lhs(new BlockRandomAccessSparseMatrix(blocks_, block_pairs)); 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong set_rhs(new double[lhs()->num_rows()]); 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 25179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolver::Summary 25279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezSparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { 253399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger switch (options().sparse_linear_algebra_library_type) { 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case SUITE_SPARSE: 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return SolveReducedLinearSystemUsingSuiteSparse(solution); 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong case CX_SPARSE: 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return SolveReducedLinearSystemUsingCXSparse(solution); 25879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez case EIGEN_SPARSE: 25979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return SolveReducedLinearSystemUsingEigen(solution); 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong default: 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong LOG(FATAL) << "Unknown sparse linear algebra library : " 262399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger << options().sparse_linear_algebra_library_type; 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 26579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return LinearSolver::Summary(); 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Solve the system Sx = r, assuming that the matrix S is stored in a 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// BlockRandomAccessSparseMatrix. The linear system is solved using 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CHOLMOD's sparse cholesky factorization routines. 27179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolver::Summary 27279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezSparseSchurComplementSolver::SolveReducedLinearSystemUsingSuiteSparse( 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* solution) { 27479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#ifdef CERES_NO_SUITESPARSE 27579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 27679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 27779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 27879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; 27979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Ceres was not built with SuiteSparse support. " 28079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Therefore, SPARSE_SCHUR cannot be used with SUITE_SPARSE"; 28179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 28279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 28379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#else 28479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 28579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 28679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 28779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_SUCCESS; 28879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Success."; 28979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* tsm = 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const_cast<TripletSparseMatrix*>( 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = tsm->num_rows(); 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The case where there are no f blocks, and the system is block 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // diagonal. 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (num_rows == 0) { 29879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 30179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 1; 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_sparse* cholmod_lhs = NULL; 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (options().use_postordering) { 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If we are going to do a full symbolic analysis of the schur 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // complement matrix from scratch and not rely on the 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // pre-ordering, then the fastest path in cholmod_factorize is the 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // one corresponding to upper triangular matrices. 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Create a upper triangular symmetric matrix. 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_lhs = ss_.CreateSparseMatrix(tsm); 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_lhs->stype = 1; 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (factor_ == NULL) { 31479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez factor_ = ss_.BlockAnalyzeCholesky(cholmod_lhs, 31579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez blocks_, 31679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez blocks_, 31779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &summary.message); 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // If we are going to use the natural ordering (i.e. rely on the 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // pre-ordering computed by solver_impl.cc), then the fastest 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // path in cholmod_factorize is the one corresponding to lower 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // triangular matrices. 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Create a upper triangular symmetric matrix. 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_lhs = ss_.CreateSparseMatrixTranspose(tsm); 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling cholmod_lhs->stype = -1; 3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (factor_ == NULL) { 33079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(cholmod_lhs, 33179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &summary.message); 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 33579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (factor_ == NULL) { 33679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ss_.Free(cholmod_lhs); 33779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; 33879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // No need to set message as it has already been set by the 33979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // symbolic analysis routines above. 34079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 34179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 34279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 34379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = 34479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ss_.Cholesky(cholmod_lhs, factor_, &summary.message); 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(cholmod_lhs); 34779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 34879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (summary.termination_type != LINEAR_SOLVER_SUCCESS) { 34979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // No need to set message as it has already been set by the 35079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // numeric factorization routine above. 35179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 35279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 35379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 35479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_dense* cholmod_rhs = 35579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); 35679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_dense* cholmod_solution = ss_.Solve(factor_, 35779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cholmod_rhs, 35879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez &summary.message); 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(cholmod_rhs); 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cholmod_solution == NULL) { 36279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = 36379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "SuiteSparse failure. Unable to perform triangular solve."; 36479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FAILURE; 36579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 3660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(solution, num_rows) 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ss_.Free(cholmod_solution); 37179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_NO_SUITESPARSE 37379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez} 3740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Solve the system Sx = r, assuming that the matrix S is stored in a 3760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// BlockRandomAccessSparseMatrix. The linear system is solved using 3770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CXSparse's sparse cholesky factorization routines. 37879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolver::Summary 37979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezSparseSchurComplementSolver::SolveReducedLinearSystemUsingCXSparse( 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* solution) { 38179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#ifdef CERES_NO_CXSPARSE 38279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 38379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 38479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 38579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; 38679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Ceres was not built with CXSparse support. " 38779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Therefore, SPARSE_SCHUR cannot be used with CX_SPARSE"; 38879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 38979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 39079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#else 39179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 39279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 39379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 39479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_SUCCESS; 39579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Success."; 39679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Extract the TripletSparseMatrix that is used for actually storing S. 3980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong TripletSparseMatrix* tsm = 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const_cast<TripletSparseMatrix*>( 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = tsm->num_rows(); 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The case where there are no f blocks, and the system is block 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // diagonal. 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (num_rows == 0) { 40679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 4070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cs_di* lhs = CHECK_NOTNULL(cxsparse_.CreateSparseMatrix(tsm)); 4100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); 4110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute symbolic factorization if not available. 4130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (cxsparse_factor_ == NULL) { 41479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(lhs, blocks_, blocks_); 4150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 41779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (cxsparse_factor_ == NULL) { 41879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; 41979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = 42079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "CXSparse failure. Unable to find symbolic factorization."; 42179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } else if (!cxsparse_.SolveCholesky(lhs, cxsparse_factor_, solution)) { 42279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FAILURE; 42379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "CXSparse::SolveCholesky failed."; 42479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 4250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong cxsparse_.Free(lhs); 42779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 42879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#endif // CERES_NO_CXPARSE 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 43079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 43179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Solve the system Sx = r, assuming that the matrix S is stored in a 43279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// BlockRandomAccessSparseMatrix. The linear system is solved using 43379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// Eigen's sparse cholesky factorization routines. 43479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezLinearSolver::Summary 43579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezSparseSchurComplementSolver::SolveReducedLinearSystemUsingEigen( 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong double* solution) { 43779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#ifndef CERES_USE_EIGEN_SPARSE 43879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 43979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 44079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 44179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; 44279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = 44379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "SPARSE_SCHUR cannot be used with EIGEN_SPARSE. " 44479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Ceres was not built with support for " 44579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Eigen's SimplicialLDLT decomposition. " 44679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "This requires enabling building with -DEIGENSPARSE=ON."; 44779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 44879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 44979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#else 45079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez EventLogger event_logger("SchurComplementSolver::EigenSolve"); 45179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez LinearSolver::Summary summary; 45279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.num_iterations = 0; 45379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_SUCCESS; 45479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Success."; 45579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 45679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Extract the TripletSparseMatrix that is used for actually storing S. 45779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez TripletSparseMatrix* tsm = 45879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const_cast<TripletSparseMatrix*>( 45979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); 46079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const int num_rows = tsm->num_rows(); 46179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 46279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The case where there are no f blocks, and the system is block 46379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // diagonal. 46479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (num_rows == 0) { 46579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 46679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 46779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 46879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This is an upper triangular matrix. 46979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez CompressedRowSparseMatrix crsm(*tsm); 47079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Map this to a column major, lower triangular matrix. 47179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Eigen::MappedSparseMatrix<double, Eigen::ColMajor> eigen_lhs( 47279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm.num_rows(), 47379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm.num_rows(), 47479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm.num_nonzeros(), 47579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm.mutable_rows(), 47679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm.mutable_cols(), 47779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez crsm.mutable_values()); 47879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez event_logger.AddEvent("ToCompressedRowSparseMatrix"); 47979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 48079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Compute symbolic factorization if one does not exist. 48179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (simplicial_ldlt_.get() == NULL) { 48279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez simplicial_ldlt_.reset(new SimplicialLDLT); 48379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // This ordering is quite bad. The scalar ordering produced by the 48479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // AMD algorithm is quite bad and can be an order of magnitude 48579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // worse than the one computed using the block version of the 48679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // algorithm. 48779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez simplicial_ldlt_->analyzePattern(eigen_lhs); 48879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez event_logger.AddEvent("Analysis"); 48979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (simplicial_ldlt_->info() != Eigen::Success) { 49079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; 49179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = 49279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez "Eigen failure. Unable to find symbolic factorization."; 49379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 49479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 49579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 49679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 49779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez simplicial_ldlt_->factorize(eigen_lhs); 49879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez event_logger.AddEvent("Factorize"); 49979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (simplicial_ldlt_->info() != Eigen::Success) { 50079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FAILURE; 50179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Eigen failure. Unable to find numeric factoriztion."; 50279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 50379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 50479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 50579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez VectorRef(solution, num_rows) = 50679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez simplicial_ldlt_->solve(ConstVectorRef(rhs(), num_rows)); 50779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez event_logger.AddEvent("Solve"); 50879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (simplicial_ldlt_->info() != Eigen::Success) { 50979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.termination_type = LINEAR_SOLVER_FAILURE; 51079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez summary.message = "Eigen failure. Unable to do triangular solve."; 51179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 51279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 51379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return summary; 51479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#endif // CERES_USE_EIGEN_SPARSE 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 5180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 519