diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp index e266d962d5b1b2d6bc691354f5a80b1b4c38761d..faa6fadc1fbf5f3776ca73ee2363ff980713b41e 100644 --- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp +++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp @@ -53,4 +53,44 @@ namespace armarx::aron::converter { return ConvertToMatrix<double, 4, 4>(n); } + + Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const datanavigator::NDArrayNavigator& n) + { + return ConvertToQuaternion<float>(n); + } + Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const datanavigator::NDArrayNavigator& n) + { + return ConvertToQuaternion<double>(n); + } + Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const datanavigator::NDArrayNavigator& n) + { + return ConvertToVector<float, 3>(n); + } + Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const datanavigator::NDArrayNavigator& n) + { + return ConvertToVector<double, 3>(n); + } + Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const datanavigator::NDArrayNavigator& n) + { + return ConvertToMatrix<float, 4, 4>(n); + } + Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const datanavigator::NDArrayNavigator& n) + { + return ConvertToMatrix<double, 4, 4>(n); + } + + void AronEigenConverter::checkDimensions( + const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected, + const std::string& method, const std::string& caller) + { + if (nav.getDimensions() != expected) + { + std::stringstream ss; + ss << "The size of an NDArray does not match."; + ss << "\n Expected: \t" << datanavigator::NDArrayNavigator::DimensionsToString(expected); + ss << "\n Got: \t" << datanavigator::NDArrayNavigator::DimensionsToString(nav.getDimensions()); + ss << "\n"; + throw error::AronException(caller, method, ss.str(), nav.getPath()); + } + } } diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h index 06a973559c24b42dd306b62f474e12a40f0badfa..787ae30a23f097821989fcd9658a3fa31d6b4761 100644 --- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h +++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h @@ -43,62 +43,85 @@ namespace armarx::aron::converter public: // TODO: Add inplace methods - // TODO: Remove Ptrs for references static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigatorPtr&); + static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigator&); static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigatorPtr&); + static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigator&); static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigatorPtr&); + static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigator&); static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigatorPtr&); + static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigator&); static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigatorPtr&); + static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigator&); static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigatorPtr&); + static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigator&); + template<typename T> static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigatorPtr& nav) { ARMARX_CHECK_NOT_NULL(nav); + return ConvertToQuaternion<T>(*nav); + } - if (nav->getDimensions() != std::vector<int>({1, 4, sizeof(T)})) - { - throw error::AronException("AronEigenConverter", "ConvertToQuaternion", "The size of an NDArray does not match.", nav->getPath()); - } - auto dims = nav->getDimensions(); + template<typename T> + static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigator& nav) + { + checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion"); + auto dims = nav.getDimensions(); 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>())); + memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>())); return ret; } + template<typename T, int Size> static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav) { ARMARX_CHECK_NOT_NULL(nav); + return ConvertToVector<T, Size>(*nav); + } - if (nav->getDimensions() != std::vector<int>({Size, 1, sizeof(T)})) - { - throw error::AronException("AronEigenConverter", "ConvertToVector", "The size of an NDArray does not match.", nav->getPath()); - } - auto dims = nav->getDimensions(); + template<typename T, int Size> + static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigator& nav) + { + checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector"); + auto dims = nav.getDimensions(); 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>())); + 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, int Rows, int Cols> static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigatorPtr& nav) { ARMARX_CHECK_NOT_NULL(nav); + return ConvertToMatrix<T, Rows, Cols>(*nav); + } - if (nav->getDimensions() != std::vector<int>({Rows, Cols, sizeof(T)})) - { - throw error::AronException("AronEigenConverter", "ConvertToMatrix", "The size of an NDArray does not match.", nav->getPath()); - } - auto dims = nav->getDimensions(); + template<typename T, int Rows, int Cols> + static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav) + { + checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix"); + auto dims = nav.getDimensions(); 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>())); + memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>())); return ret; } + + private: + + /** + * @throw `error::AronException` If `nav`'s dimensions do not match `expected`. + */ + static void checkDimensions(const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected, + const std::string& method, const std::string& caller = "AronEigenConverter"); + }; }