Merge pull request #24 from christianrauch/pose

pose estimation option

CMakeLists.txt

@@ -41,6 +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)
 find_package(apriltag 3.2 REQUIRED)
 
 if(cv_bridge_VERSION VERSION_GREATER_EQUAL 3.3.0)
@@ -51,9 +52,15 @@ 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)
+
 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 Eigen3::Eigen)
+target_link_libraries(AprilTagNode apriltag::apriltag tags pose_estimation Eigen3::Eigen)
 rclcpp_components_register_node(AprilTagNode PLUGIN "AprilTagNode" EXECUTABLE "apriltag_node")
 
 ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH})

src/AprilTagNode.cpp

@@ -1,4 +1,5 @@
 // ros
+#include "pose_estimation.hpp"
 #include <apriltag_msgs/msg/april_tag_detection.hpp>
 #include <apriltag_msgs/msg/april_tag_detection_array.hpp>
 #ifdef cv_bridge_HPP
@@ -18,8 +19,6 @@
 #include "tag_functions.hpp"
 #include <apriltag.h>
 
-#include <Eigen/Dense>
-
 
 #define IF(N, V) \
     if(assign_check(parameter, N, V)) continue;
@@ -46,9 +45,6 @@ bool assign_check(const rclcpp::Parameter& parameter, const std::string& name, T
     return false;
 }
 
-
-typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Mat3;
-
 rcl_interfaces::msg::ParameterDescriptor
 descr(const std::string& description, const bool& read_only = false)
 {
@@ -60,41 +56,6 @@ descr(const std::string& description, const bool& read_only = false)
     return descr;
 }
 
-void getPose(const matd_t& H,
-             const Mat3& Pinv,
-             geometry_msgs::msg::Transform& t,
-             const double size)
-{
-    // compute extrinsic camera parameter
-    // https://dsp.stackexchange.com/a/2737/31703
-    // H = K * T  =>  T = K^(-1) * H
-    const Mat3 T = Pinv * Eigen::Map<const Mat3>(H.data);
-    Mat3 R;
-    R.col(0) = T.col(0).normalized();
-    R.col(1) = T.col(1).normalized();
-    R.col(2) = R.col(0).cross(R.col(1));
-
-    // 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;
-
-    // the corner coordinates of the tag in the canonical frame are (+/-1, +/-1)
-    // hence the scale is half of the edge size
-    const Eigen::Vector3d tt = T.rightCols<1>() / ((T.col(0).norm() + T.col(0).norm()) / 2.0) * (size / 2.0);
-
-    const Eigen::Quaterniond q(R);
-
-    t.translation.x = tt.x();
-    t.translation.y = tt.y();
-    t.translation.z = tt.z();
-    t.rotation.w = q.w();
-    t.rotation.x = q.x();
-    t.rotation.y = q.y();
-    t.rotation.z = q.z();
-}
-
-
 class AprilTagNode : public rclcpp::Node {
 public:
     AprilTagNode(const rclcpp::NodeOptions& options);
@@ -121,6 +82,8 @@ class AprilTagNode : public rclcpp::Node {
     const rclcpp::Publisher<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr pub_detections;
     tf2_ros::TransformBroadcaster tf_broadcaster;
 
+    pose_estimation_f estimate_pose = nullptr;
+
     void onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci);
 
     rcl_interfaces::msg::SetParametersResult onParameter(const std::vector<rclcpp::Parameter>& parameters);
@@ -148,6 +111,9 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options)
     const auto frames = declare_parameter("tag.frames", std::vector<std::string>{}, descr("tag frame names per id", true));
     const auto sizes = declare_parameter("tag.sizes", std::vector<double>{}, descr("tag sizes per id", true));
 
+    // get method for estimating tag pose
+    estimate_pose = pose_estimation_methods.at(declare_parameter("pose_estimation_method", "pnp", descr("pose estimation method: \"pnp\" (more accurate) or \"homography\" (faster)", true)));
+
     // detector parameters in "detector" namespace
     declare_parameter("detector.threads", td->nthreads, descr("number of threads"));
     declare_parameter("detector.decimate", td->quad_decimate, descr("decimate resolution for quad detection"));
@@ -193,8 +159,8 @@ AprilTagNode::~AprilTagNode()
 void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_img,
                             const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_ci)
 {
-    // precompute inverse projection matrix
-    const Mat3 Pinv = Eigen::Map<const Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>(msg_ci->p.data()).leftCols<3>().inverse();
+    // camera intrinsics for rectified images
+    const std::array<double, 4> intrinsics = {msg_ci->p.data()[0], msg_ci->p.data()[5], msg_ci->p.data()[2], msg_ci->p.data()[6]};
 
     // convert to 8bit monochrome image
     const cv::Mat img_uint8 = cv_bridge::toCvShare(msg_img, "mono8")->image;
@@ -246,7 +212,10 @@ void AprilTagNode::onCamera(const sensor_msgs::msg::Image::ConstSharedPtr& msg_i
         tf.header = msg_img->header;
         // set child frame name by generic tag name or configured tag name
         tf.child_frame_id = tag_frames.count(det->id) ? tag_frames.at(det->id) : std::string(det->family->name) + ":" + std::to_string(det->id);
-        getPose(*(det->H), Pinv, tf.transform, tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size);
+        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);
+        }
 
         tfs.push_back(tf);
     }

src/pose_estimation.cpp

@@ -0,0 +1,86 @@
+#include "pose_estimation.hpp"
+#include <Eigen/Dense>
+#include <apriltag/apriltag_pose.h>
+#include <apriltag/common/homography.h>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/quaternion.hpp>
+
+
+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)
+{
+    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);
+
+    return tf_from_apriltag_pose(pose);
+}
+
+geometry_msgs::msg::Transform
+pnp(apriltag_detection_t* const detection, const std::array<double, 4>& intr, double tagsize)
+{
+    const std::vector<cv::Point3d> objectPoints{
+        {-tagsize / 2, -tagsize / 2, 0},
+        {+tagsize / 2, -tagsize / 2, 0},
+        {+tagsize / 2, +tagsize / 2, 0},
+        {-tagsize / 2, +tagsize / 2, 0},
+    };
+
+    const std::vector<cv::Point2d> imagePoints{
+        {detection->p[0][0], detection->p[0][1]},
+        {detection->p[1][0], detection->p[1][1]},
+        {detection->p[2][0], detection->p[2][1]},
+        {detection->p[3][0], detection->p[3][1]},
+    };
+
+    cv::Matx33d cameraMatrix;
+    cameraMatrix(0, 0) = intr[0];// fx
+    cameraMatrix(1, 1) = intr[1];// fy
+    cameraMatrix(0, 2) = intr[2];// cx
+    cameraMatrix(1, 2) = intr[3];// cy
+
+    cv::Mat rvec, tvec;
+    cv::solvePnP(objectPoints, imagePoints, cameraMatrix, {}, rvec, tvec);
+
+    return tf_from_cv(tvec, rvec);
+}
+
+const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods{
+    {"homography", homography},
+    {"pnp", pnp},
+};

src/pose_estimation.hpp

@@ -0,0 +1,10 @@
+#pragma once
+
+#include <apriltag/apriltag.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <unordered_map>
+
+
+typedef std::function<geometry_msgs::msg::Transform(apriltag_detection_t* const, const std::array<double, 4>&, const double&)> pose_estimation_f;
+
+extern const std::unordered_map<std::string, pose_estimation_f> pose_estimation_methods;