Merge pull request #83 from gergondet/topic/ReductionSparse

jrl-umi3218 · Dec 8, 2022 · b38f2b8 · b38f2b8
2 parents 548fdcf + afd38b6
commit b38f2b8
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 8 deletions.
diff --git a/src/GenQPSolver.cpp b/src/GenQPSolver.cpp
@@ -56,7 +56,9 @@ void GenQPSolver::setDependencies(int nrVars, std::vector<std::tuple<int, int, d
   fullToReduced_.resize(static_cast<size_t>(nrVars), -1);
   reducedToFull_.resize(static_cast<size_t>(nrVars) - dependencies_.size(), -1);
   /* Initialize the multipliers and offset variable */
-  multipliers_ = Eigen::MatrixXd::Zero(nrVars, nrVars - static_cast<int>(dependencies.size()));
+  multipliers_ = Eigen::SparseMatrix<double>(nrVars, nrVars - static_cast<int>(dependencies.size()));
+  std::vector<Eigen::Triplet<double>> triplets;
+  triplets.reserve(static_cast<size_t>(nrVars));
   /* Number of removed variables encountered so far */
   size_t shift = 0;
   for(size_t i = 0; i < fullToReduced_.size(); ++i)
@@ -67,16 +69,17 @@ void GenQPSolver::setDependencies(int nrVars, std::vector<std::tuple<int, int, d
       continue;
     }
     fullToReduced_[i] = static_cast<int>(i - shift);
-    multipliers_(static_cast<Eigen::DenseIndex>(i), fullToReduced_[i]) = 1.0;
+    triplets.push_back({static_cast<Eigen::SparseMatrix<double>::StorageIndex>(i), fullToReduced_[i], 1.0});
     reducedToFull_[i - shift] = static_cast<int>(i);
   }
   for(const auto & d : dependencies_)
   {
     auto leader_idx = std::get<0>(d);
     auto mimic_idx = std::get<1>(d);
     auto mult = std::get<2>(d);
-    multipliers_(mimic_idx, fullToReduced_[static_cast<size_t>(leader_idx)]) = mult;
+    triplets.push_back({mimic_idx, fullToReduced_[static_cast<size_t>(leader_idx)], mult});
   }
+  multipliers_.setFromTriplets(triplets.begin(), triplets.end());
 }
 
 } // namespace qp

diff --git a/src/GenQPUtils.h b/src/GenQPUtils.h
@@ -10,6 +10,7 @@
 
 // Eigen
 #include <Eigen/Core>
+#include <Eigen/Sparse>
 
 // Tasks
 #include "Tasks/QPSolver.h"
@@ -81,7 +82,7 @@ inline void reduceQC(const Eigen::MatrixXd & QFull,
                      const Eigen::VectorXd & CFull,
                      Eigen::MatrixXd & Q,
                      Eigen::VectorXd & C,
-                     const Eigen::MatrixXd & M)
+                     const Eigen::SparseMatrix<double> & M)
 {
   Q.noalias() = M.transpose() * QFull * M;
   C.noalias() = M.transpose() * CFull;
@@ -296,7 +297,7 @@ inline void fillBound(const std::vector<Bound *> & bounds, Eigen::VectorXd & XL,
  * L \leq A M y \leq U
  * \f}
  */
-inline void reduceA(const Eigen::MatrixXd & AFull, Eigen::MatrixXd & A, const Eigen::MatrixXd & M)
+inline void reduceA(const Eigen::MatrixXd & AFull, Eigen::MatrixXd & A, const Eigen::SparseMatrix<double> & M)
 {
   A.noalias() = AFull * M;
 }
@@ -345,7 +346,7 @@ inline void reduceBound(const Eigen::VectorXd & XLFull,
  */
 inline void expandResult(const Eigen::VectorXd & result,
                          Eigen::VectorXd & resultFull,
-                         const Eigen::MatrixXd & multipliers)
+                         const Eigen::SparseMatrix<double> & multipliers)
 {
   resultFull.noalias() = multipliers * result;
 }

diff --git a/src/Tasks/GenQPSolver.h b/src/Tasks/GenQPSolver.h
@@ -8,10 +8,12 @@
 // std
 #include <vector>
 
-// Eigen
+// Tasks
 #include <tasks/config.hh>
 
+// Eigen
 #include <Eigen/Core>
+#include <Eigen/Sparse>
 
 // forward declaration
 // RBDyn
@@ -116,7 +118,7 @@ class TASKS_DLLAPI GenQPSolver
    * the full variable and the factor and offset in the dependency equation: replica = factor * primary */
   std::vector<std::tuple<int, int, double>> dependencies_;
   /** Multipliers matrix M of size (nFull, nReduced) such that full = M * reduce */
-  Eigen::MatrixXd multipliers_;
+  Eigen::SparseMatrix<double> multipliers_;
 };
 
 } // namespace qp