Added BlkMatrix::isApprox()

Author: ManifoldFR

CHANGELOG.md

@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+- gar/blk-matrix : added `BlkMatrix::isApprox()` (https://github.com/Simple-Robotics/aligator/pull/366)
 - core : added [mimalloc](https://github.com/mimalloc/mimalloc)-based memory resource (https://github.com/Simple-Robotics/aligator/pull/366)
 - modelling/costs : add getter and setter for cost term weights (https://github.com/Simple-Robotics/aligator/pull/359)
 - support for Pinocchio 4 (https://github.com/Simple-Robotics/aligator/pull/361)
@@ -18,6 +19,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+- tests/gar/block-matrix.cpp : test against BunchKaufman factorization
 - move headers `allocator.hpp` and `arena-matrix.hpp` to `aligator/core` dir (https://github.com/Simple-Robotics/aligator/pull/362)
 - python: aligator now requires eigenpy version 3.10.3 at least
 - python: remove eigenpy version checks

include/aligator/gar/blk-matrix.hpp

@@ -171,6 +171,20 @@ template <typename _MatrixType, int _N, int _M> class BlkMatrix {
   MatrixType &matrix() { return m_data; }
   const MatrixType &matrix() const { return m_data; }
 
+  template <typename D>
+  inline bool isApprox(
+      const Eigen::EigenBase<D> &other,
+      const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
+    return matrix().isApprox(other, prec);
+  }
+
+  template <typename D, int N2, int M2>
+  inline bool isApprox(
+      const BlkMatrix<D, N2, M2> &other,
+      const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
+    return matrix().isApprox(other.matrix(), prec);
+  }
+
   const RowDimsType &rowDims() const { return m_rowDims; }
   const RowDimsType &rowIndices() const { return m_rowIndices; }
   const ColDimsType &colDims() const { return m_colDims; }
@@ -179,6 +193,7 @@ template <typename _MatrixType, int _N, int _M> class BlkMatrix {
   Index rows() const { return m_totalRows; }
   Index cols() const { return m_totalCols; }
 
+  /// @brief Take the top \c n block rows of the block matrix.
   auto topBlkRows(size_t n) {
     using OutType = BlkMatrix<Eigen::Ref<MatrixType>, -1, M>;
     std::vector<Index> subRowDims;
@@ -188,6 +203,8 @@ template <typename _MatrixType, int _N, int _M> class BlkMatrix {
     return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
   }
 
+  /// @copybrief topBlkRows().
+  /// This version returns a fixed-size block.
   template <size_t n> auto topBlkRows() {
     static_assert(n <= N,
                   "Cannot take n block rows of matrix with <n block rows.");

tests/gar/block-matrix.cpp

@@ -6,6 +6,7 @@
 #include "aligator/context.hpp"
 #include "aligator/gar/blk-matrix.hpp"
 #include "aligator/gar/block-tridiagonal.hpp"
+#include "aligator/core/bunchkaufman.hpp"
 #include <Eigen/Cholesky>
 
 #include "test_util.hpp"
@@ -95,26 +96,17 @@ struct BTAG_Fixture {
   }
 };
 
+constexpr double prec = 1e-12;
+
 TEST_CASE_METHOD(BTAG_Fixture, "block_tridiag_solve_up_looking", "[gar]") {
 
   bool ret = gar::symmetricBlockTridiagSolve(sub, diagonal, sup, vec, facs);
   REQUIRE(ret);
 
-  for (size_t i = 0; i <= N; i++) {
-    fmt::print("rhs[{:d}] = {}\n", i, vec[i].transpose());
-  }
-
-  {
-    // alternative solve
-    Eigen::LDLT<MatrixXs> ldlt(densemat);
-    ldlt.solveInPlace(rhs.matrix());
+  aligator::BunchKaufman<MatrixXs> ldlt(densemat);
+  ldlt.solveInPlace(rhs.matrix());
 
-    fmt::print("Alternative solve:\n");
-    for (size_t i = 0; i <= N; i++) {
-      fmt::print("rhs[{:d}] = {}\n", i, rhs[i].transpose());
-    }
-    REQUIRE(vec.matrix().isApprox(rhs.matrix(), 1e-12));
-  }
+  REQUIRE(vec.isApprox(rhs, prec));
 }
 
 TEST_CASE_METHOD(BTAG_Fixture, "block_tridiag_solve_down_looking", "[gar]") {
@@ -123,19 +115,8 @@ TEST_CASE_METHOD(BTAG_Fixture, "block_tridiag_solve_down_looking", "[gar]") {
       gar::symmetricBlockTridiagSolveDownLooking(sub, diagonal, sup, vec, facs);
   REQUIRE(ret);
 
-  for (size_t i = 0; i <= N; i++) {
-    fmt::print("rhs[{:d}] = {}\n", i, vec[i].transpose());
-  }
-
-  {
-    // alternative solve
-    Eigen::LDLT<MatrixXs> ldlt(densemat);
-    ldlt.solveInPlace(rhs.matrix());
+  aligator::BunchKaufman<MatrixXs> ldlt(densemat);
+  ldlt.solveInPlace(rhs.matrix());
 
-    fmt::print("Alternative solve:\n");
-    for (size_t i = 0; i <= N; i++) {
-      fmt::print("rhs[{:d}] = {}\n", i, rhs[i].transpose());
-    }
-    REQUIRE(vec.matrix().isApprox(rhs.matrix(), 1e-12));
-  }
+  REQUIRE(vec.isApprox(rhs, prec));
 }