Merge pull request #62 from Simple-Robotics/wjallet/topic/bk-fix-1d

[linalg] fix corner case for Bunch-Kaufman solver

Author: jorisv

CHANGELOG.md

@@ -7,8 +7,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Fixed
+
 * Add missing dependencies in `package.xml`: pinocchio, eigen.
 * Fix `ConstraintObjectTpl::operator==` constness (mandatory for eigenpy 3.3)
+* Corner case in `BunchKaufman<>` decomposition class when number of lhs rows is 1
+
+### Added
+
+* Typedef for `Scalar` in the matrix decomposition classes
+* `.solve(rhs)` (allocating version of `.solveInPlace()`) to their C++ class and Python bindings
 
 ## [0.3.4] - 2024-01-19
 

bindings/python/expose-ldlt.cpp

@@ -9,6 +9,9 @@ namespace python {
 
 template <class LDLTtype>
 struct LDLTVisitor : bp::def_visitor<LDLTVisitor<LDLTtype>> {
+  using Scalar = typename LDLTtype::Scalar;
+  using VectorXs = Eigen::Matrix<Scalar, -1, 1, Eigen::ColMajor>;
+  using MatrixXs = Eigen::Matrix<Scalar, -1, -1, Eigen::ColMajor>;
 
   static LDLTtype &compute_proxy(LDLTtype &fac,
                                  const context::ConstMatrixRef &mat) {
@@ -20,10 +23,17 @@ struct LDLTVisitor : bp::def_visitor<LDLTVisitor<LDLTtype>> {
     return fac.solveInPlace(rhsAndX);
   }
 
+  template <typename RhsType>
+  static auto solve(const LDLTtype &fac, RhsType &rhs) {
+    return fac.solve(rhs);
+  }
+
   template <typename... Args> void visit(bp::class_<Args...> &cl) const {
     cl.def("compute", compute_proxy, policies::return_internal_reference,
            ("self"_a, "mat"))
         .def("solveInPlace", solveInPlace_proxy, ("self"_a, "rhsAndX"))
+        .def("solve", solve<Eigen::Ref<const VectorXs>>, ("self"_a, "rhs"))
+        .def("solve", solve<Eigen::Ref<const MatrixXs>>, ("self"_a, "rhs"))
         .def("matrixLDLT", &LDLTtype::matrixLDLT, policies::return_by_value,
              "self"_a,
              "Get the current value of the decomposition matrix. This makes a "

include/proxsuite-nlp/linalg/block-ldlt.hpp

@@ -194,7 +194,8 @@ template <typename Scalar> struct block_impl {
 /// over the lifetime of this object when calling compute(). A change
 /// of structure should lead to recalculating the expected sparsity pattern of
 /// the factorization, and even recomputing the sparsity-optimal permutation.
-template <typename Scalar> struct BlockLDLT : ldlt_base<Scalar> {
+template <typename _Scalar> struct BlockLDLT : ldlt_base<_Scalar> {
+  using Scalar = _Scalar;
   PROXSUITE_NLP_DYNAMIC_TYPEDEFS(Scalar);
   using Base = ldlt_base<Scalar>;
   using DView = typename Base::DView;
@@ -299,6 +300,13 @@ template <typename Scalar> struct BlockLDLT : ldlt_base<Scalar> {
   template <typename Derived>
   bool solveInPlace(Eigen::MatrixBase<Derived> &b) const;
 
+  template <typename Rhs>
+  typename Rhs::PlainObject solve(const Eigen::MatrixBase<Rhs> &rhs) const {
+    typename Rhs::PlainObject out = rhs;
+    solveInPlace(out);
+    return out;
+  }
+
   const MatrixXs &matrixLDLT() const override { return m_matrix; }
 
   inline void compute() {

include/proxsuite-nlp/linalg/bunchkaufman.hpp

@@ -23,14 +23,21 @@ ComputationInfo bunch_kaufman_in_place_unblocked(MatrixType &a,
                                                  Index &pivot_count) {
   using Scalar = typename MatrixType::Scalar;
   using Real = typename Eigen::NumTraits<Scalar>::Real;
-  Real alpha = (Real(1) + numext::sqrt(Real(17))) / Real(8);
+  const Real alpha = (Real(1) + numext::sqrt(Real(17))) / Real(8);
   using std::swap;
 
   pivot_count = 0;
 
-  Index n = a.rows();
+  const Index n = a.rows();
   if (n == 0) {
     return Success;
+  } else if (n == 1) {
+    if (numext::abs(numext::real(a(0, 0))) == Real(0)) {
+      return NumericalIssue;
+    } else {
+      a(0, 0) = Real(1) / numext::real(a(0, 0));
+      return Success;
+    }
   }
 
   Index k = 0;

include/proxsuite-nlp/linalg/dense.hpp

@@ -161,7 +161,8 @@ dense_ldlt_reconstruct(typename math_types<Scalar>::ConstMatrixRef const &mat,
 } // namespace backend
 
 /// @brief  A fast, recursive divide-and-conquer LDLT algorithm.
-template <typename Scalar> struct DenseLDLT : ldlt_base<Scalar> {
+template <typename _Scalar> struct DenseLDLT : ldlt_base<_Scalar> {
+  using Scalar = _Scalar;
   PROXSUITE_NLP_DYNAMIC_TYPEDEFS(Scalar);
   using Base = ldlt_base<Scalar>;
   using DView = typename Base::DView;
@@ -193,6 +194,13 @@ template <typename Scalar> struct DenseLDLT : ldlt_base<Scalar> {
     return backend::dense_ldlt_solve_in_place(m_matrix, b);
   }
 
+  template <typename Rhs>
+  typename Rhs::PlainObject solve(const Eigen::MatrixBase<Rhs> &rhs) const {
+    typename Rhs::PlainObject out = rhs;
+    solveInPlace(out);
+    return out;
+  }
+
   MatrixXs reconstructedMatrix() const {
     MatrixXs res(m_matrix.rows(), m_matrix.cols());
     res.setIdentity();

include/proxsuite-nlp/linalg/proxsuite-ldlt-wrap.hpp

@@ -44,7 +44,8 @@ namespace nlp {
 namespace linalg {
 
 /// @brief Use the LDLT from proxsuite.
-template <typename Scalar> struct ProxSuiteLDLTWrapper : ldlt_base<Scalar> {
+template <typename _Scalar> struct ProxSuiteLDLTWrapper : ldlt_base<_Scalar> {
+  using Scalar = _Scalar;
   PROXSUITE_NLP_DYNAMIC_TYPEDEFS(Scalar);
   using Base = ldlt_base<Scalar>;
   using psldlt_t = dense_linalg::Ldlt<Scalar>;
@@ -97,6 +98,13 @@ template <typename Scalar> struct ProxSuiteLDLTWrapper : ldlt_base<Scalar> {
     return true;
   }
 
+  template <typename Rhs>
+  typename Rhs::PlainObject solve(const Eigen::MatrixBase<Rhs> &rhs) const {
+    typename Rhs::PlainObject out = rhs;
+    solveInPlace(out);
+    return out;
+  }
+
   inline DView vectorD() const {
     return dense_linalg::util::diagonal(m_ldlt.ld_col());
   }