eigen converter: convert to dynamic matrix

source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h

@@ -28,6 +28,7 @@
 // Eigen
 #include <Eigen/Geometry>
 #include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
 
 // ArmarX
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -103,9 +104,26 @@ namespace armarx::aron::converter
             return ConvertToMatrix<T, Rows, Cols>(*nav);
         }
 
-        template<typename T, int Rows, int Cols>
+        template<typename T>
+        static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const datanavigator::NDArrayNavigator& nav)
+        {
+            const auto dims = nav.getDimensions();
+
+            using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+            MatrixT ret;
+            memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+            return ret;
+        }
+
+        template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
         static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
         {
+            if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+            {
+                return ConvertToDynamicMatrix<T>(nav);
+            }
+
             checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
             auto dims = nav.getDimensions();
 
@@ -114,6 +132,61 @@ namespace armarx::aron::converter
             return ret;
         }
 
+        template<typename T>
+        static datanavigator::NDArrayNavigatorPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+        {
+            datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+            ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+            ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+            return ndArr;
+        }
+
+
+        // Eigen::Array
+
+        template<typename T>
+        static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
+        {
+            const auto dims = nav.getDimensions();
+
+            using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+            ArrayT ret;
+            memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+            return ret;
+        }
+
+        template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
+        static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const datanavigator::NDArrayNavigator& nav)
+        {
+            if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+            {
+                return ConvertToDynamicArray<T>(nav);
+            }
+
+            checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+            auto dims = nav.getDimensions();
+
+            Eigen::Array<T, Rows, Cols> ret;
+            memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+            return ret;
+        }
+
+        template<typename T>
+        static datanavigator::NDArrayNavigatorPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+        {
+            datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+            ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+            ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+            return ndArr;
+        }
+
+
+
     private:
 
         /**