From a7c4d8b77c0e7152346be93b01b9a39fabee5a88 Mon Sep 17 00:00:00 2001 From: Mihail Valchev Date: Thu, 29 Sep 2022 02:30:49 +0200 Subject: [PATCH] camera intrinsic parameters are now scaled according actual image resolution in order to be able to use different than the calibration resolution provided by the CameraInfo message --- src/AprilTagNode.cpp | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/AprilTagNode.cpp b/src/AprilTagNode.cpp index f174c44..72625af 100644 --- a/src/AprilTagNode.cpp +++ b/src/AprilTagNode.cpp @@ -58,7 +58,7 @@ descr(const std::string& description, const bool& read_only = false) } void getPose(apriltag_detection_t* det, - const Mat3& K, + Mat3& K, const Mat3& Pinv, geometry_msgs::msg::Transform& t, const double size, @@ -218,12 +218,27 @@ 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(); - // convert to 8bit monochrome image const cv::Mat img_uint8 = cv_bridge::toCvShare(msg_img, "mono8")->image; + // the CameraInfo message provides the intrinsic camera parameters that are valid only for the calibration resolution + // the camera matrices K and P have to be scaled according actual image resolution in order to get correct pose + const float scale_x = (float)img_uint8.cols / (float)msg_ci->width; + const float scale_y = (float)img_uint8.rows / (float)msg_ci->height; + Mat3 K(msg_ci->k.data()); + K(0,0) *= scale_x; + K(0,2) *= scale_x; + K(1,1) *= scale_y; + K(1,2) *= scale_y; + Eigen::Matrix P(msg_ci->p.data()); + P(0,0) *= scale_x; + P(0,2) *= scale_x; + P(1,1) *= scale_y; + P(1,2) *= scale_y; + + // precompute inverse projection matrix + const Mat3 Pinv = P.leftCols<3>().inverse(); + image_u8_t im{img_uint8.cols, img_uint8.rows, img_uint8.cols, img_uint8.data}; // detect tags @@ -266,7 +281,7 @@ 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, Eigen::Map(msg_ci->k.data()), Pinv, tf.transform, tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size, z_up, refine_pose); + getPose(det, K, Pinv, tf.transform, tag_sizes.count(det->id) ? tag_sizes.at(det->id) : tag_edge_size, z_up, refine_pose); tfs.push_back(tf); }