From 78de4b8283f229079b9641cc752467787313a93c Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Sun, 2 Jun 2024 15:19:31 +0200
Subject: [PATCH] replace cv::Quat and add tf2 conversion template
 specialisations

---
 CMakeLists.txt          | 21 ++++++++++++-
 src/conversion.cpp      | 65 +++++++++++++++++++++++++++++++++++++++++
 src/pose_estimation.cpp | 42 +++-----------------------
 3 files changed, 89 insertions(+), 39 deletions(-)
 create mode 100644 src/conversion.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1017960..47d991a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -61,6 +61,24 @@ set_property(TARGET tags PROPERTY
 )
 
 
+# conversion functions as template specialisation
+add_library(conversion STATIC
+    src/conversion.cpp
+)
+target_link_libraries(conversion
+    apriltag::apriltag
+    Eigen3::Eigen
+    opencv_core
+)
+ament_target_dependencies(conversion
+    geometry_msgs
+    tf2
+)
+set_property(TARGET conversion PROPERTY
+    POSITION_INDEPENDENT_CODE ON
+)
+
+
 # pose estimation methods
 add_library(pose_estimation STATIC
     src/pose_estimation.cpp
@@ -68,11 +86,12 @@ add_library(pose_estimation STATIC
 target_link_libraries(pose_estimation
     apriltag::apriltag
     Eigen3::Eigen
-    opencv_core
     opencv_calib3d
+    conversion
 )
 ament_target_dependencies(pose_estimation
     geometry_msgs
+    tf2
 )
 set_property(TARGET pose_estimation PROPERTY
     POSITION_INDEPENDENT_CODE ON
diff --git a/src/conversion.cpp b/src/conversion.cpp
new file mode 100644
index 0000000..b9b30a8
--- /dev/null
+++ b/src/conversion.cpp
@@ -0,0 +1,65 @@
+#include <Eigen/Geometry>
+#include <apriltag/common/matd.h>
+#include <apriltag_pose.h>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <opencv2/core/mat.hpp>
+#include <tf2/convert.h>
+
+template<>
+void tf2::convert(const Eigen::Quaterniond& eigen_quat, geometry_msgs::msg::Quaternion& msg_quat)
+{
+    msg_quat.w = eigen_quat.w();
+    msg_quat.x = eigen_quat.x();
+    msg_quat.y = eigen_quat.y();
+    msg_quat.z = eigen_quat.z();
+}
+
+template<>
+void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& msg_vec)
+{
+    assert((mat.nrows == 3 && mat.ncols == 1) || (mat.nrows == 1 && mat.ncols == 3));
+
+    msg_vec.x = mat.data[0];
+    msg_vec.y = mat.data[1];
+    msg_vec.z = mat.data[2];
+}
+
+template<>
+void tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& msg_vec)
+{
+    assert((vec.rows == 3 && vec.cols == 1) || (vec.rows == 1 && vec.cols == 3));
+
+    msg_vec.x = vec.at<double>(0);
+    msg_vec.y = vec.at<double>(1);
+    msg_vec.z = vec.at<double>(2);
+}
+
+template<>
+geometry_msgs::msg::Transform
+tf2::toMsg(const apriltag_pose_t& pose)
+{
+    const Eigen::Quaterniond q(Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(pose.R->data));
+
+    geometry_msgs::msg::Transform t;
+    tf2::convert(*pose.t, t.translation);
+    tf2::convert(q, t.rotation);
+    return t;
+}
+
+template<>
+geometry_msgs::msg::Transform
+tf2::toMsg(const std::pair<cv::Mat_<double>, cv::Mat_<double>>& pose)
+{
+    assert((pose.first.rows == 3 && pose.first.cols == 1) || (pose.first.rows == 1 && pose.first.cols == 3));
+    assert((pose.second.rows == 3 && pose.second.cols == 1) || (pose.second.rows == 1 && pose.second.cols == 3));
+
+    // convert compact rotation vector to angle-axis to quaternion
+    const Eigen::Map<const Eigen::Vector3d> rvec(reinterpret_cast<double*>(pose.second.data));
+    const Eigen::Quaterniond q({rvec.norm(), rvec.normalized()});
+
+    geometry_msgs::msg::Transform t;
+    tf2::convert(pose.first, t.translation);
+    tf2::convert(q, t.rotation);
+    return t;
+}
diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
index 979427f..e07cfbf 100644
--- a/src/pose_estimation.cpp
+++ b/src/pose_estimation.cpp
@@ -1,45 +1,11 @@
 #include "pose_estimation.hpp"
-#include <Eigen/Dense>
+#include <Eigen/Geometry>
 #include <apriltag/apriltag_pose.h>
 #include <apriltag/common/homography.h>
 #include <opencv2/calib3d.hpp>
-#include <opencv2/core/quaternion.hpp>
+#include <tf2/convert.h>
 
 
-geometry_msgs::msg::Transform tf_from_apriltag_pose(const apriltag_pose_t& pose)
-{
-    const Eigen::Quaterniond q(Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(pose.R->data));
-
-    geometry_msgs::msg::Transform t;
-
-    t.translation.x = pose.t->data[0];
-    t.translation.y = pose.t->data[1];
-    t.translation.z = pose.t->data[2];
-    t.rotation.w = q.w();
-    t.rotation.x = q.x();
-    t.rotation.y = q.y();
-    t.rotation.z = q.z();
-
-    return t;
-}
-
-geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv::Mat_<double>& rvec)
-{
-    const cv::Quat<double> q = cv::Quat<double>::createFromRvec(rvec);
-
-    geometry_msgs::msg::Transform t;
-
-    t.translation.x = tvec.at<double>(0);
-    t.translation.y = tvec.at<double>(1);
-    t.translation.z = tvec.at<double>(2);
-    t.rotation.w = q.w;
-    t.rotation.x = q.x;
-    t.rotation.y = q.y;
-    t.rotation.z = q.z;
-
-    return t;
-}
-
 geometry_msgs::msg::Transform
 homography(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
 {
@@ -48,7 +14,7 @@ homography(apriltag_detection_t* const detection, const std::array<double, 4>& i
     apriltag_pose_t pose;
     estimate_pose_for_tag_homography(&info, &pose);
 
-    return tf_from_apriltag_pose(pose);
+    return tf2::toMsg<apriltag_pose_t, geometry_msgs::msg::Transform>(const_cast<const apriltag_pose_t&>(pose));
 }
 
 geometry_msgs::msg::Transform
@@ -77,7 +43,7 @@ pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, do
     cv::Mat rvec, tvec;
     cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);
 
-    return tf_from_cv(tvec, rvec);
+    return tf2::toMsg<std::pair<cv::Mat_<double>, cv::Mat_<double>>, geometry_msgs::msg::Transform>(std::make_pair(tvec, rvec));
 }
 
 const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods{