pose_estimation_method parameter introduced different orientation for…

… pose when homography is used (the old method's z-axis flipped)

README.md

@@ -33,6 +33,7 @@ apriltag:                 # node name
     family: 36h11         # tag family name: 16h5, 25h9, 36h11
     size: 1.0             # default tag edge size in meter
     profile: false        # print profiling information to stdout
+    z_up: true            # rotate about x-axis to have Z pointing upwards
 
     # tuning of detection (defaults)
     max_hamming: 0        # maximum allowed hamming distance (corrected bits)

cfg/tags_36h11.yaml

@@ -4,6 +4,7 @@
         family: 36h11           # tag family name
         size: 0.173             # tag edge size in meter
         max_hamming: 0          # maximum allowed hamming distance (corrected bits)
+        z_up: True              # rotate about x-axis to have Z pointing upwards
 
         # see "apriltag.h" 'struct apriltag_detector' for more documentation on these optional parameters
         detector:
@@ -16,7 +17,7 @@
 
         # optional list of tags
         tag:
-            track_all: false        # optionally track all tags with default name except the once listed in frames
+            track_all: False        # optionally track all tags with default name except the once listed in frames
             ids: [9, 14]            # tag ID
             frames: [base, object]  # optional frame name
             sizes: [0.162, 0.162]   # optional tag-specific edge size

src/AprilTagNode.cpp

@@ -14,6 +14,7 @@
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
 
 // apriltag
 #include "tag_functions.hpp"
@@ -74,6 +75,7 @@ class AprilTagNode : public rclcpp::Node {
     std::atomic<int> max_hamming;
     std::atomic<bool> profile;
     std::atomic<bool> tag_track_all;
+    std::atomic<bool> z_up;
     std::unordered_map<int, std::string> tag_frames;
     std::unordered_map<int, double> tag_sizes;
 
@@ -112,6 +114,7 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
     const std::string tag_family = declare_parameter("family", "36h11", descr("tag family", true));
     tag_edge_size = declare_parameter("size", 1.0, descr("default tag size", true));
     tag_track_all = declare_parameter("tag.track_all", false, descr("default tag size", true));
+    z_up = declare_parameter("z_up", true, descr("rotate about x-axis to have Z pointing upwards", true));
 
     // get tag names, IDs and sizes
     const auto ids = declare_parameter("tag.ids", std::vector<int64_t>{}, descr("tag ids", true));
@@ -221,7 +224,7 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
         tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id);
         const double size = tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size;
         if(estimate_pose != nullptr) {
-            tf.transform = estimate_pose(det, intrinsics, size);
+            tf.transform = estimate_pose(det, intrinsics, size, z_up);
         }
 
         tfs.push_back(tf);

src/pose_estimation.cpp

@@ -7,18 +7,27 @@
 
 
 geometry_msgs::msg::Transform
-homography(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
+homography(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize, bool z_up)
 {
     apriltag_detection_info_t info = {detection, tagsize, intr[0], intr[1], intr[2], intr[3]};
 
     apriltag_pose_t pose;
     estimate_pose_for_tag_homography(&info, &pose);
 
+    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R(pose.R->data);
+
+    if (z_up) {
+        // rotate by half rotation about x-axis to have z-axis
+        // point upwards orthogonal to the tag plane
+        R.col(1) *= -1;
+        R.col(2) *= -1;
+    }
+
     return tf2::toMsg<apriltag_pose_t, geometry_msgs::msg::Transform>(const_cast<const apriltag_pose_t&>(pose));
 }
 
 geometry_msgs::msg::Transform
-pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
+pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize, bool z_up)
 {
     const std::vector<cv::Point3d> objectPoints{
         {-tagsize / 2, -tagsize / 2, 0},
@@ -43,6 +52,18 @@ pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, do
     cv::Mat rvec, tvec;
     cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);
 
+    if (!z_up) {
+        cv::Mat rvecR;
+        cv::Rodrigues(rvec, rvecR);
+        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R(reinterpret_cast<double*>(rvecR.data));
+        // rotate by half rotation about x-axis to have z-axis
+        // point upwards orthogonal to the tag plane
+        rvecR.col(1) *= -1;
+        rvecR.col(2) *= -1;
+        cv::Rodrigues(rvecR, rvec);
+    }
+
+    // rvec = cv::Mat(3,1, cv::DataType<double>::type);
     return tf2::toMsg<std::pair<cv::Mat_<double>, cv::Mat_<double>>, geometry_msgs::msg::Transform>(std::make_pair(tvec, rvec));
 }
 

src/pose_estimation.hpp

@@ -6,6 +6,6 @@
 #include <unordered_map>
 
 
-typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const std::array<double, 4>&, const double&)> pose_estimation_f;
+typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const std::array<double, 4>&, const double&, const bool&)> pose_estimation_f;
 
 extern const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods;