Merge pull request #30020 from vrabaud:eigen

Add Eigen conversions for Affine3 and Quat

modules/core/include/opencv2/core/eigen.hpp

@@ -70,6 +70,9 @@
 namespace cv
 {
 
+template<typename _Tp> class Quat;
+template<typename _Tp> 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<typename _Tp, int _options> 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<typename _Tp, int _options> 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<typename _Tp, int _options> 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<typename _Tp, int _options> inline
+void cv2eigen( const Affine3<_Tp>& src, Eigen::Transform<_Tp, 3, Eigen::Isometry, _options>& dst )
+{
+    cv2eigen(src.matrix, dst.matrix());
+}
+#endif
+
 //! @}
 
 } // cv

modules/core/include/opencv2/core/private.hpp

@@ -68,6 +68,7 @@
 #    pragma warning(disable:4714)  // const marked as __forceinline not inlined
 #  endif
 #  include <Eigen/Core>
+#  include <Eigen/Geometry>
 #  if defined(_MSC_VER)
 #    pragma warning(pop)
 #  endif

modules/core/test/test_mat.cpp

@@ -6,6 +6,8 @@
 #ifdef HAVE_EIGEN
 #include <Eigen/Core>
 #include <Eigen/Dense>
+#include "opencv2/core/quaternion.hpp"
+#include "opencv2/core/affine.hpp"
 #include "opencv2/core/eigen.hpp"
 #endif
 
@@ -2325,6 +2327,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<float, 3, Eigen::Isometry> 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<double, 3, Eigen::Isometry> 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