| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2015 Google Inc. All rights reserved.// http://ceres-solver.org///// Redistribution and use in source and binary forms, with or without// modification, are permitted provided that the following conditions are met://// * Redistributions of source code must retain the above copyright notice,//   this list of conditions and the following disclaimer.// * Redistributions in binary form must reproduce the above copyright notice,//   this list of conditions and the following disclaimer in the documentation//   and/or other materials provided with the distribution.// * Neither the name of Google Inc. nor the names of its contributors may be//   used to endorse or promote products derived from this software without//   specific prior written permission.//// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// POSSIBILITY OF SUCH DAMAGE.//// Author: sameeragarwal@google.com (Sameer Agarwal)#include "ceres/iterative_schur_complement_solver.h"#include <algorithm>#include <cstring>#include <vector>#include "Eigen/Dense"#include "ceres/block_sparse_matrix.h"#include "ceres/block_structure.h"#include "ceres/conjugate_gradients_solver.h"#include "ceres/detect_structure.h"#include "ceres/implicit_schur_complement.h"#include "ceres/internal/eigen.h"#include "ceres/linear_solver.h"#include "ceres/preconditioner.h"#include "ceres/schur_jacobi_preconditioner.h"#include "ceres/triplet_sparse_matrix.h"#include "ceres/types.h"#include "ceres/visibility_based_preconditioner.h"#include "ceres/wall_time.h"#include "glog/logging.h"namespace ceres {namespace internal {IterativeSchurComplementSolver::IterativeSchurComplementSolver(    const LinearSolver::Options& options)    : options_(options) {}IterativeSchurComplementSolver::~IterativeSchurComplementSolver() {}LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl(    BlockSparseMatrix* A,    const double* b,    const LinearSolver::PerSolveOptions& per_solve_options,    double* x) {  EventLogger event_logger("IterativeSchurComplementSolver::Solve");  CHECK(A->block_structure() != nullptr);  const int num_eliminate_blocks = options_.elimination_groups[0];  // Initialize a ImplicitSchurComplement object.  if (schur_complement_ == NULL) {    DetectStructure(*(A->block_structure()),                    num_eliminate_blocks,                    &options_.row_block_size,                    &options_.e_block_size,                    &options_.f_block_size);    schur_complement_.reset(new ImplicitSchurComplement(options_));  }  schur_complement_->Init(*A, per_solve_options.D, b);  const int num_schur_complement_blocks =      A->block_structure()->cols.size() - num_eliminate_blocks;  if (num_schur_complement_blocks == 0) {    VLOG(2) << "No parameter blocks left in the schur complement.";    LinearSolver::Summary summary;    summary.num_iterations = 0;    summary.termination_type = LINEAR_SOLVER_SUCCESS;    schur_complement_->BackSubstitute(NULL, x);    return summary;  }  // Initialize the solution to the Schur complement system to zero.  reduced_linear_system_solution_.resize(schur_complement_->num_rows());  reduced_linear_system_solution_.setZero();  LinearSolver::Options cg_options;  cg_options.min_num_iterations = options_.min_num_iterations;  cg_options.max_num_iterations = options_.max_num_iterations;  ConjugateGradientsSolver cg_solver(cg_options);  LinearSolver::PerSolveOptions cg_per_solve_options;  cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance;  cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance;  CreatePreconditioner(A);  if (preconditioner_.get() != NULL) {    if (!preconditioner_->Update(*A, per_solve_options.D)) {      LinearSolver::Summary summary;      summary.num_iterations = 0;      summary.termination_type = LINEAR_SOLVER_FAILURE;      summary.message = "Preconditioner update failed.";      return summary;    }    cg_per_solve_options.preconditioner = preconditioner_.get();  }  event_logger.AddEvent("Setup");  LinearSolver::Summary summary =      cg_solver.Solve(schur_complement_.get(),                      schur_complement_->rhs().data(),                      cg_per_solve_options,                      reduced_linear_system_solution_.data());  if (summary.termination_type != LINEAR_SOLVER_FAILURE &&      summary.termination_type != LINEAR_SOLVER_FATAL_ERROR) {    schur_complement_->BackSubstitute(reduced_linear_system_solution_.data(),                                      x);  }  event_logger.AddEvent("Solve");  return summary;}void IterativeSchurComplementSolver::CreatePreconditioner(    BlockSparseMatrix* A) {  if (options_.preconditioner_type == IDENTITY ||      preconditioner_.get() != NULL) {    return;  }  Preconditioner::Options preconditioner_options;  preconditioner_options.type = options_.preconditioner_type;  preconditioner_options.visibility_clustering_type =      options_.visibility_clustering_type;  preconditioner_options.sparse_linear_algebra_library_type =      options_.sparse_linear_algebra_library_type;  preconditioner_options.num_threads = options_.num_threads;  preconditioner_options.row_block_size = options_.row_block_size;  preconditioner_options.e_block_size = options_.e_block_size;  preconditioner_options.f_block_size = options_.f_block_size;  preconditioner_options.elimination_groups = options_.elimination_groups;  CHECK(options_.context != NULL);  preconditioner_options.context = options_.context;  switch (options_.preconditioner_type) {    case JACOBI:      preconditioner_.reset(new SparseMatrixPreconditionerWrapper(          schur_complement_->block_diagonal_FtF_inverse()));      break;    case SCHUR_JACOBI:      preconditioner_.reset(new SchurJacobiPreconditioner(          *A->block_structure(), preconditioner_options));      break;    case CLUSTER_JACOBI:    case CLUSTER_TRIDIAGONAL:      preconditioner_.reset(new VisibilityBasedPreconditioner(          *A->block_structure(), preconditioner_options));      break;    default:      LOG(FATAL) << "Unknown Preconditioner Type";  }};}  // namespace internal}  // namespace ceres
 |