diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h index d1e5c6cd52cd9e28324692b61d1cf4651d73377c..6c45ffbdb7400b4d3bbc365559c3fa6563e1fd89 100644 --- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h +++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h @@ -72,8 +72,7 @@ namespace armarx::aron::converter checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion"); auto dims = nav.getShape(); - Eigen::Quaternion<T> ret; - memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>())); + Eigen::Map<Eigen::Quaternion<T>> ret(reinterpret_cast<T*>(nav.getData())); return ret; } @@ -85,14 +84,14 @@ namespace armarx::aron::converter return ConvertToVector<T, Size>(*nav); } + template<typename T, int Size> static Eigen::Matrix<T, Size, 1> ConvertToVector(const data::NDArray& nav) { checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector"); auto dims = nav.getShape(); - Eigen::Matrix<T, Size, 1> ret; - memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>())); + Eigen::Map<Eigen::Matrix<T, Size, 1>> ret(reinterpret_cast<T*>(nav.getData())); return ret; } @@ -104,19 +103,22 @@ namespace armarx::aron::converter return ConvertToMatrix<T, Rows, Cols>(*nav); } + template<typename T> static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const data::NDArray& nav) { - const auto dims = nav.getShape(); - using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>; + const auto dims = nav.getShape(); ARMARX_CHECK_EQUAL(dims.size(), 2); // for now ... - MatrixT ret(dims.at(0), dims.at(1)); - memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<>())); - return ret; + + Eigen::Map<MatrixT> map( + reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1) + ); + return map; } + template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic> static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const data::NDArray& nav) { @@ -124,17 +126,19 @@ namespace armarx::aron::converter { return ConvertToDynamicMatrix<T>(nav); } + else + { + checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix"); - checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix"); - auto dims = nav.getShape(); - - Eigen::Matrix<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; + Eigen::Map<Eigen::Matrix<T, Rows, Cols>> map(reinterpret_cast<T*>(nav.getData())); + return map; + } } + template<typename T> - static data::NDArrayPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat) + static data::NDArrayPtr ConvertFromMatrix( + const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat) { data::NDArrayPtr ndArr(new data::NDArray); @@ -160,7 +164,7 @@ namespace armarx::aron::converter } template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic> - static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const data::NDArray& nav) + static Eigen::Array<T, Rows, Cols> ConvertToArray(const data::NDArray& nav) { if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic) { @@ -170,8 +174,7 @@ namespace armarx::aron::converter checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix"); auto dims = nav.getShape(); - 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>())); + Eigen::Map<Eigen::Array<T, Rows, Cols>> ret(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1)); return ret; }