iterative_schur_complement_solver.cc 7.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200
  1. // Ceres Solver - A fast non-linear least squares minimizer
  2. // Copyright 2023 Google Inc. All rights reserved.
  3. // http://ceres-solver.org/
  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. #include "ceres/iterative_schur_complement_solver.h"
  31. #include <algorithm>
  32. #include <cstring>
  33. #include <utility>
  34. #include <vector>
  35. #include "Eigen/Dense"
  36. #include "ceres/block_sparse_matrix.h"
  37. #include "ceres/block_structure.h"
  38. #include "ceres/conjugate_gradients_solver.h"
  39. #include "ceres/detect_structure.h"
  40. #include "ceres/implicit_schur_complement.h"
  41. #include "ceres/internal/eigen.h"
  42. #include "ceres/linear_solver.h"
  43. #include "ceres/power_series_expansion_preconditioner.h"
  44. #include "ceres/preconditioner.h"
  45. #include "ceres/schur_jacobi_preconditioner.h"
  46. #include "ceres/triplet_sparse_matrix.h"
  47. #include "ceres/types.h"
  48. #include "ceres/visibility_based_preconditioner.h"
  49. #include "ceres/wall_time.h"
  50. #include "glog/logging.h"
  51. namespace ceres::internal {
  52. IterativeSchurComplementSolver::IterativeSchurComplementSolver(
  53. LinearSolver::Options options)
  54. : options_(std::move(options)) {}
  55. IterativeSchurComplementSolver::~IterativeSchurComplementSolver() = default;
  56. LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl(
  57. BlockSparseMatrix* A,
  58. const double* b,
  59. const LinearSolver::PerSolveOptions& per_solve_options,
  60. double* x) {
  61. EventLogger event_logger("IterativeSchurComplementSolver::Solve");
  62. CHECK(A->block_structure() != nullptr);
  63. CHECK(A->transpose_block_structure() != nullptr);
  64. const int num_eliminate_blocks = options_.elimination_groups[0];
  65. // Initialize a ImplicitSchurComplement object.
  66. if (schur_complement_ == nullptr) {
  67. DetectStructure(*(A->block_structure()),
  68. num_eliminate_blocks,
  69. &options_.row_block_size,
  70. &options_.e_block_size,
  71. &options_.f_block_size);
  72. schur_complement_ = std::make_unique<ImplicitSchurComplement>(options_);
  73. }
  74. schur_complement_->Init(*A, per_solve_options.D, b);
  75. const int num_schur_complement_blocks =
  76. A->block_structure()->cols.size() - num_eliminate_blocks;
  77. if (num_schur_complement_blocks == 0) {
  78. VLOG(2) << "No parameter blocks left in the schur complement.";
  79. LinearSolver::Summary summary;
  80. summary.num_iterations = 0;
  81. summary.termination_type = LinearSolverTerminationType::SUCCESS;
  82. schur_complement_->BackSubstitute(nullptr, x);
  83. return summary;
  84. }
  85. // Initialize the solution to the Schur complement system.
  86. reduced_linear_system_solution_.resize(schur_complement_->num_rows());
  87. reduced_linear_system_solution_.setZero();
  88. if (options_.use_spse_initialization) {
  89. Preconditioner::Options preconditioner_options(options_);
  90. preconditioner_options.type = SCHUR_POWER_SERIES_EXPANSION;
  91. PowerSeriesExpansionPreconditioner pse_solver(
  92. schur_complement_.get(),
  93. options_.max_num_spse_iterations,
  94. options_.spse_tolerance,
  95. preconditioner_options);
  96. pse_solver.RightMultiplyAndAccumulate(
  97. schur_complement_->rhs().data(),
  98. reduced_linear_system_solution_.data());
  99. }
  100. CreatePreconditioner(A);
  101. if (preconditioner_ != nullptr) {
  102. if (!preconditioner_->Update(*A, per_solve_options.D)) {
  103. LinearSolver::Summary summary;
  104. summary.num_iterations = 0;
  105. summary.termination_type = LinearSolverTerminationType::FAILURE;
  106. summary.message = "Preconditioner update failed.";
  107. return summary;
  108. }
  109. }
  110. ConjugateGradientsSolverOptions cg_options;
  111. cg_options.min_num_iterations = options_.min_num_iterations;
  112. cg_options.max_num_iterations = options_.max_num_iterations;
  113. cg_options.residual_reset_period = options_.residual_reset_period;
  114. cg_options.q_tolerance = per_solve_options.q_tolerance;
  115. cg_options.r_tolerance = per_solve_options.r_tolerance;
  116. LinearOperatorAdapter lhs(*schur_complement_);
  117. LinearOperatorAdapter preconditioner(*preconditioner_);
  118. Vector scratch[4];
  119. for (int i = 0; i < 4; ++i) {
  120. scratch[i].resize(schur_complement_->num_cols());
  121. }
  122. Vector* scratch_ptr[4] = {&scratch[0], &scratch[1], &scratch[2], &scratch[3]};
  123. event_logger.AddEvent("Setup");
  124. LinearSolver::Summary summary =
  125. ConjugateGradientsSolver(cg_options,
  126. lhs,
  127. schur_complement_->rhs(),
  128. preconditioner,
  129. scratch_ptr,
  130. reduced_linear_system_solution_);
  131. if (summary.termination_type != LinearSolverTerminationType::FAILURE &&
  132. summary.termination_type != LinearSolverTerminationType::FATAL_ERROR) {
  133. schur_complement_->BackSubstitute(reduced_linear_system_solution_.data(),
  134. x);
  135. }
  136. event_logger.AddEvent("Solve");
  137. return summary;
  138. }
  139. void IterativeSchurComplementSolver::CreatePreconditioner(
  140. BlockSparseMatrix* A) {
  141. if (preconditioner_ != nullptr) {
  142. return;
  143. }
  144. Preconditioner::Options preconditioner_options(options_);
  145. CHECK(options_.context != nullptr);
  146. switch (options_.preconditioner_type) {
  147. case IDENTITY:
  148. preconditioner_ = std::make_unique<IdentityPreconditioner>(
  149. schur_complement_->num_cols());
  150. break;
  151. case JACOBI:
  152. preconditioner_ = std::make_unique<SparseMatrixPreconditionerWrapper>(
  153. schur_complement_->block_diagonal_FtF_inverse(),
  154. preconditioner_options);
  155. break;
  156. case SCHUR_POWER_SERIES_EXPANSION:
  157. // Ignoring the value of spse_tolerance to ensure preconditioner stays
  158. // fixed during the iterations of cg.
  159. preconditioner_ = std::make_unique<PowerSeriesExpansionPreconditioner>(
  160. schur_complement_.get(),
  161. options_.max_num_spse_iterations,
  162. 0,
  163. preconditioner_options);
  164. break;
  165. case SCHUR_JACOBI:
  166. preconditioner_ = std::make_unique<SchurJacobiPreconditioner>(
  167. *A->block_structure(), preconditioner_options);
  168. break;
  169. case CLUSTER_JACOBI:
  170. case CLUSTER_TRIDIAGONAL:
  171. preconditioner_ = std::make_unique<VisibilityBasedPreconditioner>(
  172. *A->block_structure(), preconditioner_options);
  173. break;
  174. default:
  175. LOG(FATAL) << "Unknown Preconditioner Type";
  176. }
  177. };
  178. } // namespace ceres::internal