Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

pose estimation option #24

Merged
merged 1 commit into from
Jan 24, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
replace pose estimation method with options "pnp" and "homography"
  • Loading branch information
christianrauch committed Jan 24, 2024
commit 7f76af2b393b63d24aaca0353efad348e24fabc6
9 changes: 8 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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})
55 changes: 12 additions & 43 deletions src/AprilTagNode.cpp
Original file line number Diff line number Diff line change
@@ -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);
}
86 changes: 86 additions & 0 deletions src/pose_estimation.cpp
Original file line number Diff line number Diff line change
@@ -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},
};
10 changes: 10 additions & 0 deletions src/pose_estimation.hpp
Original file line number Diff line number Diff line change
@@ -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;
Loading