From 73f147999757b1636b4726756690eed17614c4d9 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] t2 type conversions

---
 src/conversion.cpp      | 37 +++++++++++++++++++++
 src/conversion.hpp      |  6 ++++
 src/pose_estimation.cpp | 74 +++++++++++++++++++++++++++++++++++++----
 3 files changed, 110 insertions(+), 7 deletions(-)
 create mode 100644 src/conversion.cpp
 create mode 100644 src/conversion.hpp

diff --git a/src/conversion.cpp b/src/conversion.cpp
new file mode 100644
index 0000000..cdf4d1f
--- /dev/null
+++ b/src/conversion.cpp
@@ -0,0 +1,37 @@
+#include <tf2/convert.h>
+#include <opencv2/core/mat.hpp>
+
+// template<>
+// geometry_msgs::msg::Vector3
+// tf2::toMsg(const cv::Mat_<double>& vec)
+// {
+//     assert(vec.rows == 3 || vec.cols == 3);
+
+//     geometry_msgs::msg::Vector3 t;
+//     t.x = vec.at<double>(0);
+//     t.y = vec.at<double>(1);
+//     t.z = vec.at<double>(2);
+//     return t;
+// }
+
+template<>
+void
+tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
+{
+    assert(vec.rows == 3 || vec.cols == 3);
+
+    t.x = vec.at<double>(0);
+    t.y = vec.at<double>(1);
+    t.z = vec.at<double>(2);
+}
+
+// template<>
+// void
+// tf2::toMsg(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t)
+// {
+//     assert(vec.rows == 3 || vec.cols == 3);
+
+//     t.x = vec.at<double>(0);
+//     t.y = vec.at<double>(1);
+//     t.z = vec.at<double>(2);
+// }
diff --git a/src/conversion.hpp b/src/conversion.hpp
new file mode 100644
index 0000000..7919603
--- /dev/null
+++ b/src/conversion.hpp
@@ -0,0 +1,6 @@
+#include <tf2/convert.h>
+#include <opencv2/core/mat.hpp>
+
+template<>
+void
+tf2::convert(const cv::Mat_<double>& vec, geometry_msgs::msg::Vector3& t);
diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
index 979427f..ac2fb28 100644
--- a/src/pose_estimation.cpp
+++ b/src/pose_estimation.cpp
@@ -4,9 +4,44 @@
 #include <apriltag/common/homography.h>
 #include <opencv2/calib3d.hpp>
 #include <opencv2/core/quaternion.hpp>
+// #include <tf2_eigen/tf2_eigen.hpp>
+#include <tf2/convert.h>
+#include "conversion.hpp"
+
+
+// std::string& operator+ (const int k, std::string& c)
+// {
+//     size_t len = c.length();
+//     for (size_t i = 0; i < len; i++)
+//         c[i] += k;
+//     return c;
+// }
+
+// std::string& operator= (const int k)
+// {
+//     size_t len = k;
+//     for (size_t i = 0; i < len; i++)
+//         c[i] += k;
+//     return k;
+// }
+
+// template<>
+// geometry_msgs::msg::Vector3
+// toMsg(const matd_t& t)
+// {
+//     assert(vec.rows == 3 || vec.cols == 3);
+
+//     geometry_msgs::msg::Vector3 t;
+//     t.x = vec.at<double>(0);
+//     t.y = vec.at<double>(1);
+//     t.z = vec.at<double>(2);
+//     return t;
+// }
 
 
-geometry_msgs::msg::Transform tf_from_apriltag_pose(const apriltag_pose_t& pose)
+
+geometry_msgs::msg::Transform
+toMsg(const apriltag_pose_t& pose)
 {
     const Eigen::Quaterniond q(Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(pose.R->data));
 
@@ -20,18 +55,30 @@ geometry_msgs::msg::Transform tf_from_apriltag_pose(const apriltag_pose_t& pose)
     t.rotation.y = q.y();
     t.rotation.z = q.z();
 
+    // geometry_msgs::msg::Transform t = tf2::eigenToTransform(const Eigen::Isometry3d & TODO);
+
     return t;
 }
 
-geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv::Mat_<double>& rvec)
+geometry_msgs::msg::Transform
+toMsg(const cv::Mat_<double>& tvec, const cv::Mat_<double>& rvec)
 {
     const cv::Quat<double> q = cv::Quat<double>::createFromRvec(rvec);
 
+    // convert rotation vector to angle-axis to quaternion
+    const Eigen::Map<const Eigen::Vector3d> v((double*)rvec.data);
+    const Eigen::Quaterniond qq({v.norm(), v.normalized()});
+
     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.translation.x = tvec.at<double>(0);
+    // t.translation.y = tvec.at<double>(1);
+    // t.translation.z = tvec.at<double>(2);
+
+    // t.translation = tf2::toMsg<cv::Mat_<double>, geometry_msgs::msg::Vector3>(tvec);
+    // tf2::convert<cv::Mat_<double>, geometry_msgs::msg::Vector3>(tvec, t.translation);
+    tf2::convert(tvec, t.translation);
+
     t.rotation.w = q.w;
     t.rotation.x = q.x;
     t.rotation.y = q.y;
@@ -40,6 +87,19 @@ geometry_msgs::msg::Transform tf_from_cv(const cv::Mat_<double>& tvec, const cv:
     return t;
 }
 
+// geometry_msgs::msg::Transform tf_from_eigen(const Eigen::Vector3d &t, const Eigen::Quaterniond &q)
+// {
+//     geometry_msgs::msg::Transform tf;
+
+//     // tf.translation = tf2::toMsg(t);
+//     tf2::convert(t, tf.translation);
+//     // tf2::toMsg(t, tf.translation);
+//     // tf.rotation = tf2::toMsg(q);
+//     // tf2::toMsg(q, tf.rotation);
+
+//     return tf;
+// }
+
 geometry_msgs::msg::Transform
 homography(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
 {
@@ -48,7 +108,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 toMsg(pose);
 }
 
 geometry_msgs::msg::Transform
@@ -77,7 +137,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 toMsg(tvec, rvec);
 }
 
 const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods{