123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278 |
- #include "ceres/implicit_schur_complement.h"
- #include "Eigen/Dense"
- #include "ceres/block_sparse_matrix.h"
- #include "ceres/block_structure.h"
- #include "ceres/internal/eigen.h"
- #include "ceres/linear_solver.h"
- #include "ceres/parallel_for.h"
- #include "ceres/parallel_vector_ops.h"
- #include "ceres/types.h"
- #include "glog/logging.h"
- namespace ceres::internal {
- ImplicitSchurComplement::ImplicitSchurComplement(
- const LinearSolver::Options& options)
- : options_(options) {}
- void ImplicitSchurComplement::Init(const BlockSparseMatrix& A,
- const double* D,
- const double* b) {
-
-
- if (A_ == nullptr) {
- A_ = PartitionedMatrixViewBase::Create(options_, A);
- }
- D_ = D;
- b_ = b;
- compute_ftf_inverse_ =
- options_.use_spse_initialization ||
- options_.preconditioner_type == JACOBI ||
- options_.preconditioner_type == SCHUR_POWER_SERIES_EXPANSION;
-
-
- if (block_diagonal_EtE_inverse_ == nullptr) {
- block_diagonal_EtE_inverse_ = A_->CreateBlockDiagonalEtE();
- if (compute_ftf_inverse_) {
- block_diagonal_FtF_inverse_ = A_->CreateBlockDiagonalFtF();
- }
- rhs_.resize(A_->num_cols_f());
- rhs_.setZero();
- tmp_rows_.resize(A_->num_rows());
- tmp_e_cols_.resize(A_->num_cols_e());
- tmp_e_cols_2_.resize(A_->num_cols_e());
- tmp_f_cols_.resize(A_->num_cols_f());
- } else {
- A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get());
- if (compute_ftf_inverse_) {
- A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get());
- }
- }
-
-
-
- AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get());
- if (compute_ftf_inverse_) {
- AddDiagonalAndInvert((D_ == nullptr) ? nullptr : D_ + A_->num_cols_e(),
- block_diagonal_FtF_inverse_.get());
- }
-
- UpdateRhs();
- }
- void ImplicitSchurComplement::RightMultiplyAndAccumulate(const double* x,
- double* y) const {
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_rows_);
- A_->RightMultiplyAndAccumulateF(x, tmp_rows_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_);
- A_->LeftMultiplyAndAccumulateE(tmp_rows_.data(), tmp_e_cols_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_2_);
- block_diagonal_EtE_inverse_->RightMultiplyAndAccumulate(tmp_e_cols_.data(),
- tmp_e_cols_2_.data(),
- options_.context,
- options_.num_threads);
- ParallelAssign(
- options_.context, options_.num_threads, tmp_e_cols_2_, -tmp_e_cols_2_);
-
- A_->RightMultiplyAndAccumulateE(tmp_e_cols_2_.data(), tmp_rows_.data());
-
- if (D_ != nullptr) {
- ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols());
- VectorRef y_cols(y, num_cols());
- ParallelAssign(
- options_.context,
- options_.num_threads,
- y_cols,
- (Dref.array().square() * ConstVectorRef(x, num_cols()).array()));
- } else {
- ParallelSetZero(options_.context, options_.num_threads, y, num_cols());
- }
-
- A_->LeftMultiplyAndAccumulateF(tmp_rows_.data(), y);
- }
- void ImplicitSchurComplement::InversePowerSeriesOperatorRightMultiplyAccumulate(
- const double* x, double* y) const {
- CHECK(compute_ftf_inverse_);
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_rows_);
- A_->RightMultiplyAndAccumulateF(x, tmp_rows_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_);
- A_->LeftMultiplyAndAccumulateE(tmp_rows_.data(), tmp_e_cols_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_2_);
- block_diagonal_EtE_inverse_->RightMultiplyAndAccumulate(tmp_e_cols_.data(),
- tmp_e_cols_2_.data(),
- options_.context,
- options_.num_threads);
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_rows_);
- A_->RightMultiplyAndAccumulateE(tmp_e_cols_2_.data(), tmp_rows_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_f_cols_);
- A_->LeftMultiplyAndAccumulateF(tmp_rows_.data(), tmp_f_cols_.data());
-
- block_diagonal_FtF_inverse_->RightMultiplyAndAccumulate(
- tmp_f_cols_.data(), y, options_.context, options_.num_threads);
- }
- void ImplicitSchurComplement::AddDiagonalAndInvert(
- const double* D, BlockSparseMatrix* block_diagonal) {
- const CompressedRowBlockStructure* block_diagonal_structure =
- block_diagonal->block_structure();
- ParallelFor(options_.context,
- 0,
- block_diagonal_structure->rows.size(),
- options_.num_threads,
- [block_diagonal_structure, D, block_diagonal](int row_block_id) {
- auto& row = block_diagonal_structure->rows[row_block_id];
- const int row_block_pos = row.block.position;
- const int row_block_size = row.block.size;
- const Cell& cell = row.cells[0];
- MatrixRef m(block_diagonal->mutable_values() + cell.position,
- row_block_size,
- row_block_size);
- if (D != nullptr) {
- ConstVectorRef d(D + row_block_pos, row_block_size);
- m += d.array().square().matrix().asDiagonal();
- }
- m = m.selfadjointView<Eigen::Upper>().llt().solve(
- Matrix::Identity(row_block_size, row_block_size));
- });
- }
- void ImplicitSchurComplement::BackSubstitute(const double* x, double* y) {
- const int num_cols_e = A_->num_cols_e();
- const int num_cols_f = A_->num_cols_f();
- const int num_cols = A_->num_cols();
- const int num_rows = A_->num_rows();
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_rows_);
- A_->RightMultiplyAndAccumulateF(x, tmp_rows_.data());
-
- ParallelAssign(options_.context,
- options_.num_threads,
- tmp_rows_,
- ConstVectorRef(b_, num_rows) - tmp_rows_);
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_);
- A_->LeftMultiplyAndAccumulateE(tmp_rows_.data(), tmp_e_cols_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, y, num_cols);
- block_diagonal_EtE_inverse_->RightMultiplyAndAccumulate(
- tmp_e_cols_.data(), y, options_.context, options_.num_threads);
-
-
-
-
-
- VectorRef y_cols_f(y + num_cols_e, num_cols_f);
- ParallelAssign(options_.context,
- options_.num_threads,
- y_cols_f,
- ConstVectorRef(x, num_cols_f));
- }
- void ImplicitSchurComplement::UpdateRhs() {
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_);
- A_->LeftMultiplyAndAccumulateE(b_, tmp_e_cols_.data());
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_e_cols_2_);
- block_diagonal_EtE_inverse_->RightMultiplyAndAccumulate(tmp_e_cols_.data(),
- tmp_e_cols_2_.data(),
- options_.context,
- options_.num_threads);
-
- ParallelSetZero(options_.context, options_.num_threads, tmp_rows_);
- A_->RightMultiplyAndAccumulateE(tmp_e_cols_2_.data(), tmp_rows_.data());
-
- ParallelAssign(options_.context,
- options_.num_threads,
- tmp_rows_,
- ConstVectorRef(b_, A_->num_rows()) - tmp_rows_);
-
- ParallelSetZero(options_.context, options_.num_threads, rhs_);
- A_->LeftMultiplyAndAccumulateF(tmp_rows_.data(), rhs_.data());
- }
- }
|