35 #include "common/homography.h" 36 #include "tagStandard52h13.h" 37 #include "tagStandard41h12.h" 41 #include "tagCustom48h12.h" 42 #include "tagCircle21h7.h" 43 #include "tagCircle49h12.h" 61 if(!pnh.
getParam(
"standalone_tags", standalone_tag_descriptions))
83 if(!pnh.
getParam(
"tag_bundles", tag_bundle_descriptions))
85 ROS_WARN(
"No tag bundles specified");
104 ROS_WARN(
"remove_duplicates parameter not provided. Defaulting to true");
110 if (
family_ ==
"tagStandard52h13")
112 tf_ = tagStandard52h13_create();
114 else if (
family_ ==
"tagStandard41h12")
116 tf_ = tagStandard41h12_create();
118 else if (
family_ ==
"tag36h11")
120 tf_ = tag36h11_create();
124 tf_ = tag25h9_create();
128 tf_ = tag16h5_create();
130 else if (
family_ ==
"tagCustom48h12")
132 tf_ = tagCustom48h12_create();
134 else if (
family_ ==
"tagCircle21h7")
136 tf_ = tagCircle21h7_create();
138 else if (
family_ ==
"tagCircle49h12")
140 tf_ = tagCircle49h12_create();
144 ROS_WARN(
"Invalid tag family specified! Aborting");
149 td_ = apriltag_detector_create();
170 apriltag_detector_destroy(
td_);
176 if (
family_ ==
"tagStandard52h13")
178 tagStandard52h13_destroy(
tf_);
180 else if (
family_ ==
"tagStandard41h12")
182 tagStandard41h12_destroy(
tf_);
184 else if (
family_ ==
"tag36h11")
186 tag36h11_destroy(
tf_);
190 tag25h9_destroy(
tf_);
194 tag16h5_destroy(
tf_);
196 else if (
family_ ==
"tagCustom48h12")
198 tagCustom48h12_destroy(
tf_);
200 else if (
family_ ==
"tagCircle21h7")
202 tagCircle21h7_destroy(
tf_);
204 else if (
family_ ==
"tagCircle49h12")
206 tagCircle49h12_destroy(
tf_);
212 const sensor_msgs::CameraInfoConstPtr& camera_info) {
215 if (image->image.channels() == 1)
217 gray_image = image->image;
221 cv::cvtColor(image->image, gray_image, CV_BGR2GRAY);
223 image_u8_t apriltag_image = { .width = gray_image.cols,
224 .height = gray_image.rows,
225 .stride = gray_image.cols,
226 .buf = gray_image.data
233 double fx = camera_model.
fx();
234 double fy = camera_model.
fy();
235 double cx = camera_model.
cx();
236 double cy = camera_model.
cy();
257 AprilTagDetectionArray tag_detection_array;
258 std::vector<std::string > detection_names;
259 tag_detection_array.header = image->header;
260 std::map<std::string, std::vector<cv::Point3d > > bundleObjectPoints;
261 std::map<std::string, std::vector<cv::Point2d > > bundleImagePoints;
265 apriltag_detection_t *detection;
273 int tagID = detection->id;
274 bool is_part_of_bundle =
false;
284 is_part_of_bundle =
true;
285 std::string bundleName = bundle.
name();
290 bundleObjectPoints[bundleName]);
312 double tag_size = standaloneDescription->
size();
332 std::vector<cv::Point3d > standaloneTagObjectPoints;
333 std::vector<cv::Point2d > standaloneTagImagePoints;
334 addObjectPoints(tag_size/2, cv::Matx44d::eye(), standaloneTagObjectPoints);
337 standaloneTagImagePoints,
339 Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
340 Eigen::Quaternion<double> rot_quaternion(rot);
342 geometry_msgs::PoseWithCovarianceStamped tag_pose =
343 makeTagPose(transform, rot_quaternion, image->header);
346 AprilTagDetection tag_detection;
347 tag_detection.pose = tag_pose;
348 tag_detection.id.push_back(detection->id);
349 tag_detection.size.push_back(tag_size);
350 tag_detection_array.detections.push_back(tag_detection);
351 detection_names.push_back(standaloneDescription->
frame_name());
363 std::map<std::string,
364 std::vector<cv::Point3d> >::iterator it =
365 bundleObjectPoints.find(bundleName);
366 if (it != bundleObjectPoints.end())
372 Eigen::Matrix4d transform =
374 bundleImagePoints[bundleName], fx, fy, cx, cy);
375 Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
376 Eigen::Quaternion<double> rot_quaternion(rot);
378 geometry_msgs::PoseWithCovarianceStamped bundle_pose =
379 makeTagPose(transform, rot_quaternion, image->header);
382 AprilTagDetection tag_detection;
383 tag_detection.pose = bundle_pose;
386 tag_detection_array.detections.push_back(tag_detection);
387 detection_names.push_back(bundle.
name());
393 for (
unsigned int i=0; i<tag_detection_array.detections.size(); i++) {
394 geometry_msgs::PoseStamped pose;
395 pose.pose = tag_detection_array.detections[i].pose.pose.pose;
396 pose.header = tag_detection_array.detections[i].pose.header;
402 detection_names[i]));
406 return tag_detection_array;
411 int id1 = ((apriltag_detection_t*) first)->id;
412 int id2 = ((apriltag_detection_t*) second)->id;
413 return (id1 < id2) ? -1 : ((id1 == id2) ? 0 : 1);
420 bool duplicate_detected =
false;
428 apriltag_detection_t *detection;
430 int id_current = detection->id;
437 id_next = detection->id;
439 if (id_current == id_next || (id_current != id_next && duplicate_detected))
441 duplicate_detected =
true;
445 if (id_current != id_next)
448 "appears more than once in the image.");
449 duplicate_detected =
false;
461 double s, cv::Matx44d T_oi, std::vector<cv::Point3d >& objectPoints)
const 465 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1));
466 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s,-s, 0, 1));
467 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s, s, 0, 1));
468 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s, s, 0, 1));
472 apriltag_detection_t *detection,
473 std::vector<cv::Point2d >& imagePoints)
const 477 double tag_x[4] = {-1,1,1,-1};
478 double tag_y[4] = {1,1,-1,-1};
482 for (
int i=0; i<4; i++)
486 homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y);
487 imagePoints.push_back(cv::Point2d(im_x, im_y));
492 std::vector<cv::Point3d > objectPoints,
493 std::vector<cv::Point2d > imagePoints,
494 double fx,
double fy,
double cx,
double cy)
const 499 cv::Matx33d cameraMatrix(fx, 0, cx,
502 cv::Vec4f distCoeffs(0,0,0,0);
506 cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
508 cv::Rodrigues(rvec, R);
510 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);
513 T.topLeftCorner(3, 3) = wRo;
515 tvec.at<
double>(0), tvec.at<
double>(1), tvec.at<
double>(2);
521 const Eigen::Matrix4d& transform,
522 const Eigen::Quaternion<double> rot_quaternion,
523 const std_msgs::Header& header)
525 geometry_msgs::PoseWithCovarianceStamped pose;
526 pose.header = header;
528 pose.pose.pose.position.x = transform(0, 3);
529 pose.pose.pose.position.y = transform(1, 3);
530 pose.pose.pose.position.z = transform(2, 3);
531 pose.pose.pose.orientation.x = rot_quaternion.x();
532 pose.pose.pose.orientation.y = rot_quaternion.y();
533 pose.pose.pose.orientation.z = rot_quaternion.z();
534 pose.pose.pose.orientation.w = rot_quaternion.w();
542 apriltag_detection_t *det;
548 bool is_part_of_bundle =
false;
554 is_part_of_bundle =
true;
560 if (!is_part_of_bundle &&
572 line(image->image, cv::Point((
int)det->p[0][0], (
int)det->p[0][1]),
573 cv::Point((
int)det->p[1][0], (
int)det->p[1][1]),
574 cv::Scalar(0, 0xff, 0));
575 line(image->image, cv::Point((
int)det->p[0][0], (
int)det->p[0][1]),
576 cv::Point((
int)det->p[3][0], (
int)det->p[3][1]),
577 cv::Scalar(0, 0, 0xff));
578 line(image->image, cv::Point((
int)det->p[1][0], (
int)det->p[1][1]),
579 cv::Point((
int)det->p[2][0], (
int)det->p[2][1]),
580 cv::Scalar(0xff, 0, 0));
581 line(image->image, cv::Point((
int)det->p[2][0], (
int)det->p[2][1]),
582 cv::Point((
int)det->p[3][0], (
int)det->p[3][1]),
583 cv::Scalar(0xff, 0, 0));
586 std::stringstream ss;
588 cv::String text = ss.str();
589 int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
590 double fontscale = 0.5;
592 cv::Size textsize = cv::getTextSize(text, fontface,
593 fontscale, 2, &baseline);
594 cv::putText(image->image, text,
595 cv::Point((
int)(det->c[0]-textsize.width/2),
596 (
int)(det->c[1]+textsize.height/2)),
597 fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2);
606 std::map<int, StandaloneTagDescription> descriptions;
610 for (int32_t i = 0; i < standalone_tags.
size(); i++)
623 ROS_ASSERT(tag_description[
"size"].getType() ==
626 int id = (int)tag_description[
"id"];
628 double size = (double)tag_description[
"size"];
631 std::string frame_name;
635 ROS_ASSERT(tag_description[
"name"].getType() ==
637 frame_name = (std::string)tag_description[
"name"];
641 std::stringstream frame_name_stream;
642 frame_name_stream <<
"tag_" << id;
643 frame_name = frame_name_stream.str();
648 size <<
", frame_name: " << frame_name.c_str());
650 descriptions.insert(std::make_pair(
id, description));
660 std::vector<TagBundleDescription > descriptions;
664 for (int32_t i=0; i<tag_bundles.
size(); i++)
670 std::string bundleName;
671 if (bundle_description.
hasMember(
"name"))
673 ROS_ASSERT(bundle_description[
"name"].getType() ==
675 bundleName = (std::string)bundle_description[
"name"];
679 std::stringstream bundle_name_stream;
680 bundle_name_stream <<
"bundle_" << i;
681 bundleName = bundle_name_stream.str();
684 ROS_INFO(
"Loading tag bundle '%s'",bundle_i.
name().c_str());
686 ROS_ASSERT(bundle_description[
"layout"].getType() ==
691 for (int32_t j=0; j<member_tags.
size(); j++)
700 double size = tag[
"size"];
707 ROS_ASSERT(size == standaloneDescription->
size());
718 Eigen::Quaterniond q_tag(qw, qx, qy, qz);
720 Eigen::Matrix3d R_oi = q_tag.toRotationMatrix();
723 cv::Matx44d T_mj(R_oi(0,0), R_oi(0,1), R_oi(0,2), x,
724 R_oi(1,0), R_oi(1,1), R_oi(1,2), y,
725 R_oi(2,0), R_oi(2,1), R_oi(2,2), z,
730 ROS_INFO_STREAM(
" " << j <<
") id: " <<
id <<
", size: " << size <<
", " 731 <<
"p = [" << x <<
"," << y <<
"," << z <<
"], " 732 <<
"q = [" << qw <<
"," << qx <<
"," << qy <<
"," 735 descriptions.push_back(bundle_i);
741 std::string field)
const 747 int tmp = xmlValue[field];
752 return xmlValue[field];
758 double defaultValue)
const 766 int tmp = xmlValue[field];
771 return xmlValue[field];
783 std::map<int, StandaloneTagDescription>::iterator description_itr =
790 " but no description was found...",
id);
794 descriptionContainer = &(description_itr->second);
cv::Matx44d memberT_oi(int tagID)
static int idComparison(const void *first, const void *second)
const std::string & getMessage() const
int max_hamming_distance_
void addImagePoints(apriltag_detection_t *detection, std::vector< cv::Point2d > &imagePoints) const
#define ROS_WARN_THROTTLE(rate,...)
std::map< int, StandaloneTagDescription > parseStandaloneTags(XmlRpc::XmlRpcValue &standalone_tag_descriptions)
bool findStandaloneTagDescription(int id, StandaloneTagDescription *&descriptionContainer, bool printWarning=true)
void addMemberTag(int id, double size, cv::Matx44d T_oi)
void addObjectPoints(double s, cv::Matx44d T_oi, std::vector< cv::Point3d > &objectPoints) const
std::map< int, StandaloneTagDescription > standalone_tag_descriptions_
TagDetector(ros::NodeHandle pnh)
T getAprilTagOption(ros::NodeHandle &pnh, const std::string ¶m_name, const T &default_val)
Type const & getType() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
tf::TransformBroadcaster tf_pub_
AprilTagDetectionArray detectTags(const cv_bridge::CvImagePtr &image, const sensor_msgs::CameraInfoConstPtr &camera_info)
geometry_msgs::PoseWithCovarianceStamped makeTagPose(const Eigen::Matrix4d &transform, const Eigen::Quaternion< double > rot_quaternion, const std_msgs::Header &header)
void drawDetections(cv_bridge::CvImagePtr image)
double xmlRpcGetDouble(XmlRpc::XmlRpcValue &xmlValue, std::string field) const
std::vector< int > bundleIds()
std::string camera_tf_frame_
apriltag_detector_t * td_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
double xmlRpcGetDoubleWithDefault(XmlRpc::XmlRpcValue &xmlValue, std::string field, double defaultValue) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_WARN_STREAM(args)
std::vector< double > bundleSizes()
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool hasMember(const std::string &name) const
Eigen::Matrix4d getRelativeTransform(std::vector< cv::Point3d > objectPoints, std::vector< cv::Point2d > imagePoints, double fx, double fy, double cx, double cy) const
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
std::vector< TagBundleDescription > tag_bundle_descriptions_
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
#define ROS_ERROR_STREAM(args)
std::string & frame_name()
double memberSize(int tagID)
std::vector< TagBundleDescription > parseTagBundles(XmlRpc::XmlRpcValue &tag_bundles)
std::map< int, int > id2idx_