From 3a2e5150a3a6460bce378ff03c49aea66075ea77 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Mon, 21 Sep 2026 11:14:12 +0200 Subject: [PATCH] Add Eigen conversions for Affine3 and Quat --- modules/core/include/opencv2/core/eigen.hpp | 43 ++++++++ modules/core/include/opencv2/core/private.hpp | 1 + modules/core/test/test_mat.cpp | 102 ++++++++++++++++++ 3 files changed, 146 insertions(+) diff --git a/modules/core/include/opencv2/core/eigen.hpp b/modules/core/include/opencv2/core/eigen.hpp index 5810f98af3..eb6dbcb674 100644 --- a/modules/core/include/opencv2/core/eigen.hpp +++ b/modules/core/include/opencv2/core/eigen.hpp @@ -70,6 +70,9 @@ namespace cv { +template class Quat; +template class Affine3; + /** @addtogroup core_eigen These functions are provided for OpenCV-Eigen interoperability. They convert `Mat` objects to corresponding `Eigen::Matrix` objects and vice-versa. Consult the [Eigen @@ -418,6 +421,46 @@ void cv2eigen( const Matx<_Tp, 1, _cols>& src, } } +#if defined(EIGEN_GEOMETRY_MODULE_H) +/** @brief Converts an Eigen::Quaternion to a cv::Quat. +*/ +template inline +void eigen2cv( const Eigen::Quaternion<_Tp, _options>& src, Quat<_Tp>& dst ) +{ + dst.w = src.w(); + dst.x = src.x(); + dst.y = src.y(); + dst.z = src.z(); +} + +/** @brief Converts a cv::Quat to an Eigen::Quaternion. +*/ +template inline +void cv2eigen( const Quat<_Tp>& src, Eigen::Quaternion<_Tp, _options>& dst ) +{ + dst.w() = src.w; + dst.x() = src.x; + dst.y() = src.y; + dst.z() = src.z; +} + +/** @brief Converts an Eigen::Transform (Isometry) to a cv::Affine3. +*/ +template inline +void eigen2cv( const Eigen::Transform<_Tp, 3, Eigen::Isometry, _options>& src, Affine3<_Tp>& dst ) +{ + eigen2cv(src.matrix(), dst.matrix); +} + +/** @brief Converts a cv::Affine3 to an Eigen::Transform (Isometry). +*/ +template inline +void cv2eigen( const Affine3<_Tp>& src, Eigen::Transform<_Tp, 3, Eigen::Isometry, _options>& dst ) +{ + cv2eigen(src.matrix, dst.matrix()); +} +#endif + //! @} } // cv diff --git a/modules/core/include/opencv2/core/private.hpp b/modules/core/include/opencv2/core/private.hpp index 8db9410609..0ccacd6c4f 100644 --- a/modules/core/include/opencv2/core/private.hpp +++ b/modules/core/include/opencv2/core/private.hpp @@ -68,6 +68,7 @@ # pragma warning(disable:4714) // const marked as __forceinline not inlined # endif # include +# include # if defined(_MSC_VER) # pragma warning(pop) # endif diff --git a/modules/core/test/test_mat.cpp b/modules/core/test/test_mat.cpp index e0c4074545..1948bb0656 100644 --- a/modules/core/test/test_mat.cpp +++ b/modules/core/test/test_mat.cpp @@ -6,6 +6,8 @@ #ifdef HAVE_EIGEN #include #include +#include "opencv2/core/quaternion.hpp" +#include "opencv2/core/affine.hpp" #include "opencv2/core/eigen.hpp" #endif @@ -2308,6 +2310,106 @@ TEST(Core_Eigen, cv2eigen_check_RowMajor) ASSERT_EQ(5.0, eigen_A(2, 0)); ASSERT_EQ(6.0, eigen_A(2, 1)); } + +TEST(Core_Eigen, quaternion_conversion) +{ + // Test float version + { + cv::Quatf cv_q(1.0f, 2.0f, 3.0f, 4.0f); + Eigen::Quaternionf eigen_q; + cv2eigen(cv_q, eigen_q); + EXPECT_FLOAT_EQ(cv_q.w, eigen_q.w()); + EXPECT_FLOAT_EQ(cv_q.x, eigen_q.x()); + EXPECT_FLOAT_EQ(cv_q.y, eigen_q.y()); + EXPECT_FLOAT_EQ(cv_q.z, eigen_q.z()); + + cv::Quatf cv_q_back; + eigen2cv(eigen_q, cv_q_back); + EXPECT_FLOAT_EQ(cv_q.w, cv_q_back.w); + EXPECT_FLOAT_EQ(cv_q.x, cv_q_back.x); + EXPECT_FLOAT_EQ(cv_q.y, cv_q_back.y); + EXPECT_FLOAT_EQ(cv_q.z, cv_q_back.z); + } + + // Test double version + { + cv::Quatd cv_q(1.0, 2.0, 3.0, 4.0); + Eigen::Quaterniond eigen_q; + cv2eigen(cv_q, eigen_q); + EXPECT_DOUBLE_EQ(cv_q.w, eigen_q.w()); + EXPECT_DOUBLE_EQ(cv_q.x, eigen_q.x()); + EXPECT_DOUBLE_EQ(cv_q.y, eigen_q.y()); + EXPECT_DOUBLE_EQ(cv_q.z, eigen_q.z()); + + cv::Quatd cv_q_back; + eigen2cv(eigen_q, cv_q_back); + EXPECT_DOUBLE_EQ(cv_q.w, cv_q_back.w); + EXPECT_DOUBLE_EQ(cv_q.x, cv_q_back.x); + EXPECT_DOUBLE_EQ(cv_q.y, cv_q_back.y); + EXPECT_DOUBLE_EQ(cv_q.z, cv_q_back.z); + } +} + +TEST(Core_Eigen, isometry_conversion) +{ + // Test float version + { + cv::Matx33f R = cv::Matx33f::eye(); + cv::Vec3f t(1.0f, 2.0f, 3.0f); + cv::Affine3f cv_aff(R, t); + + Eigen::Transform eigen_iso; + cv2eigen(cv_aff, eigen_iso); + + // Verify elements + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + EXPECT_FLOAT_EQ(cv_aff.matrix(i, j), eigen_iso.matrix()(i, j)); + } + } + + cv::Affine3f cv_aff_back; + eigen2cv(eigen_iso, cv_aff_back); + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + EXPECT_FLOAT_EQ(cv_aff.matrix(i, j), cv_aff_back.matrix(i, j)); + } + } + } + + // Test double version + { + cv::Matx33d R = cv::Matx33d::eye(); + cv::Vec3d t(1.0, 2.0, 3.0); + cv::Affine3d cv_aff(R, t); + + Eigen::Transform eigen_iso; + cv2eigen(cv_aff, eigen_iso); + + // Verify elements + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + EXPECT_DOUBLE_EQ(cv_aff.matrix(i, j), eigen_iso.matrix()(i, j)); + } + } + + cv::Affine3d cv_aff_back; + eigen2cv(eigen_iso, cv_aff_back); + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + EXPECT_DOUBLE_EQ(cv_aff.matrix(i, j), cv_aff_back.matrix(i, j)); + } + } + } +} #endif // HAVE_EIGEN #ifdef OPENCV_EIGEN_TENSOR_SUPPORT