From 7f76af2b393b63d24aaca0353efad348e24fabc6 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Tue, 9 Jan 2024 22:46:11 +0100 Subject: [PATCH] replace pose estimation method with options "pnp" and "homography" --- CMakeLists.txt | 9 ++++- src/AprilTagNode.cpp | 55 ++++++-------------------- src/pose_estimation.cpp | 86 +++++++++++++++++++++++++++++++++++++++++ src/pose_estimation.hpp | 10 +++++ 4 files changed, 116 insertions(+), 44 deletions(-) create mode 100644 src/pose_estimation.cpp create mode 100644 src/pose_estimation.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c906851..0adf765 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp index b717986..5be0d32 100644 --- a/src/AprilTagNode.cpp +++ b/src/AprilTagNode.cpp @@ -1,4 +1,5 @@ // ros +#include "pose_estimation.hpp" #include #include #ifdef cv_bridge_HPP @@ -18,8 +19,6 @@ #include "tag_functions.hpp" #include -#include - #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 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(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::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& parameters); @@ -148,6 +111,9 @@ AprilTagNode::AprilTagNode(const rclcpp::NodeOptions& options) const auto frames = declare_parameter("tag.frames", std::vector{}, descr("tag frame names per id", true)); const auto sizes = declare_parameter("tag.sizes", std::vector{}, 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>(msg_ci->p.data()).leftCols<3>().inverse(); + // camera intrinsics for rectified images + const std::array 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); } diff --git a/src/pose_estimation.cpp b/src/pose_estimation.cpp new file mode 100644 index 0000000..979427f --- /dev/null +++ b/src/pose_estimation.cpp @@ -0,0 +1,86 @@ +#include "pose_estimation.hpp" +#include +#include +#include +#include +#include + + +geometry_msgs::msg::Transform tf_from_apriltag_pose(const apriltag_pose_t& pose) +{ + const Eigen::Quaterniond q(Eigen::Map>(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_& tvec, const cv::Mat_& rvec) +{ + const cv::Quat q = cv::Quat::createFromRvec(rvec); + + geometry_msgs::msg::Transform t; + + t.translation.x = tvec.at(0); + t.translation.y = tvec.at(1); + t.translation.z = tvec.at(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& 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& intr, double tagsize) +{ + const std::vector objectPoints{ + {-tagsize / 2, -tagsize / 2, 0}, + {+tagsize / 2, -tagsize / 2, 0}, + {+tagsize / 2, +tagsize / 2, 0}, + {-tagsize / 2, +tagsize / 2, 0}, + }; + + const std::vector 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 pose_estimation_methods{ + {"homography", homography}, + {"pnp", pnp}, +}; diff --git a/src/pose_estimation.hpp b/src/pose_estimation.hpp new file mode 100644 index 0000000..7ba1654 --- /dev/null +++ b/src/pose_estimation.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include +#include +#include + + +typedef std::function&, const double&)> pose_estimation_f; + +extern const std::unordered_map pose_estimation_methods;