From bcce53f895230a1958ecc27a8bb3f01676a5e250 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Jun 2021 08:50:23 +0200
Subject: [PATCH] eigen converter: convert to dynamic matrix
---
.../aron/converter/eigen/EigenConverter.h | 75 ++++++++++++++++++-
1 file changed, 74 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 787ae30a2..4a5a02e56 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/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:
/**
--
GitLab