From 00300846d58ac9915a4c8d57cafc1aa9059ccf3d Mon Sep 17 00:00:00 2001
From: Ted Steiner <tsteiner2@gmail.com>
Date: Fri, 6 Sep 2024 17:13:48 -0400
Subject: [PATCH 1/3] Use TF2 package for quaternion conversion

The OpenCV quaternion class was not added until OpenCV 4.5.1, so it's less widely available than the TF2 conversion.
---
 image_proc/package.xml          |  2 ++
 image_proc/src/track_marker.cpp | 11 +++++------
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/image_proc/package.xml b/image_proc/package.xml
index 9457ed952..5df57c4d6 100644
--- a/image_proc/package.xml
+++ b/image_proc/package.xml
@@ -30,6 +30,8 @@
   <depend>rclcpp_components</depend>
   <depend>rcutils</depend>
   <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>tracetools_image_pipeline</depend>
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp
index e103de067..7150870ba 100644
--- a/image_proc/src/track_marker.cpp
+++ b/image_proc/src/track_marker.cpp
@@ -40,9 +40,10 @@
 #include <image_proc/track_marker.hpp>
 #include <image_proc/utils.hpp>
 #include <image_transport/image_transport.hpp>
-#include <opencv2/core/quaternion.hpp>
 #include <rclcpp/qos.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/LinearMath/Quaternion.h>
 
 namespace image_proc
 {
@@ -135,11 +136,9 @@ void TrackMarkerNode::imageCb(
       pose.pose.position.y = tvecs[0][1];
       pose.pose.position.z = tvecs[0][2];
       // Convert angle-axis to quaternion
-      cv::Quatd q = cv::Quatd::createFromRvec(rvecs[0]);
-      pose.pose.orientation.x = q.x;
-      pose.pose.orientation.y = q.y;
-      pose.pose.orientation.z = q.z;
-      pose.pose.orientation.w = q.w;
+      const tf2::Vector3 rvec(rvecs[0][0], rvecs[0][1], rvecs[0][2]);
+      const tf2::Quaternion q(rvec.normalized(), rvec.length());
+      tf2::convert(q, pose.pose.orientation);
       pub_->publish(pose);
     }
   }

From b71cb0ab7d90b5d2603cb3f9ec2d10bb89f71679 Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Tue, 29 Oct 2024 13:36:31 -0400
Subject: [PATCH 2/3] reorder includes to fix tests

---
 image_proc/src/track_marker.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp
index 7150870ba..e1fabbe6a 100644
--- a/image_proc/src/track_marker.cpp
+++ b/image_proc/src/track_marker.cpp
@@ -36,6 +36,7 @@
 #include <string>
 #include <vector>
 
+#include <tf2/LinearMath/Quaternion.h>
 #include <cv_bridge/cv_bridge.hpp>
 #include <image_proc/track_marker.hpp>
 #include <image_proc/utils.hpp>
@@ -43,7 +44,6 @@
 #include <rclcpp/qos.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2/LinearMath/Quaternion.h>
 
 namespace image_proc
 {

From abb96f7c660eba1ecf06eeebfa9a664b2542957f Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Tue, 29 Oct 2024 13:45:35 -0400
Subject: [PATCH 3/3] one more time

---
 image_proc/src/track_marker.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp
index e1fabbe6a..861774388 100644
--- a/image_proc/src/track_marker.cpp
+++ b/image_proc/src/track_marker.cpp
@@ -30,13 +30,14 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#include <tf2/LinearMath/Quaternion.h>
+
 #include <cstddef>
 #include <functional>
 #include <memory>
 #include <string>
 #include <vector>
 
-#include <tf2/LinearMath/Quaternion.h>
 #include <cv_bridge/cv_bridge.hpp>
 #include <image_proc/track_marker.hpp>
 #include <image_proc/utils.hpp>