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 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/implicit_schur_complement.h" 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Dense" 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_sparse_matrix.h" 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/block_structure.h" 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/eigen.h" 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/internal/scoped_ptr.h" 3879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez#include "ceres/linear_solver.h" 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/types.h" 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace internal { 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos HernandezImplicitSchurComplement::ImplicitSchurComplement( 4679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const LinearSolver::Options& options) 4779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : options_(options), 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong D_(NULL), 4979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez b_(NULL) { 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongImplicitSchurComplement::~ImplicitSchurComplement() { 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid ImplicitSchurComplement::Init(const BlockSparseMatrix& A, 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* D, 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* b) { 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Since initialization is reasonably heavy, perhaps we can save on 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // constructing a new object everytime. 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (A_ == NULL) { 6179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez A_.reset(PartitionedMatrixViewBase::Create(options_, A)); 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong D_ = D; 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong b_ = b; 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Initialize temporary storage and compute the block diagonals of 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // E'E and F'E. 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (block_diagonal_EtE_inverse_ == NULL) { 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); 7179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.preconditioner_type == JACOBI) { 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rhs_.resize(A_->num_cols_f()); 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rhs_.setZero(); 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rows_.resize(A_->num_rows()); 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_.resize(A_->num_cols_e()); 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_2_.resize(A_->num_cols_e()); 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_f_cols_.resize(A_->num_cols_f()); 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); 8279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.preconditioner_type == JACOBI) { 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The block diagonals of the augmented linear system contain 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // contributions from the diagonal D if it is non-null. Add that to 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the block diagonals and invert them. 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); 9179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (options_.preconditioner_type == JACOBI) { 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AddDiagonalAndInvert((D_ == NULL) ? NULL : D_ + A_->num_cols_e(), 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_FtF_inverse_.get()); 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compute the RHS of the Schur complement system. 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong UpdateRhs(); 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Evaluate the product 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Sx = [F'F - F'E (E'E)^-1 E'F]x 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// By breaking it down into individual matrix vector products 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// involving the matrices E and F. This is implemented using a 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// PartitionedMatrixView of the input matrix A. 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y1 = F x 1090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rows_.setZero(); 1100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->RightMultiplyF(x, tmp_rows_.data()); 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y2 = E' y1 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_.setZero(); 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y3 = -(E'E)^-1 y2 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_2_.setZero(); 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_2_.data()); 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_2_ *= -1.0; 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y1 = y1 + E y3 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y5 = D * x 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (D_ != NULL) { 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(y, num_cols()) = 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong (Dref.array().square() * 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstVectorRef(x, num_cols()).array()).matrix(); 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(y, num_cols()).setZero(); 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y = y5 + F' y1 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->LeftMultiplyF(tmp_rows_.data(), y); 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Given a block diagonal matrix and an optional array of diagonal 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// entries D, add them to the diagonal of the matrix and compute the 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// inverse of each diagonal block. 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ImplicitSchurComplement::AddDiagonalAndInvert( 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double* D, 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong BlockSparseMatrix* block_diagonal) { 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const CompressedRowBlockStructure* block_diagonal_structure = 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal->block_structure(); 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_pos = block_diagonal_structure->rows[r].block.position; 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int row_block_size = block_diagonal_structure->rows[r].block.size; 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Cell& cell = block_diagonal_structure->rows[r].cells[0]; 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MatrixRef m(block_diagonal->mutable_values() + cell.position, 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong row_block_size, row_block_size); 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (D != NULL) { 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ConstVectorRef d(D + row_block_pos, row_block_size); 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m += d.array().square().matrix().asDiagonal(); 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong m = m 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong .selfadjointView<Eigen::Upper>() 161399f7d09e0c45af54b77b4ab9508d6f23759b927Scott Ettinger .llt() 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong .solve(Matrix::Identity(row_block_size, row_block_size)); 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Similar to RightMultiply, use the block structure of the matrix A 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to compute y = (E'E)^-1 (E'b - E'F x). 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ImplicitSchurComplement::BackSubstitute(const double* x, double* y) { 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_cols_e = A_->num_cols_e(); 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_cols_f = A_->num_cols_f(); 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_cols = A_->num_cols(); 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const int num_rows = A_->num_rows(); 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y1 = F x 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rows_.setZero(); 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->RightMultiplyF(x, tmp_rows_.data()); 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y2 = b - y1 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rows_ = ConstVectorRef(b_, num_rows) - tmp_rows_; 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y3 = E' y2 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_.setZero(); 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y = (E'E)^-1 y3 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(y, num_cols).setZero(); 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y); 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The full solution vector y has two blocks. The first block of 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // variables corresponds to the eliminated variables, which we just 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // computed via back substitution. The second block of variables 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // corresponds to the Schur complement system, so we just copy those 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // values from the solution to the Schur complement. 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong VectorRef(y + num_cols_e, num_cols_f) = ConstVectorRef(x, num_cols_f); 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Compute the RHS of the Schur complement system. 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// rhs = F'b - F'E (E'E)^-1 E'b 2000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 2010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Like BackSubstitute, we use the block structure of A to implement 2020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this using a series of matrix vector products. 2030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid ImplicitSchurComplement::UpdateRhs() { 2040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y1 = E'b 2050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_e_cols_.setZero(); 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->LeftMultiplyE(b_, tmp_e_cols_.data()); 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y2 = (E'E)^-1 y1 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Vector y2 = Vector::Zero(A_->num_cols_e()); 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y2.data()); 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y3 = E y2 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rows_.setZero(); 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->RightMultiplyE(y2.data(), tmp_rows_.data()); 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // y3 = b - y3 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong tmp_rows_ = ConstVectorRef(b_, A_->num_rows()) - tmp_rows_; 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // rhs = F' y3 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong rhs_.setZero(); 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong A_->LeftMultiplyF(tmp_rows_.data(), rhs_.data()); 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace internal 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 226