1// Ceres Solver - A fast non-linear least squares minimizer 2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3// http://code.google.com/p/ceres-solver/ 4// 5// Redistribution and use in source and binary forms, with or without 6// modification, are permitted provided that the following conditions are met: 7// 8// * Redistributions of source code must retain the above copyright notice, 9// this list of conditions and the following disclaimer. 10// * Redistributions in binary form must reproduce the above copyright notice, 11// this list of conditions and the following disclaimer in the documentation 12// and/or other materials provided with the distribution. 13// * Neither the name of Google Inc. nor the names of its contributors may be 14// used to endorse or promote products derived from this software without 15// specific prior written permission. 16// 17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27// POSSIBILITY OF SUCH DAMAGE. 28// 29// Author: sameeragarwal@google.com (Sameer Agarwal) 30 31#include "ceres/implicit_schur_complement.h" 32 33#include "Eigen/Dense" 34#include "ceres/block_sparse_matrix.h" 35#include "ceres/block_structure.h" 36#include "ceres/internal/eigen.h" 37#include "ceres/internal/scoped_ptr.h" 38#include "ceres/linear_solver.h" 39#include "ceres/types.h" 40#include "glog/logging.h" 41 42namespace ceres { 43namespace internal { 44 45ImplicitSchurComplement::ImplicitSchurComplement( 46 const LinearSolver::Options& options) 47 : options_(options), 48 D_(NULL), 49 b_(NULL) { 50} 51 52ImplicitSchurComplement::~ImplicitSchurComplement() { 53} 54 55void ImplicitSchurComplement::Init(const BlockSparseMatrix& A, 56 const double* D, 57 const double* b) { 58 // Since initialization is reasonably heavy, perhaps we can save on 59 // constructing a new object everytime. 60 if (A_ == NULL) { 61 A_.reset(PartitionedMatrixViewBase::Create(options_, A)); 62 } 63 64 D_ = D; 65 b_ = b; 66 67 // Initialize temporary storage and compute the block diagonals of 68 // E'E and F'E. 69 if (block_diagonal_EtE_inverse_ == NULL) { 70 block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); 71 if (options_.preconditioner_type == JACOBI) { 72 block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); 73 } 74 rhs_.resize(A_->num_cols_f()); 75 rhs_.setZero(); 76 tmp_rows_.resize(A_->num_rows()); 77 tmp_e_cols_.resize(A_->num_cols_e()); 78 tmp_e_cols_2_.resize(A_->num_cols_e()); 79 tmp_f_cols_.resize(A_->num_cols_f()); 80 } else { 81 A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); 82 if (options_.preconditioner_type == JACOBI) { 83 A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); 84 } 85 } 86 87 // The block diagonals of the augmented linear system contain 88 // contributions from the diagonal D if it is non-null. Add that to 89 // the block diagonals and invert them. 90 AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); 91 if (options_.preconditioner_type == JACOBI) { 92 AddDiagonalAndInvert((D_ == NULL) ? NULL : D_ + A_->num_cols_e(), 93 block_diagonal_FtF_inverse_.get()); 94 } 95 96 // Compute the RHS of the Schur complement system. 97 UpdateRhs(); 98} 99 100// Evaluate the product 101// 102// Sx = [F'F - F'E (E'E)^-1 E'F]x 103// 104// By breaking it down into individual matrix vector products 105// involving the matrices E and F. This is implemented using a 106// PartitionedMatrixView of the input matrix A. 107void ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { 108 // y1 = F x 109 tmp_rows_.setZero(); 110 A_->RightMultiplyF(x, tmp_rows_.data()); 111 112 // y2 = E' y1 113 tmp_e_cols_.setZero(); 114 A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); 115 116 // y3 = -(E'E)^-1 y2 117 tmp_e_cols_2_.setZero(); 118 block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), 119 tmp_e_cols_2_.data()); 120 tmp_e_cols_2_ *= -1.0; 121 122 // y1 = y1 + E y3 123 A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); 124 125 // y5 = D * x 126 if (D_ != NULL) { 127 ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); 128 VectorRef(y, num_cols()) = 129 (Dref.array().square() * 130 ConstVectorRef(x, num_cols()).array()).matrix(); 131 } else { 132 VectorRef(y, num_cols()).setZero(); 133 } 134 135 // y = y5 + F' y1 136 A_->LeftMultiplyF(tmp_rows_.data(), y); 137} 138 139// Given a block diagonal matrix and an optional array of diagonal 140// entries D, add them to the diagonal of the matrix and compute the 141// inverse of each diagonal block. 142void ImplicitSchurComplement::AddDiagonalAndInvert( 143 const double* D, 144 BlockSparseMatrix* block_diagonal) { 145 const CompressedRowBlockStructure* block_diagonal_structure = 146 block_diagonal->block_structure(); 147 for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { 148 const int row_block_pos = block_diagonal_structure->rows[r].block.position; 149 const int row_block_size = block_diagonal_structure->rows[r].block.size; 150 const Cell& cell = block_diagonal_structure->rows[r].cells[0]; 151 MatrixRef m(block_diagonal->mutable_values() + cell.position, 152 row_block_size, row_block_size); 153 154 if (D != NULL) { 155 ConstVectorRef d(D + row_block_pos, row_block_size); 156 m += d.array().square().matrix().asDiagonal(); 157 } 158 159 m = m 160 .selfadjointView<Eigen::Upper>() 161 .llt() 162 .solve(Matrix::Identity(row_block_size, row_block_size)); 163 } 164} 165 166// Similar to RightMultiply, use the block structure of the matrix A 167// to compute y = (E'E)^-1 (E'b - E'F x). 168void ImplicitSchurComplement::BackSubstitute(const double* x, double* y) { 169 const int num_cols_e = A_->num_cols_e(); 170 const int num_cols_f = A_->num_cols_f(); 171 const int num_cols = A_->num_cols(); 172 const int num_rows = A_->num_rows(); 173 174 // y1 = F x 175 tmp_rows_.setZero(); 176 A_->RightMultiplyF(x, tmp_rows_.data()); 177 178 // y2 = b - y1 179 tmp_rows_ = ConstVectorRef(b_, num_rows) - tmp_rows_; 180 181 // y3 = E' y2 182 tmp_e_cols_.setZero(); 183 A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); 184 185 // y = (E'E)^-1 y3 186 VectorRef(y, num_cols).setZero(); 187 block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y); 188 189 // The full solution vector y has two blocks. The first block of 190 // variables corresponds to the eliminated variables, which we just 191 // computed via back substitution. The second block of variables 192 // corresponds to the Schur complement system, so we just copy those 193 // values from the solution to the Schur complement. 194 VectorRef(y + num_cols_e, num_cols_f) = ConstVectorRef(x, num_cols_f); 195} 196 197// Compute the RHS of the Schur complement system. 198// 199// rhs = F'b - F'E (E'E)^-1 E'b 200// 201// Like BackSubstitute, we use the block structure of A to implement 202// this using a series of matrix vector products. 203void ImplicitSchurComplement::UpdateRhs() { 204 // y1 = E'b 205 tmp_e_cols_.setZero(); 206 A_->LeftMultiplyE(b_, tmp_e_cols_.data()); 207 208 // y2 = (E'E)^-1 y1 209 Vector y2 = Vector::Zero(A_->num_cols_e()); 210 block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y2.data()); 211 212 // y3 = E y2 213 tmp_rows_.setZero(); 214 A_->RightMultiplyE(y2.data(), tmp_rows_.data()); 215 216 // y3 = b - y3 217 tmp_rows_ = ConstVectorRef(b_, A_->num_rows()) - tmp_rows_; 218 219 // rhs = F' y3 220 rhs_.setZero(); 221 A_->LeftMultiplyF(tmp_rows_.data(), rhs_.data()); 222} 223 224} // namespace internal 225} // namespace ceres 226