quaternionEKF compiles with Eigen

- Note: Tests pending.
- setSubmatrix in Eigen implementation added.
- setSubmatrix test added.
- setColumn added and tasted.
- Made Matrix_Wrapper() constructor protected as it’s more standard for
an interface.

src/wrappers/matrix/matrix_EIGEN.cpp

@@ -10,8 +10,11 @@ using namespace std;
 
 // Passing the constructor arguments...
 MyMatrix::Matrix() : EigenMatrix() {}
-MyMatrix::Matrix(int num_rows, int num_cols) : EigenMatrix(num_rows,
-                               num_cols){}
+
+MyMatrix::Matrix(int num_rows, int num_cols) : EigenMatrix(num_rows, num_cols) {};
+
+//template <typename Derived, int num_rows, int num_cols>
+//MyMatrix::Matrix<Derived, num_rows, num_cols>() : Eigen::Matrix<Derived, num_rows, num_cols> {}
 
 // Destructor
 MyMatrix::~Matrix(){}
@@ -271,6 +274,15 @@ MyMatrix::resize(unsigned int i, unsigned int j, bool copy, bool initialize)
     }
 }
 
+void MyMatrix::setSubMatrix ( const MatrixWrapper::Matrix& b, unsigned int i_start, unsigned int i_end, unsigned int j_start, unsigned int j_end )
+{
+    EigenMatrix& tmp = (*this);
+    int num_rows = (i_end - i_start) + 1;
+    int num_cols = (j_end - j_start) + 1;
+    tmp.block(i_start-1, j_start-1, num_rows, num_cols) = b;
+    // set submatrix from Eigen done here.
+}
+
 // get sub matrix
 MyMatrix MyMatrix::sub(int i_start, int i_end, int j_start , int j_end) const
 {
@@ -530,7 +542,10 @@ MyMatrix MySymmetricMatrix::sub(int i_start, int i_end, int j_start , int j_end)
   return submatrix;
 }
 
-
+void MySymmetricMatrix::setSubMatrix(const MatrixWrapper::Matrix &b, unsigned int i_start, unsigned int i_end, unsigned int j_start, unsigned int j_end)
+{
+
+}
 
 double& MySymmetricMatrix::operator()(unsigned int a, unsigned int b)
 {

src/wrappers/matrix/matrix_EIGEN.h

@@ -26,7 +26,8 @@ class Matrix : public EigenMatrix, public Matrix_Wrapper
 
   // Constructors
   Matrix();
-  Matrix(int m, int n);
+  Matrix(int num_rows, int num_cols);
+  //template<typename Derived, int m, int n> Matrix();
 
   // Destructor
   virtual ~Matrix();
@@ -84,9 +85,10 @@ class Matrix : public EigenMatrix, public Matrix_Wrapper
   virtual MyMatrix transpose() const;
   virtual double determinant() const;
   virtual int convertToSymmetricMatrix(MySymmetricMatrix& sym);
+  virtual void setSubMatrix ( const MyMatrix& b, unsigned int i_start, unsigned int i_end, unsigned int j_start, unsigned int j_end );
   virtual MyMatrix sub(int i_start, int i_end, int j_start , int j_end) const;
   virtual void setColumn(const MyColumnVector &b, int i) const;
-    virtual void setColumn(MyColumnVector &b, int j);
+  virtual void setColumn(MyColumnVector &b, int j);
 
 };
 
@@ -160,6 +162,8 @@ class SymmetricMatrix : public EigenSymmetricMatrix, public SymmetricMatrix_Wrap
 
   virtual void resize(unsigned int i, bool copy=true, bool initialize=true);
   virtual MyMatrix sub(int i_start, int i_end, int j_start , int j_end) const;
+  void setSubMatrix (const MyMatrix& b, unsigned int i_start, unsigned int i_end, unsigned int j_start, unsigned int j_end );
+
 
 };
 

src/wrappers/matrix/matrix_wrapper.h

@@ -40,11 +40,10 @@ class SymmetricMatrix;
 /// Class Matrixwrapper
 class Matrix_Wrapper
 {
+protected:
+    /// Constructor
+    Matrix_Wrapper() {};
 public:
-
-  /// Constructor
-  Matrix_Wrapper() {};
-
   /// Destructor
   virtual ~Matrix_Wrapper() {};
 
@@ -155,11 +154,14 @@ class Matrix_Wrapper
   */
   virtual int convertToSymmetricMatrix(MySymmetricMatrix& sym) = 0;
 
+  /// Set sub matrix
+  virtual void setSubMatrix ( const MatrixWrapper::Matrix& b, unsigned int i_start, unsigned int i_end, unsigned int j_start, unsigned int j_end ) = 0;
+
   /// get sub matrix
   virtual MyMatrix sub(int i_start, int i_end, int j_start , int j_end) const = 0;
 
   /// Set column
-    virtual void setColumn(MyColumnVector &b, int j) = 0;
+  virtual void setColumn(MyColumnVector &b, int j) = 0;
   virtual void setColumn(const MyColumnVector &b, int i) const = 0;
 
   /// SVD Decomposition (for pseudo-inverse properties)
@@ -289,7 +291,10 @@ class SymmetricMatrix_Wrapper
 
   /// get sub matrix
   virtual MyMatrix sub(int i_start, int i_end, int j_start , int j_end) const = 0;
-
+
+  /// Set sub matrix
+  virtual void setSubMatrix ( const MatrixWrapper::Matrix& b, unsigned int i_start, unsigned int i_end, unsigned int j_start, unsigned int j_end ) = 0;
+
   /// Cholesky Decomposition for semidefinite matrices
   virtual bool cholesky_semidefinite(MyMatrix& m) const ;
 

tests/matrixwrapper_test.cpp

@@ -798,7 +798,14 @@ void MatrixwrapperTest::testMatrixwrapperValue()
     // Print quaternion
     std::cout << colFromQuat << std::endl;
     std::cout << quat << std::endl;
-
+    // setSubmatrix
+    Matrix mat2(2,2);
+    mat2 = 1.0;
+    mat.setSubMatrix(mat2, 2, 3, 2, 3);
+    CPPUNIT_ASSERT_EQUAL(mat2(1,1), mat(2,2));
+    CPPUNIT_ASSERT_EQUAL(mat2(2,1), mat(3,2));
+    CPPUNIT_ASSERT_EQUAL(mat2(1,2), mat(2,3));
+    CPPUNIT_ASSERT_EQUAL(mat2(2,2), mat(3,3));
 
 }