Skip to content

Commit

Permalink
Merge pull request #81 from AprilRobotics/wxm-const-ref-and-isometry3d
Browse files Browse the repository at this point in the history 
Minor improvements: const-ref; Eigen::Isometry3d, warn if intrinsics aren't set
  • Loading branch information
@wxmerkt
wxmerkt authored Nov 21, 2022
2 parents b7175fc + 5beae1a commit a734108
Show file tree
Hide file tree
Showing 3 changed files with 43 additions and 42 deletions.
13 changes: 6 additions & 7 deletions apriltag_ros/include/apriltag_ros/common_functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ class TagBundleDescription
public:
std::map<int, int > id2idx_; // (id2idx_[<tag ID>]=<index in tags_>) mapping

TagBundleDescription(std::string name) :
TagBundleDescription(const std::string& name) :
name_(name) {}

void addMemberTag(int id, double size, cv::Matx44d T_oi) {
Expand All @@ -125,7 +125,7 @@ class TagBundleDescription
id2idx_[id] = tags_.size()-1;
}

std::string name () const { return name_; }
const std::string& name() const { return name_; }
// Get IDs of bundle member tags
std::vector<int> bundleIds () {
std::vector<int> ids;
Expand Down Expand Up @@ -206,8 +206,7 @@ class TagDetector
bool printWarning = true);

geometry_msgs::PoseWithCovarianceStamped makeTagPose(
const Eigen::Matrix4d& transform,
const Eigen::Quaternion<double> rot_quaternion,
const Eigen::Isometry3d& transform,
const std_msgs::Header& header);

