Add power series expansion preconditioner Implementation of "Power Bundle Adjustment for Large-Scale 3D Reconstruction" by Weber et. al. added in the form of preconditioner. Change-Id: Ie85526a5fc46f74256f6dfe9173c3571f7160f3a
diff --git a/include/ceres/types.h b/include/ceres/types.h index c1e973a..52e1e7b 100644 --- a/include/ceres/types.h +++ b/include/ceres/types.h
@@ -97,7 +97,7 @@ // Block diagonal of the Gauss-Newton Hessian. JACOBI, - // Note: The following three preconditioners can only be used with + // Note: The following four preconditioners can only be used with // the ITERATIVE_SCHUR solver. They are well suited for Structure // from Motion problems. @@ -105,6 +105,13 @@ // only be used with the ITERATIVE_SCHUR solver. SCHUR_JACOBI, + // Use power series expansion to approximate the inversion of Schur complement + // as a preconditioner. + // WARNING! Application of this preconditioner currently is not integrated + // into linear solvers, so failure to use it via public API is expected + // behaviour. + SCHUR_POWER_SERIES_EXPANSION, + // Visibility clustering based preconditioners. // // The following two preconditioners use the visibility structure of
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index e4ef4c0..afe91fb 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -237,6 +237,7 @@ parameter_block_ordering.cc partitioned_matrix_view.cc polynomial.cc + power_series_expansion_preconditioner.cc preconditioner.cc preprocessor.cc problem_impl.cc @@ -529,6 +530,7 @@ ceres_test(parameter_dims) ceres_test(partitioned_matrix_view) ceres_test(polynomial) + ceres_test(power_series_expansion_preconditioner) ceres_test(problem) ceres_test(program) ceres_test(reorder_program)
diff --git a/internal/ceres/implicit_schur_complement.cc b/internal/ceres/implicit_schur_complement.cc index 751612f..4e837f3 100644 --- a/internal/ceres/implicit_schur_complement.cc +++ b/internal/ceres/implicit_schur_complement.cc
@@ -60,7 +60,8 @@ // E'E and F'E. if (block_diagonal_EtE_inverse_ == nullptr) { block_diagonal_EtE_inverse_ = A_->CreateBlockDiagonalEtE(); - if (options_.preconditioner_type == JACOBI) { + if (options_.preconditioner_type == JACOBI || + options_.preconditioner_type == SCHUR_POWER_SERIES_EXPANSION) { block_diagonal_FtF_inverse_ = A_->CreateBlockDiagonalFtF(); } rhs_.resize(A_->num_cols_f()); @@ -71,7 +72,8 @@ tmp_f_cols_.resize(A_->num_cols_f()); } else { A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); - if (options_.preconditioner_type == JACOBI) { + if (options_.preconditioner_type == JACOBI || + options_.preconditioner_type == SCHUR_POWER_SERIES_EXPANSION) { A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); } } @@ -80,7 +82,8 @@ // contributions from the diagonal D if it is non-null. Add that to // the block diagonals and invert them. AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); - if (options_.preconditioner_type == JACOBI) { + if (options_.preconditioner_type == JACOBI || + options_.preconditioner_type == SCHUR_POWER_SERIES_EXPANSION) { AddDiagonalAndInvert((D_ == nullptr) ? nullptr : D_ + A_->num_cols_e(), block_diagonal_FtF_inverse_.get()); } @@ -129,6 +132,33 @@ A_->LeftMultiplyAndAccumulateF(tmp_rows_.data(), y); } +void ImplicitSchurComplement::InversePowerSeriesOperatorRightMultiplyAccumulate( + const double* x, double* y) const { + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyAndAccumulateF(x, tmp_rows_.data()); + + // y2 = E' y1 + tmp_e_cols_.setZero(); + A_->LeftMultiplyAndAccumulateE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y3 = (E'E)^-1 y2 + tmp_e_cols_2_.setZero(); + block_diagonal_EtE_inverse_->RightMultiplyAndAccumulate(tmp_e_cols_.data(), + tmp_e_cols_2_.data()); + // y1 = E y3 + tmp_rows_.setZero(); + A_->RightMultiplyAndAccumulateE(tmp_e_cols_2_.data(), tmp_rows_.data()); + + // y4 = F' y1 + tmp_f_cols_.setZero(); + A_->LeftMultiplyAndAccumulateF(tmp_rows_.data(), tmp_f_cols_.data()); + + // y += (F'F)^-1 y4 + block_diagonal_FtF_inverse_->RightMultiplyAndAccumulate(tmp_f_cols_.data(), + y); +} + // Given a block diagonal matrix and an optional array of diagonal // entries D, add them to the diagonal of the matrix and compute the // inverse of each diagonal block.
diff --git a/internal/ceres/implicit_schur_complement.h b/internal/ceres/implicit_schur_complement.h index 8fcc309..435ddda 100644 --- a/internal/ceres/implicit_schur_complement.h +++ b/internal/ceres/implicit_schur_complement.h
@@ -122,6 +122,12 @@ RightMultiplyAndAccumulate(x, y); } + // Following is useful for approximation of S^-1 via power series expansion. + // Z = (F'F)^-1 F'E (E'E)^-1 E'F + // y += Zx + void InversePowerSeriesOperatorRightMultiplyAccumulate(const double* x, + double* y) const; + // y = (E'E)^-1 (E'b - E'F x). Given an estimate of the solution to // the Schur complement system, this method computes the value of // the e_block variables that were eliminated to form the Schur
diff --git a/internal/ceres/implicit_schur_complement_test.cc b/internal/ceres/implicit_schur_complement_test.cc index 0ebde31..66c6707 100644 --- a/internal/ceres/implicit_schur_complement_test.cc +++ b/internal/ceres/implicit_schur_complement_test.cc
@@ -136,17 +136,38 @@ ImplicitSchurComplement isc(options); isc.Init(*A_, D, b_.get()); - int num_sc_cols = lhs.cols(); + const int num_f_cols = lhs.cols(); + const int num_e_cols = num_cols_ - num_f_cols; - for (int i = 0; i < num_sc_cols; ++i) { - Vector x(num_sc_cols); + Matrix A_dense, E, F, DE, DF; + A_->ToDenseMatrix(&A_dense); + E = A_dense.leftCols(A_->num_cols() - num_f_cols); + F = A_dense.rightCols(num_f_cols); + if (D) { + DE = VectorRef(D, num_e_cols).asDiagonal(); + DF = VectorRef(D + num_e_cols, num_f_cols).asDiagonal(); + } else { + DE = Matrix::Zero(num_e_cols, num_e_cols); + DF = Matrix::Zero(num_f_cols, num_f_cols); + } + + // Z = (block_diagonal(F'F))^-1 F'E (E'E)^-1 E'F + // Here, assuming that block_diagonal(F'F) == diagonal(F'F) + Matrix Z_reference = + (F.transpose() * F + DF).diagonal().asDiagonal().inverse() * + F.transpose() * E * (E.transpose() * E + DE).inverse() * + E.transpose() * F; + + for (int i = 0; i < num_f_cols; ++i) { + Vector x(num_f_cols); x.setZero(); x(i) = 1.0; - Vector y(num_sc_cols); + Vector y(num_f_cols); y = lhs * x; - Vector z(num_sc_cols); + + Vector z(num_f_cols); isc.RightMultiplyAndAccumulate(x.data(), z.data()); // The i^th column of the implicit schur complement is the same as @@ -157,6 +178,22 @@ << "column " << i << ". explicit: " << y.transpose() << " implicit: " << z.transpose(); } + + y.setZero(); + y = Z_reference * x; + z.setZero(); + isc.InversePowerSeriesOperatorRightMultiplyAccumulate(x.data(), z.data()); + + // The i^th column of operator Z stored implicitly is the same as its + // explicit version. + if ((y - z).norm() > kEpsilon) { + return testing::AssertionFailure() + << "Explicit and Implicit operators used to approximate the " + "inversion of schur complement via power series expansion " + "differ in column " + << i << ". explicit: " << y.transpose() + << " implicit: " << z.transpose(); + } } // Compare the rhs of the reduced linear system
diff --git a/internal/ceres/linear_least_squares_problems.cc b/internal/ceres/linear_least_squares_problems.cc index d0262e3..b8633aa 100644 --- a/internal/ceres/linear_least_squares_problems.cc +++ b/internal/ceres/linear_least_squares_problems.cc
@@ -61,6 +61,8 @@ return LinearLeastSquaresProblem3(); case 4: return LinearLeastSquaresProblem4(); + case 5: + return LinearLeastSquaresProblem5(); default: LOG(FATAL) << "Unknown problem id requested " << id; } @@ -629,6 +631,183 @@ return problem; } +/* +A problem with block-diagonal F'F. + + A = [1 0 | 0 0 2 + 3 0 | 0 0 4 + 0 -1 | 0 1 0 + 0 -3 | 0 1 0 + 0 -1 | 3 0 0 + 0 -2 | 1 0 0] + + b = [0 + 1 + 2 + 3 + 4 + 5] + + c = A'* b = [ 22 + -25 + 17 + 7 + 4] + + A'A = [10 0 0 0 10 + 0 15 -5 -4 0 + 0 -5 10 0 0 + 0 -4 0 2 0 + 10 0 0 0 20] + + cond(A'A) = 41.402 + + S = [ 8.3333 -1.3333 0 + -1.3333 0.9333 0 + 0 0 10.0000] + + r = [ 8.6667 + -1.6667 + 1.0000] + + S\r = [ 0.9778 + -0.3889 + 0.1000] + + A\b = [ 0.2 + -1.4444 + 0.9777 + -0.3888 + 0.1] +*/ + +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem5() { + int num_rows = 6; + int num_cols = 5; + + std::unique_ptr<LinearLeastSquaresProblem> problem = + std::make_unique<LinearLeastSquaresProblem>(); + + problem->b = std::make_unique<double[]>(num_rows); + problem->D = std::make_unique<double[]>(num_cols); + problem->num_eliminate_blocks = 2; + + // TODO: add x + problem->x = std::make_unique<double[]>(num_cols); + problem->x[0] = 0.2; + problem->x[1] = -1.4444; + problem->x[2] = 0.9777; + problem->x[3] = -0.3888; + problem->x[4] = 0.1; + + auto* bs = new CompressedRowBlockStructure; + std::unique_ptr<double[]> values = + std::make_unique<double[]>(num_rows * num_cols); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.emplace_back(); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = -1; + values[nnz++] = 2; + + bs->rows.emplace_back(); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.emplace_back(0, 0); + row.cells.emplace_back(4, 1); + } + + // Row 2 + { + values[nnz++] = 3; + values[nnz++] = 4; + + bs->rows.emplace_back(); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.emplace_back(0, 2); + row.cells.emplace_back(4, 3); + } + + // Row 3 + { + values[nnz++] = -1; + values[nnz++] = 1; + + bs->rows.emplace_back(); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.emplace_back(1, 4); + row.cells.emplace_back(3, 5); + } + + // Row 4 + { + values[nnz++] = -3; + values[nnz++] = 1; + + bs->rows.emplace_back(); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.emplace_back(1, 6); + row.cells.emplace_back(3, 7); + } + + // Row 5 + { + values[nnz++] = -1; + values[nnz++] = 3; + + bs->rows.emplace_back(); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.emplace_back(1, 8); + row.cells.emplace_back(2, 9); + } + + // Row 6 + { + // values[nnz++] = 2; + values[nnz++] = -2; + values[nnz++] = 1; + + bs->rows.emplace_back(); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 5; + // row.cells.emplace_back(0, 10); + row.cells.emplace_back(1, 10); + row.cells.emplace_back(2, 11); + } + + auto A = std::make_unique<BlockSparseMatrix>(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A = std::move(A); + + return problem; +} + namespace { bool DumpLinearLeastSquaresProblemToConsole(const SparseMatrix* A, const double* D,
diff --git a/internal/ceres/linear_least_squares_problems.h b/internal/ceres/linear_least_squares_problems.h index 7968c46..0e14517 100644 --- a/internal/ceres/linear_least_squares_problems.h +++ b/internal/ceres/linear_least_squares_problems.h
@@ -73,6 +73,8 @@ std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem3(); CERES_NO_EXPORT std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem4(); +CERES_NO_EXPORT +std::unique_ptr<LinearLeastSquaresProblem> LinearLeastSquaresProblem5(); // Write the linear least squares problem to disk. The exact format // depends on dump_format_type.
diff --git a/internal/ceres/power_series_expansion_preconditioner.cc b/internal/ceres/power_series_expansion_preconditioner.cc new file mode 100644 index 0000000..1b2dbb9 --- /dev/null +++ b/internal/ceres/power_series_expansion_preconditioner.cc
@@ -0,0 +1,81 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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: markshachkov@gmail.com (Mark Shachkov) + +#include "ceres/power_series_expansion_preconditioner.h" + +namespace ceres::internal { + +PowerSeriesExpansionPreconditioner::PowerSeriesExpansionPreconditioner( + const ImplicitSchurComplement* isc, + const double spse_tolerance, + const int min_num_iterations, + const int max_num_iterations) + : isc_(isc), + spse_tolerance_(spse_tolerance), + min_num_iterations_(min_num_iterations), + max_num_iterations_(max_num_iterations) {} + +PowerSeriesExpansionPreconditioner::~PowerSeriesExpansionPreconditioner() = + default; + +bool PowerSeriesExpansionPreconditioner::Update(const LinearOperator& A, + const double* D) { + return true; +} + +void PowerSeriesExpansionPreconditioner::RightMultiplyAndAccumulate( + const double* x, double* y) const { + VectorRef yref(y, num_rows()); + Vector series_term(num_rows()); + Vector previous_series_term(num_rows()); + yref.setZero(); + isc_->block_diagonal_FtF_inverse()->RightMultiplyAndAccumulate(x, y); + previous_series_term = yref; + + const double norm_threshold = spse_tolerance_ * yref.norm(); + + for (int i = 1;; i++) { + series_term.setZero(); + isc_->InversePowerSeriesOperatorRightMultiplyAccumulate( + previous_series_term.data(), series_term.data()); + yref += series_term; + if (i >= min_num_iterations_ && + (i >= max_num_iterations_ || series_term.norm() < norm_threshold)) { + break; + } + std::swap(previous_series_term, series_term); + } +} + +int PowerSeriesExpansionPreconditioner::num_rows() const { + return isc_->num_rows(); +} + +} // namespace ceres::internal
diff --git a/internal/ceres/power_series_expansion_preconditioner.h b/internal/ceres/power_series_expansion_preconditioner.h new file mode 100644 index 0000000..c5cf32d --- /dev/null +++ b/internal/ceres/power_series_expansion_preconditioner.h
@@ -0,0 +1,70 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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: markshachkov@gmail.com (Mark Shachkov) + +#ifndef CERES_INTERNAL_POWER_SERIES_EXPANSION_PRECONDITIONER_H_ +#define CERES_INTERNAL_POWER_SERIES_EXPANSION_PRECONDITIONER_H_ + +#include "ceres/implicit_schur_complement.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" +#include "ceres/preconditioner.h" + +namespace ceres::internal { + +// This is a preconditioner via power series expansion of Schur +// complement inverse based on "Weber et al, Power Bundle Adjustment for +// Large-Scale 3D Reconstruction". + +class CERES_NO_EXPORT PowerSeriesExpansionPreconditioner + : public Preconditioner { + public: + PowerSeriesExpansionPreconditioner(const ImplicitSchurComplement* isc, + const double spse_tolerance_, + const int min_num_iterations_, + const int max_num_iterations_); + PowerSeriesExpansionPreconditioner( + const PowerSeriesExpansionPreconditioner&) = delete; + void operator=(const PowerSeriesExpansionPreconditioner&) = delete; + ~PowerSeriesExpansionPreconditioner() override; + + void RightMultiplyAndAccumulate(const double* x, double* y) const final; + bool Update(const LinearOperator& A, const double* D) final; + int num_rows() const final; + + private: + const ImplicitSchurComplement* isc_; + const double spse_tolerance_; + const int min_num_iterations_; + const int max_num_iterations_; +}; + +} // namespace ceres::internal + +#endif // CERES_INTERNAL_POWER_SERIES_EXPANSION_PRECONDITIONER_H_
diff --git a/internal/ceres/power_series_expansion_preconditioner_test.cc b/internal/ceres/power_series_expansion_preconditioner_test.cc new file mode 100644 index 0000000..a101831 --- /dev/null +++ b/internal/ceres/power_series_expansion_preconditioner_test.cc
@@ -0,0 +1,173 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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: markshachkov@gmail.com (Mark Shachkov) + +#include "ceres/power_series_expansion_preconditioner.h" + +#include "Eigen/Dense" +#include "ceres/linear_least_squares_problems.h" +#include "gtest/gtest.h" + +namespace ceres::internal { + +const double kEpsilon = 1e-14; + +class PowerSeriesExpansionPreconditionerTest : public ::testing::Test { + protected: + void SetUp() final { + problem_ = CreateLinearLeastSquaresProblemFromId(5); + const auto A = down_cast<BlockSparseMatrix*>(problem_->A.get()); + const auto D = problem_->D.get(); + + LinearSolver::Options options; + options.elimination_groups.push_back(problem_->num_eliminate_blocks); + options.preconditioner_type = SCHUR_POWER_SERIES_EXPANSION; + isc_ = std::make_unique<ImplicitSchurComplement>(options); + isc_->Init(*A, D, problem_->b.get()); + num_f_cols_ = isc_->rhs().rows(); + const int num_rows = A->num_rows(), num_cols = A->num_cols(), + num_e_cols = num_cols - num_f_cols_; + + // Using predefined linear operator with schur structure and block-diagonal + // F'F to explicitly construct schur complement and to calculate its inverse + // to be used as a reference. + Matrix A_dense, E, F, DE, DF; + problem_->A->ToDenseMatrix(&A_dense); + E = A_dense.leftCols(num_e_cols); + F = A_dense.rightCols(num_f_cols_); + DE = VectorRef(D, num_e_cols).asDiagonal(); + DF = VectorRef(D + num_e_cols, num_f_cols_).asDiagonal(); + + sc_inverse_expected_ = + (F.transpose() * + (Matrix::Identity(num_rows, num_rows) - + E * (E.transpose() * E + DE).inverse() * E.transpose()) * + F + + DF) + .inverse(); + } + std::unique_ptr<LinearLeastSquaresProblem> problem_; + std::unique_ptr<ImplicitSchurComplement> isc_; + int num_f_cols_; + Matrix sc_inverse_expected_; +}; + +TEST_F(PowerSeriesExpansionPreconditionerTest, + InverseValidPreconditionerToleranceReached) { + const double spse_tolerance = kEpsilon; + const int min_iterations = 1; + const int max_iterations = 50; + PowerSeriesExpansionPreconditioner preconditioner( + isc_.get(), spse_tolerance, min_iterations, max_iterations); + + Vector x(num_f_cols_), y(num_f_cols_); + for (int i = 0; i < num_f_cols_; i++) { + x.setZero(); + x(i) = 1.0; + + y.setZero(); + preconditioner.RightMultiplyAndAccumulate(x.data(), y.data()); + EXPECT_LT((y - sc_inverse_expected_.col(i)).norm(), kEpsilon) + << "Reference Schur complement inverse and its estimate via " + "PowerSeriesExpansionPreconditioner differs in " + << i + << " column.\nreference : " << sc_inverse_expected_.col(i).transpose() + << "\nestimated: " << y.transpose(); + } +} + +TEST_F(PowerSeriesExpansionPreconditionerTest, + InverseValidPreconditionerMaxIterations) { + const double spse_tolerance = 1 / kEpsilon; + const int num_iterations = 50; + PowerSeriesExpansionPreconditioner preconditioner_fixed_n_iterations( + isc_.get(), spse_tolerance, num_iterations, num_iterations); + + Vector x(num_f_cols_), y(num_f_cols_); + for (int i = 0; i < num_f_cols_; i++) { + x.setZero(); + x(i) = 1.0; + + y.setZero(); + preconditioner_fixed_n_iterations.RightMultiplyAndAccumulate(x.data(), + y.data()); + EXPECT_LT((y - sc_inverse_expected_.col(i)).norm(), kEpsilon) + << "Reference Schur complement inverse and its estimate via " + "PowerSeriesExpansionPreconditioner differs in " + << i + << " column.\nreference : " << sc_inverse_expected_.col(i).transpose() + << "\nestimated: " << y.transpose(); + } +} + +TEST_F(PowerSeriesExpansionPreconditionerTest, + InverseInvalidBadPreconditionerTolerance) { + const double spse_tolerance = 1 / kEpsilon; + const int min_iterations = 1; + const int max_iterations = 50; + PowerSeriesExpansionPreconditioner preconditioner_bad_tolerance( + isc_.get(), spse_tolerance, min_iterations, max_iterations); + + Vector x(num_f_cols_), y(num_f_cols_); + for (int i = 0; i < num_f_cols_; i++) { + x.setZero(); + x(i) = 1.0; + + y.setZero(); + preconditioner_bad_tolerance.RightMultiplyAndAccumulate(x.data(), y.data()); + EXPECT_GT((y - sc_inverse_expected_.col(i)).norm(), kEpsilon) + << "Reference Schur complement inverse and its estimate via " + "PowerSeriesExpansionPreconditioner are too similar, tolerance " + "stopping criteria failed."; + } +} + +TEST_F(PowerSeriesExpansionPreconditionerTest, + InverseInvalidBadPreconditionerMaxIterations) { + const double spse_tolerance = kEpsilon; + const int num_iterations = 1; + PowerSeriesExpansionPreconditioner preconditioner_bad_iterations_limit( + isc_.get(), spse_tolerance, num_iterations, num_iterations); + + Vector x(num_f_cols_), y(num_f_cols_); + for (int i = 0; i < num_f_cols_; i++) { + x.setZero(); + x(i) = 1.0; + + y.setZero(); + preconditioner_bad_iterations_limit.RightMultiplyAndAccumulate(x.data(), + y.data()); + EXPECT_GT((y - sc_inverse_expected_.col(i)).norm(), kEpsilon) + << "Reference Schur complement inverse and its estimate via " + "PowerSeriesExpansionPreconditioner are too similar, maximum " + "iterations stopping criteria failed."; + } +} + +} // namespace ceres::internal \ No newline at end of file