| 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
 
 
  |