Merge pull request #57 from S-Dafarra/fix/force_csc

Forcing the input eigen matrix to be csc when copying it into an osqp matrix
robotology · May 14, 2020 · d27c484 · d27c484
2 parents 2eda1ea + 2dcec5e
commit d27c484
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 13 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,7 +5,7 @@
 cmake_minimum_required(VERSION 3.5)
 
 project(OsqpEigen
-  VERSION 0.6.102)
+  VERSION 0.6.103)
 
 # ouptut paths
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")

diff --git a/include/OsqpEigen/SparseMatrixHelper.tpp b/include/OsqpEigen/SparseMatrixHelper.tpp
@@ -10,18 +10,16 @@ bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(const Eigen::SparseCo
                                                              csc*& osqpSparseMatrix)
 
 {
+    Eigen::SparseMatrix<typename Derived::value_type, Eigen::ColMajor> colMajorCopy; //Copying into a new sparse matrix to be sure to use a CSC matrix
+    colMajorCopy = eigenSparseMatrix; //This may perform merory allocation, but this is already the case for allocating the osqpSparseMatrix
     // get number of row, columns and nonZeros from Eigen SparseMatrix
-    c_int rows = eigenSparseMatrix.rows();
-    c_int cols = eigenSparseMatrix.cols();
-    c_int numberOfNonZeroCoeff = eigenSparseMatrix.nonZeros();
+    c_int rows = colMajorCopy.rows();
+    c_int cols = colMajorCopy.cols();
+    c_int numberOfNonZeroCoeff = colMajorCopy.nonZeros();
 
     // get innerr and outer index
-    const int* innerIndexPtr = eigenSparseMatrix.innerIndexPtr();
-    const int* outerIndexPtr = eigenSparseMatrix.outerIndexPtr();
-    const int* innerNonZerosPtr = eigenSparseMatrix.innerNonZeroPtr();
-
-    // get nonzero values
-    auto valuePtr = eigenSparseMatrix.valuePtr();
+    const int* outerIndexPtr = colMajorCopy.outerIndexPtr();
+    const int* innerNonZerosPtr = colMajorCopy.innerNonZeroPtr();
 
     // instantiate csc matrix
     // MEMORY ALLOCATION!!
@@ -35,7 +33,7 @@ bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(const Eigen::SparseCo
 
     int innerOsqpPosition = 0;
     for(int k = 0; k < cols; k++) {
-        if (eigenSparseMatrix.isCompressed()) {
+        if (colMajorCopy.isCompressed()) {
             osqpSparseMatrix->p[k] = static_cast<c_int>(outerIndexPtr[k]);
         } else {
             if (k == 0) {
@@ -44,7 +42,7 @@ bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(const Eigen::SparseCo
                 osqpSparseMatrix->p[k] = osqpSparseMatrix->p[k-1] + innerNonZerosPtr[k-1];
             }
         }
-        for (typename Eigen::SparseCompressedBase<Derived>::InnerIterator it(eigenSparseMatrix,k); it; ++it) {
+        for (typename Eigen::SparseMatrix<typename Derived::value_type, Eigen::ColMajor>::InnerIterator it(colMajorCopy,k); it; ++it) {
             osqpSparseMatrix->i[innerOsqpPosition] = static_cast<c_int>(it.row());
             osqpSparseMatrix->x[innerOsqpPosition] = static_cast<c_float>(it.value());
             innerOsqpPosition++;

diff --git a/tests/SparseMatrixTest.cpp b/tests/SparseMatrixTest.cpp
@@ -14,16 +14,29 @@
 template<typename T, int n, int m>
 bool computeTest(const Eigen::Matrix<T, n, m> &mEigen)
 {
-    Eigen::SparseMatrix<T> matrix, newMatrix;
+    Eigen::SparseMatrix<T, Eigen::ColMajor> matrix, newMatrix, newMatrixFromCSR;
     matrix = mEigen.sparseView();
 
     csc* osqpSparseMatrix = nullptr;
     //NOTE: Dynamic memory allocation
     if(!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(matrix, osqpSparseMatrix))
         return false;
 
+    Eigen::SparseMatrix<T, Eigen::RowMajor> csrMatrix;
+    csrMatrix = matrix;
+    csc* otherOsqpSparseMatrix = nullptr;
+    if(!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(csrMatrix, otherOsqpSparseMatrix))
+        return false;
+
     if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(osqpSparseMatrix, newMatrix))
         return false;
+
+    if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(otherOsqpSparseMatrix, newMatrixFromCSR))
+        return false;
+
+    if (!newMatrixFromCSR.isApprox(newMatrix))
+        return false;
+
     std::vector<Eigen::Triplet<T>> tripletListCsc;
     if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(osqpSparseMatrix, tripletListCsc))
         return false;
@@ -41,6 +54,8 @@ bool computeTest(const Eigen::Matrix<T, n, m> &mEigen)
     bool outcome = matrix.isApprox(newMatrix);
 
     csc_spfree(osqpSparseMatrix);
+    csc_spfree(otherOsqpSparseMatrix);
+
 
     return outcome;
 }