replace cv::Quat and add dedicated conversion library

CMakeLists.txt

@@ -41,27 +41,85 @@ find_package(image_transport REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(Threads REQUIRED)
-find_package(OpenCV REQUIRED)
+find_package(OpenCV REQUIRED COMPONENTS core calib3d)
 find_package(apriltag 3.2 REQUIRED)
 
 if(cv_bridge_VERSION VERSION_GREATER_EQUAL 3.3.0)
     add_compile_definitions(cv_bridge_HPP)
 endif()
 
-add_library(tags OBJECT src/tag_functions.cpp)
-target_link_libraries(tags apriltag::apriltag)
-set_property(TARGET tags PROPERTY POSITION_INDEPENDENT_CODE ON)
 
-add_library(pose_estimation OBJECT src/pose_estimation.cpp)
-target_include_directories(pose_estimation PRIVATE ${OpenCV_INCLUDE_DIRS})
-target_link_libraries(pose_estimation apriltag::apriltag Eigen3::Eigen ${OpenCV_LIBS})
-set_property(TARGET pose_estimation PROPERTY POSITION_INDEPENDENT_CODE ON)
-ament_target_dependencies(pose_estimation tf2_ros)
+# database of tag functions
+add_library(tags STATIC
+    src/tag_functions.cpp
+)
+target_link_libraries(tags
+    apriltag::apriltag
+)
+set_property(TARGET tags PROPERTY
+    POSITION_INDEPENDENT_CODE ON
+)
+
+
+# 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
+)
+target_link_libraries(pose_estimation
+    apriltag::apriltag
+    Eigen3::Eigen
+    opencv_calib3d
+    conversion
+)
+ament_target_dependencies(pose_estimation
+    geometry_msgs
+    tf2
+)
+set_property(TARGET pose_estimation PROPERTY
+    POSITION_INDEPENDENT_CODE ON
+)
 
-add_library(AprilTagNode SHARED src/AprilTagNode.cpp)
-ament_target_dependencies(AprilTagNode rclcpp rclcpp_components sensor_msgs apriltag_msgs tf2_ros image_transport cv_bridge)
-target_link_libraries(AprilTagNode apriltag::apriltag tags pose_estimation Eigen3::Eigen)
-rclcpp_components_register_node(AprilTagNode PLUGIN "AprilTagNode" EXECUTABLE "apriltag_node")
+
+# composable node
+add_library(AprilTagNode SHARED
+    src/AprilTagNode.cpp
+)
+ament_target_dependencies(AprilTagNode
+    rclcpp
+    rclcpp_components
+    sensor_msgs
+    apriltag_msgs
+    tf2_ros
+    image_transport
+    cv_bridge
+)
+target_link_libraries(AprilTagNode
+    apriltag::apriltag
+    tags
+    pose_estimation
+)
+rclcpp_components_register_node(AprilTagNode
+    PLUGIN "AprilTagNode"
+    EXECUTABLE "apriltag_node"
+)
 
 ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH})
 
@@ -70,9 +128,13 @@ install(TARGETS AprilTagNode
     LIBRARY DESTINATION lib
 )
 
-install(DIRECTORY cfg/ DESTINATION share/${PROJECT_NAME}/cfg)
+install(DIRECTORY cfg
+    DESTINATION share/${PROJECT_NAME}
+)
 
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+)
 
 if(BUILD_TESTING)
   set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_SOURCE_DIR}/.clang-format")

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>apriltag_ros</name>
-  <version>3.2.1</version>
+  <version>3.2.2</version>
   <description>AprilTag detection node</description>
   <maintainer email="[email protected]">Christian Rauch</maintainer>
   <license>MIT</license>

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;
+}

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{