add tf2 conversion library and replace cv::Quat

CMakeLists.txt

@@ -41,7 +41,7 @@ find_package(image_transport REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(Threads REQUIRED)
-find_package(OpenCV REQUIRED COMPONENTS core calib3d)
+find_package(OpenCV REQUIRED COMPONENTS core)
 find_package(apriltag 3.2 REQUIRED)
 
 if(cv_bridge_VERSION VERSION_GREATER_EQUAL 3.3.0)
@@ -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
@@ -70,9 +88,11 @@ target_link_libraries(pose_estimation
     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

src/conversion.cpp

@@ -0,0 +1,72 @@
+#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 <opencv2/core/quaternion.hpp>
+#include <tf2/convert.h>
+
+template<>
+void tf2::convert(const Eigen::Quaterniond& eq, geometry_msgs::msg::Quaternion& gq)
+{
+    gq.w = eq.w();
+    gq.x = eq.x();
+    gq.y = eq.y();
+    gq.z = eq.z();
+}
+
+template<>
+void tf2::convert(const matd_t& mat, geometry_msgs::msg::Vector3& t)
+{
+    assert(mat.nrows == 3 || mat.ncols == 3);
+
+    t.x = mat.data[0];
+    t.y = mat.data[1];
+    t.z = mat.data[2];
+}
+
+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<>
+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)
+{
+    const cv::Quat<double> cvq = cv::Quat<double>::createFromRvec(pose.second);
+
+    std::cout << "cv: " << cvq << std::endl;
+
+    assert(pose.first.rows == 3 || pose.first.cols == 3);
+    assert(pose.second.rows == 3 || 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()});
+
+    std::cout << "eigen (x,y,z,w): " << q.coeffs().transpose() << std::endl;
+
+    geometry_msgs::msg::Transform t;
+    tf2::convert(pose.first, t.translation);
+    tf2::convert(q, t.rotation);
+    return t;
+}

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{