From 73defb8faae9fdf4eae0cbee16d671c4a0fc3078 Mon Sep 17 00:00:00 2001
From: Christian Rauch <Rauch.Christian@gmx.de>
Date: Tue, 2 Jul 2024 23:23:37 +0200
Subject: [PATCH] fix orientation of tag frame estimated by homography method

---
 src/pose_estimation.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp
index e07cfbf..35a9e11 100644
--- a/src/pose_estimation.cpp
+++ b/src/pose_estimation.cpp
@@ -14,6 +14,14 @@ homography(apriltag_detection_t* const detection, const std::array<double, 4>& i
     apriltag_pose_t pose;
     estimate_pose_for_tag_homography(&info, &pose);
 
+    // rotate frame such that z points in the opposite direction towards the camera
+    for(int i = 0; i < 3; i++) {
+        // swap x and y axes
+        std::swap(MATD_EL(pose.R, 0, i), MATD_EL(pose.R, 1, i));
+        // invert z axis
+        MATD_EL(pose.R, 2, i) *= -1;
+    }
+
     return tf2::toMsg<apriltag_pose_t, geometry_msgs::msg::Transform>(const_cast<const apriltag_pose_t&>(pose));
 }