// Detect tags in an image
Expand All @@ -222,9 +221,9 @@ class TagDetector
// rotation from the tag frame to the camera frame and t is the
// vector from the camera frame origin to the tag frame origin,
// expressed in the camera frame.
Eigen::Matrix4d getRelativeTransform(
std::vector<cv::Point3d > objectPoints,
std::vector<cv::Point2d > imagePoints,
Eigen::Isometry3d getRelativeTransform(
const std::vector<cv::Point3d >& objectPoints,
const std::vector<cv::Point2d >& imagePoints,
double fx, double fy, double cx, double cy) const;

void addImagePoints(apriltag_detection_t *detection,
Expand Down
19 changes: 11 additions & 8 deletions apriltag_ros/launch/continuous_detection.launch
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,23 @@
<launch>
<arg name="launch_prefix" default="" /> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
<arg name="node_namespace" default="apriltag_ros_continuous_node" />
<!-- set to value="gdbserver localhost:10000" for remote debugging -->
<arg name="launch_prefix" default="" />

<!-- configure camera input -->
<arg name="camera_name" default="/camera_rect" />
<arg name="image_topic" default="image_rect" />
<arg name="queue_size" default="1" />

<!-- Set parameters -->
<rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="$(arg node_namespace)" />
<rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="$(arg node_namespace)" />

<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
<!-- apriltag_ros continuous detection node -->
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)">
<!-- Remap topics from those used in code to those on the ROS network -->
<remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
<remap from="camera_info" to="$(arg camera_name)/camera_info" />

<param name="publish_tag_detections_image" type="bool" value="true" /> <!-- default: false -->
<param name="publish_tag_detections_image" type="bool" value="true" /><!-- default: false -->
<param name="queue_size" type="int" value="$(arg queue_size)" />

<!-- load parameters (incl. tag family, tags, etc.) -->
<rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml"/>
<rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml"/>
</node>
</launch>
53 changes: 26 additions & 27 deletions apriltag_ros/src/common_functions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,6 +231,10 @@ AprilTagDetectionArray TagDetector::detectTags (
double cx = camera_model.cx(); // optical center x-coordinate [px]
double cy = camera_model.cy(); // optical center y-coordinate [px]

// Check if camera intrinsics are not available - if not the calculated
// transforms are meaningless.
if (fx == 0 && fy == 0) ROS_WARN_STREAM_THROTTLE(5, "fx and fy are zero. Are the camera intrinsics set?");

// Run AprilTag 2 algorithm on the image
if (detections_)
{
Expand All @@ -239,7 +243,7 @@ AprilTagDetectionArray TagDetector::detectTags (
}
detections_ = apriltag_detector_detect(td_, &apriltag_image);

// If remove_dulpicates_ is set to true, then duplicate tags are not allowed.
// If remove_duplicates_ is set to true, then duplicate tags are not allowed.
// Thus any duplicate tag IDs visible in the scene must include at least 1
// erroneous detection. Remove any tags with duplicate IDs to ensure removal
// of these erroneous detections
Expand Down Expand Up @@ -329,14 +333,11 @@ AprilTagDetectionArray TagDetector::detectTags (
std::vector<cv::Point2d > standaloneTagImagePoints;
addObjectPoints(tag_size/2, cv::Matx44d::eye(), standaloneTagObjectPoints);
addImagePoints(detection, standaloneTagImagePoints);
Eigen::Matrix4d transform = getRelativeTransform(standaloneTagObjectPoints,
Eigen::Isometry3d transform = getRelativeTransform(standaloneTagObjectPoints,
standaloneTagImagePoints,
fx, fy, cx, cy);
Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
Eigen::Quaternion<double> rot_quaternion(rot);

geometry_msgs::PoseWithCovarianceStamped tag_pose =
makeTagPose(transform, rot_quaternion, image->header);
makeTagPose(transform, image->header);

// Add the detection to the back of the tag detection array
AprilTagDetection tag_detection;
Expand Down Expand Up @@ -365,14 +366,11 @@ AprilTagDetectionArray TagDetector::detectTags (
// position!
TagBundleDescription& bundle = tag_bundle_descriptions_[j];

Eigen::Matrix4d transform =
Eigen::Isometry3d transform =
getRelativeTransform(bundleObjectPoints[bundleName],
bundleImagePoints[bundleName], fx, fy, cx, cy);
Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
Eigen::Quaternion<double> rot_quaternion(rot);

geometry_msgs::PoseWithCovarianceStamped bundle_pose =
makeTagPose(transform, rot_quaternion, image->header);
makeTagPose(transform, image->header);

// Add the detection to the back of the tag detection array
AprilTagDetection tag_detection;
Expand Down Expand Up @@ -484,11 +482,13 @@ void TagDetector::addImagePoints (
}
}

Eigen::Matrix4d TagDetector::getRelativeTransform(
std::vector<cv::Point3d > objectPoints,
std::vector<cv::Point2d > imagePoints,
Eigen::Isometry3d TagDetector::getRelativeTransform(
const std::vector<cv::Point3d >& objectPoints,
const std::vector<cv::Point2d >& imagePoints,
double fx, double fy, double cx, double cy) const
{
Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // homogeneous transformation matrix

// perform Perspective-n-Point camera pose estimation using the
// above 3D-2D point correspondences
cv::Mat rvec, tvec;
Expand All @@ -502,28 +502,27 @@ Eigen::Matrix4d TagDetector::getRelativeTransform(
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Matx33d R;
cv::Rodrigues(rvec, R);
Eigen::Matrix3d wRo;
wRo << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2);

Eigen::Matrix4d T; // homogeneous transformation matrix
T.topLeftCorner(3, 3) = wRo;
T.col(3).head(3) <<
tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
T.row(3) << 0,0,0,1;

// rotation
T.linear() << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2);

// translation
T.translation() = Eigen::Vector3d::Map(reinterpret_cast<const double*>(tvec.data));

return T;
}

geometry_msgs::PoseWithCovarianceStamped TagDetector::makeTagPose(
const Eigen::Matrix4d& transform,
const Eigen::Quaternion<double> rot_quaternion,
const Eigen::Isometry3d& transform,
const std_msgs::Header& header)
{
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header = header;
Eigen::Quaterniond rot_quaternion(transform.linear());
//===== Position and orientation
pose.pose.pose.position.x = transform(0, 3);
pose.pose.pose.position.y = transform(1, 3);
pose.pose.pose.position.z = transform(2, 3);
pose.pose.pose.position.x = transform.translation().x();
pose.pose.pose.position.y = transform.translation().y();
pose.pose.pose.position.z = transform.translation().z();
pose.pose.pose.orientation.x = rot_quaternion.x();
pose.pose.pose.orientation.y = rot_quaternion.y();
pose.pose.pose.orientation.z = rot_quaternion.z();
Expand Down

0 comments on commit a734108

Please sign in to comment.