diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h index 787ae30a23f097821989fcd9658a3fa31d6b4761..4a5a02e56edc8fae399b2a216b4ee77a6f4536d8 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: /**