From a9a5ec276b2c2f94f7407632d236a1734e051006 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Thu, 2 Jan 2025 17:27:07 +0100 Subject: [PATCH] Update PCG implementation --- g2o/solvers/pcg/linear_solver_pcg.h | 84 ++++++++++--------------- unit_test/solver/linear_solver_test.cpp | 22 +++++++ 2 files changed, 55 insertions(+), 51 deletions(-) diff --git a/g2o/solvers/pcg/linear_solver_pcg.h b/g2o/solvers/pcg/linear_solver_pcg.h index 0fa03afd9..f4f73d9a2 100644 --- a/g2o/solvers/pcg/linear_solver_pcg.h +++ b/g2o/solvers/pcg/linear_solver_pcg.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -77,7 +78,7 @@ class LinearSolverPCG : public LinearSolver { protected: using MatrixVector = std::vector; - using MatrixPtrVector = std::vector; + using MatrixPtrVector = std::vector>; double tolerance_ = cst(1e-6); double residual_ = -1.; @@ -88,12 +89,11 @@ class LinearSolverPCG : public LinearSolver { MatrixPtrVector diag_; MatrixVector J_; - std::vector > indices_; + std::vector> indices_; MatrixPtrVector sparseMat_; - void multDiag(const std::vector& colBlockIndices, MatrixVector& A, - const VectorX& src, VectorX& dest); - void multDiag(const std::vector& colBlockIndices, MatrixPtrVector& A, + template + void multDiag(const std::vector& colBlockIndices, std::vector& A, const VectorX& src, VectorX& dest); void mult(const std::vector& colBlockIndices, const VectorX& src, VectorX& dest); @@ -165,40 +165,32 @@ bool LinearSolverPCG::solve(const SparseBlockMatrix& A, for (size_t i = 0; i < A.blockCols().size(); ++i) { const typename SparseBlockMatrix::IntBlockMap& col = A.blockCols()[i]; - if (col.size() > 0) { - typename SparseBlockMatrix::IntBlockMap::const_iterator it; - for (it = col.begin(); it != col.end(); ++it) { - // only the upper triangular block is needed - if (it->first == static_cast(i)) { - diag_.push_back(it->second); - J_.push_back(it->second->inverse()); - break; - } - if (indexRequired) { - indices_.push_back(std::make_pair( - it->first > 0 ? A.rowBlockIndices()[it->first - 1] : 0, colIdx)); - sparseMat_.push_back(it->second); - } + for (const auto& elem : col) { + // only the upper triangular block is needed + if (elem.first == static_cast(i)) { + diag_.push_back(std::cref(*elem.second)); + J_.push_back(elem.second->inverse()); + break; + } + if (indexRequired) { + indices_.emplace_back( + elem.first > 0 ? A.rowBlockIndices()[elem.first - 1] : 0, colIdx); + sparseMat_.push_back(std::cref(*elem.second)); } } colIdx = A.colBlockIndices()[i]; } - int n = A.rows(); + const int n = A.rows(); assert(n > 0 && "Hessian has 0 rows/cols"); VectorX::MapType xvec(x, A.cols()); - const VectorX::ConstMapType bvec(b, n); xvec.setZero(); - VectorX r; - VectorX d; - VectorX q; - VectorX s; - d.setZero(n); - q.setZero(n); - s.setZero(n); + VectorX r = VectorX::ConstMapType(b, n); + VectorX d = VectorX::Zero(n); + VectorX q = VectorX::Zero(n); + VectorX s = VectorX::Zero(n); - r = bvec; multDiag(A.colBlockIndices(), J_, r, d); double dn = r.dot(d); double d0 = tolerance_ * dn; @@ -214,14 +206,14 @@ bool LinearSolverPCG::solve(const SparseBlockMatrix& A, G2O_DEBUG("residual [{}]: {}", iteration, dn); if (dn <= d0) break; // done mult(A.colBlockIndices(), d, q); - double a = dn / d.dot(q); - xvec += a * d; + const double a = dn / d.dot(q); + xvec.noalias() += a * d; // TODO(goki): reset residual here every 50 iterations - r -= a * q; + r.noalias() -= a * q; multDiag(A.colBlockIndices(), J_, r, s); - double dold = dn; + const double dold = dn; dn = r.dot(s); - double ba = dn / dold; + const double ba = dn / dold; d = s + ba * d; } residual_ = 0.5 * dn; @@ -234,23 +226,14 @@ bool LinearSolverPCG::solve(const SparseBlockMatrix& A, } template +template void LinearSolverPCG::multDiag( - const std::vector& colBlockIndices, MatrixVector& A, - const VectorX& src, VectorX& dest) { - int row = 0; - for (size_t i = 0; i < A.size(); ++i) { - internal::pcg_axy(A[i], src, row, dest, row); - row = colBlockIndices[i]; - } -} - -template -void LinearSolverPCG::multDiag( - const std::vector& colBlockIndices, MatrixPtrVector& A, + const std::vector& colBlockIndices, std::vector& A, const VectorX& src, VectorX& dest) { int row = 0; for (size_t i = 0; i < A.size(); ++i) { - internal::pcg_axy(*A[i], src, row, dest, row); + const MatrixType& mat = A[i]; + internal::pcg_axy(mat, src, row, dest, row); row = colBlockIndices[i]; } } @@ -268,12 +251,11 @@ void LinearSolverPCG::mult(const std::vector& colBlockIndices, const int& destOffset = indices_[i].first; const int& srcOffsetT = destOffset; - const typename SparseBlockMatrix::SparseMatrixBlock* a = - sparseMat_[i]; + const MatrixType& a = sparseMat_[i]; // destVec += *a * srcVec (according to the sub-vector parts) - internal::pcg_axpy(*a, src, srcOffset, dest, destOffset); + internal::pcg_axpy(a, src, srcOffset, dest, destOffset); // destVec += *a.transpose() * srcVec (according to the sub-vector parts) - internal::pcg_atxpy(*a, src, srcOffsetT, dest, destOffsetT); + internal::pcg_atxpy(a, src, srcOffsetT, dest, destOffsetT); } } diff --git a/unit_test/solver/linear_solver_test.cpp b/unit_test/solver/linear_solver_test.cpp index 1dfebffd5..6762c2505 100644 --- a/unit_test/solver/linear_solver_test.cpp +++ b/unit_test/solver/linear_solver_test.cpp @@ -30,6 +30,7 @@ #include "g2o/solvers/dense/linear_solver_dense.h" #include "g2o/solvers/eigen/linear_solver_eigen.h" +#include "g2o/solvers/pcg/linear_solver_pcg.h" #ifdef G2O_HAVE_CSPARSE #include "g2o/solvers/csparse/linear_solver_csparse.h" #endif @@ -181,3 +182,24 @@ using LinearSolverTypes = ::testing::Types< std::pair, BlockOrdering>, std::pair, NoopOrdering>>; INSTANTIATE_TYPED_TEST_SUITE_P(LinearSolver, LS, LinearSolverTypes); + +// A single test for PCG +TEST(LS, LinearSolverPCGSolve) { + g2o::LinearSolverPCG solver; + + // test data + g2o::SparseBlockMatrixX sparse_matrix; + g2o::internal::fillTestMatrix(sparse_matrix); + g2o::VectorX x_vector = g2o::internal::createTestVectorX(); + g2o::VectorX b_vector = g2o::internal::createTestVectorB(); + + g2o::VectorX solver_solution; + for (int solve_iter = 0; solve_iter < 2; ++solve_iter) { + solver.init(); + solver_solution.setZero(b_vector.size()); + solver.solve(sparse_matrix, solver_solution.data(), b_vector.data()); + + EXPECT_TRUE(solver_solution.isApprox(x_vector, 1e-3)) + << "Solution differs on iteration " << solve_iter; + } +}