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
