Merge pull request #92 from jcarpent/devel

Fix heap allocation in dense solver
Simple-Robotics · Nov 8, 2022 · 68d881a · 68d881a
2 parents c4aaad3 + 2ec747d
commit 68d881a
Show file tree

Hide file tree

Showing 5 changed files with 30 additions and 14 deletions.
diff --git a/include/proxsuite/linalg/dense/core.hpp b/include/proxsuite/linalg/dense/core.hpp
@@ -792,6 +792,12 @@ noalias_mul_add_impl(Dst dst, Lhs lhs, Rhs rhs, T factor)
     return;
   }
 
+#if !EIGEN_VERSION_AT_LEAST(3, 3, 8)
+#define LAZY_PRODUCT(a, b) a.lazyProduct(b)
+#else
+#define LAZY_PRODUCT(a, b) a.operator*(b)
+#endif
+
 #if !EIGEN_VERSION_AT_LEAST(3, 3, 8)
   if ((dst.rows() < 20) && (dst.cols() < 20) && (rhs.rows() < 20)) {
     // gemm
@@ -804,12 +810,14 @@ noalias_mul_add_impl(Dst dst, Lhs lhs, Rhs rhs, T factor)
 
     auto dst_ =
       MapMut(dst.data(), dst.rows(), dst.cols(), { dst.outerStride() });
-    dst_.noalias().operator+=(lhs.operator*(rhs).operator*(factor));
+    dst_.noalias().operator+=(factor * LAZY_PRODUCT(lhs, rhs));
   } else
 #endif
   {
-    dst.noalias().operator+=(lhs.operator*(rhs).operator*(factor));
+    dst.noalias().operator+=(factor * LAZY_PRODUCT(lhs, rhs));
   }
+
+#undef LAZY_PRODUCT
 }
 } // namespace _detail
 namespace util {

diff --git a/include/proxsuite/linalg/veg/memory/alloc.hpp b/include/proxsuite/linalg/veg/memory/alloc.hpp
@@ -1,6 +1,7 @@
 #ifndef VEG_ALLOC_HPP_TAWYRUICS
 #define VEG_ALLOC_HPP_TAWYRUICS
 
+#include "proxsuite/fwd.hpp"
 #include "proxsuite/linalg/veg/ref.hpp"
 #include "proxsuite/linalg/veg/type_traits/constructible.hpp"
 #include "proxsuite/linalg/veg/type_traits/assignable.hpp"
@@ -168,15 +169,15 @@ aligned_alloc(usize align, usize size) noexcept -> void*
 #if defined(_WIN32)
   return _aligned_malloc((size + mask) & ~mask, align);
 #elif defined(__APPLE__)
-#if defined(PROXSUITE_WITH_CPP_17)
+#ifdef PROXSUITE_WITH_CPP_17
   return alignment::aligned_alloc(align, (size + mask) & ~mask);
-#elif defined(PROXSUITE_WITH_CPP_14)
+#else
   return alignment::detail::aligned_alloc(align, (size + mask) & ~mask);
 #endif
 #else
 #ifdef PROXSUITE_WITH_CPP_17
   return std::aligned_alloc(align, (size + mask) & ~mask);
-#elif defined(PROXSUITE_WITH_CPP_14)
+#else
   return alignment::detail::aligned_alloc(align, (size + mask) & ~mask);
 #endif
 #endif

diff --git a/include/proxsuite/proxqp/dense/helpers.hpp b/include/proxsuite/proxqp/dense/helpers.hpp
@@ -87,7 +87,7 @@ setup_factorization(Workspace<T>& qpwork,
     .segment(qpmodel.dim, qpmodel.n_eq)
     .setConstant(-qpresults.info.mu_eq);
 
-  qpwork.ldl.factorize(qpwork.kkt, stack);
+  qpwork.ldl.factorize(qpwork.kkt.transpose(), stack);
 }
 /*!
  * Performs the equilibration of the QP problem for reducing its

diff --git a/include/proxsuite/proxqp/dense/solver.hpp b/include/proxsuite/proxqp/dense/solver.hpp
@@ -55,7 +55,7 @@ refactorize(const Model<T>& qpmodel,
   proxsuite::linalg::veg::dynstack::DynStackMut stack{
     proxsuite::linalg::veg::from_slice_mut, qpwork.ldl_stack.as_mut()
   };
-  qpwork.ldl.factorize(qpwork.kkt, stack);
+  qpwork.ldl.factorize(qpwork.kkt.transpose(), stack);
 
   isize n = qpmodel.dim;
   isize n_eq = qpmodel.n_eq;

diff --git a/include/proxsuite/proxqp/dense/views.hpp b/include/proxsuite/proxqp/dense/views.hpp
@@ -1117,6 +1117,12 @@ noalias_mul_add(MatrixViewMut<T, colmajor> dst,
     return;
   }
 
+#if !EIGEN_VERSION_AT_LEAST(3, 3, 8)
+#define LAZY_PRODUCT(a, b) a.lazyProduct(b)
+#else
+#define LAZY_PRODUCT(a, b) a.operator*(b)
+#endif
+
   if (dst.cols == 1 && dst.rows == 1) {
     // dot
     auto rhs_col = rhs.col(0);
@@ -1133,7 +1139,7 @@ noalias_mul_add(MatrixViewMut<T, colmajor> dst,
     auto rhs_col = rhs.col(0);
     auto dst_col = dst.col(0);
     dst_col.to_eigen().noalias().operator+=(
-      factor * (lhs.to_eigen().operator*(rhs_col.to_eigen())));
+      factor * LAZY_PRODUCT(lhs.to_eigen(), rhs_col.to_eigen()));
   }
 
 #if !EIGEN_VERSION_AT_LEAST(3, 3, 8)
@@ -1151,19 +1157,20 @@ noalias_mul_add(MatrixViewMut<T, colmajor> dst,
       .noalias()
       .
       operator+=(
-        Map(lhs.data, lhs.rows, lhs.cols, Stride(lhs.outer_stride))
-          .
-          operator*(Map(rhs.data, rhs.rows, rhs.cols, Stride(rhs.outer_stride))
-                      .
-                      operator*(factor)));
+        factor *
+        LAZY_PRODUCT(
+          Map(lhs.data, lhs.rows, lhs.cols, Stride(lhs.outer_stride)),
+          Map(rhs.data, rhs.rows, rhs.cols, Stride(rhs.outer_stride))));
   }
 #endif
 
   else {
     // gemm
     dst.to_eigen().noalias().operator+=(
-      lhs.to_eigen().operator*(rhs.to_eigen().operator*(factor)));
+      factor * LAZY_PRODUCT(lhs.to_eigen(), rhs.to_eigen()));
   }
+
+#undef LAZY_PRODUCT
 }
 
 template<typename T>