60 if(!pnh.
getParam(
"standalone_tags", standalone_tag_descriptions))
82 if(!pnh.
getParam(
"tag_bundles", tag_bundle_descriptions))
84 ROS_WARN(
"No tag bundles specified");
103 ROS_WARN(
"remove_duplicates parameter not provided. Defaulting to true");
113 else if (
family_ ==
"tag36h10")
131 ROS_WARN(
"Invalid tag family specified! Aborting");
138 apriltag_detector_add_family(
td_,
tf_);
170 else if (
family_ ==
"tag36h10")
190 const sensor_msgs::CameraInfoConstPtr& camera_info) {
193 cv::cvtColor(image->image, gray_image, CV_BGR2GRAY);
195 .height = gray_image.rows,
196 .stride = gray_image.cols,
197 .buf = gray_image.data
204 double fx = camera_model.
fx();
205 double fy = camera_model.
fy();
206 double cx = camera_model.
cx();
207 double cy = camera_model.
cy();
228 AprilTagDetectionArray tag_detection_array;
229 std::vector<std::string > detection_names;
230 tag_detection_array.header = image->header;
231 std::map<std::string, std::vector<cv::Point3d > > bundleObjectPoints;
232 std::map<std::string, std::vector<cv::Point2d > > bundleImagePoints;
244 int tagID = detection->
id;
245 bool is_part_of_bundle =
false;
255 is_part_of_bundle =
true;
256 std::string bundleName = bundle.
name();
261 bundleObjectPoints[bundleName]);
283 double tag_size = standaloneDescription->
size();
303 std::vector<cv::Point3d > standaloneTagObjectPoints;
304 std::vector<cv::Point2d > standaloneTagImagePoints;
305 addObjectPoints(tag_size/2, cv::Matx44d::eye(), standaloneTagObjectPoints);
308 standaloneTagImagePoints,
310 Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
311 Eigen::Quaternion<double> rot_quaternion(rot);
313 geometry_msgs::PoseWithCovarianceStamped tag_pose =
314 makeTagPose(transform, rot_quaternion, image->header);
317 AprilTagDetection tag_detection;
318 tag_detection.pose = tag_pose;
319 tag_detection.id.push_back(detection->
id);
320 tag_detection.size.push_back(tag_size);
321 tag_detection_array.detections.push_back(tag_detection);
322 detection_names.push_back(standaloneDescription->
frame_name());
334 std::map<std::string,
335 std::vector<cv::Point3d> >::iterator it =
336 bundleObjectPoints.find(bundleName);
337 if (it != bundleObjectPoints.end())
343 Eigen::Matrix4d transform =
345 bundleImagePoints[bundleName], fx, fy, cx, cy);
346 Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
347 Eigen::Quaternion<double> rot_quaternion(rot);
349 geometry_msgs::PoseWithCovarianceStamped bundle_pose =
350 makeTagPose(transform, rot_quaternion, image->header);
353 AprilTagDetection tag_detection;
354 tag_detection.pose = bundle_pose;
357 tag_detection_array.detections.push_back(tag_detection);
358 detection_names.push_back(bundle.
name());
364 for (
unsigned int i=0; i<tag_detection_array.detections.size(); i++) {
365 geometry_msgs::PoseStamped pose;
366 pose.pose = tag_detection_array.detections[i].pose.pose.pose;
367 pose.header = tag_detection_array.detections[i].pose.header;
373 detection_names[i]));
377 return tag_detection_array;
384 return (id1 < id2) ? -1 : ((id1 == id2) ? 0 : 1);
391 bool duplicate_detected =
false;
401 int id_current = detection->
id;
408 id_next = detection->
id;
410 if (id_current == id_next || (id_current != id_next && duplicate_detected))
412 duplicate_detected =
true;
416 if (id_current != id_next)
419 "appears more than once in the image.");
420 duplicate_detected =
false;
432 double s, cv::Matx44d T_oi, std::vector<cv::Point3d >& objectPoints)
const 436 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1));
437 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s,-s, 0, 1));
438 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s, s, 0, 1));
439 objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s, s, 0, 1));
444 std::vector<cv::Point2d >& imagePoints)
const 448 double tag_x[4] = {-1,1,1,-1};
449 double tag_y[4] = {1,1,-1,-1};
453 for (
int i=0; i<4; i++)
457 homography_project(detection->
H, tag_x[i], tag_y[i], &im_x, &im_y);
458 imagePoints.push_back(cv::Point2d(im_x, im_y));
463 std::vector<cv::Point3d > objectPoints,
464 std::vector<cv::Point2d > imagePoints,
465 double fx,
double fy,
double cx,
double cy)
const 470 cv::Matx33d cameraMatrix(fx, 0, cx,
473 cv::Vec4f distCoeffs(0,0,0,0);
477 cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
479 cv::Rodrigues(rvec, R);
481 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);
484 T.topLeftCorner(3, 3) = wRo;
486 tvec.at<
double>(0), tvec.at<
double>(1), tvec.at<
double>(2);
492 const Eigen::Matrix4d& transform,
493 const Eigen::Quaternion<double> rot_quaternion,
494 const std_msgs::Header& header)
496 geometry_msgs::PoseWithCovarianceStamped pose;
497 pose.header = header;
499 pose.pose.pose.position.x = transform(0, 3);
500 pose.pose.pose.position.y = transform(1, 3);
501 pose.pose.pose.position.z = transform(2, 3);
502 pose.pose.pose.orientation.x = rot_quaternion.x();
503 pose.pose.pose.orientation.y = rot_quaternion.y();
504 pose.pose.pose.orientation.z = rot_quaternion.z();
505 pose.pose.pose.orientation.w = rot_quaternion.w();
519 bool is_part_of_bundle =
false;
525 is_part_of_bundle =
true;
531 if (!is_part_of_bundle &&
543 line(image->image, cv::Point((
int)det->
p[0][0], (
int)det->
p[0][1]),
544 cv::Point((
int)det->
p[1][0], (
int)det->
p[1][1]),
545 cv::Scalar(0, 0xff, 0));
546 line(image->image, cv::Point((
int)det->
p[0][0], (
int)det->
p[0][1]),
547 cv::Point((
int)det->
p[3][0], (
int)det->
p[3][1]),
548 cv::Scalar(0, 0, 0xff));
549 line(image->image, cv::Point((
int)det->
p[1][0], (
int)det->
p[1][1]),
550 cv::Point((
int)det->
p[2][0], (
int)det->
p[2][1]),
551 cv::Scalar(0xff, 0, 0));
552 line(image->image, cv::Point((
int)det->
p[2][0], (
int)det->
p[2][1]),
553 cv::Point((
int)det->
p[3][0], (
int)det->
p[3][1]),
554 cv::Scalar(0xff, 0, 0));
557 std::stringstream ss;
559 cv::String text = ss.str();
560 int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
561 double fontscale = 0.5;
563 cv::Size textsize = cv::getTextSize(text, fontface,
564 fontscale, 2, &baseline);
565 cv::putText(image->image, text,
566 cv::Point((
int)(det->
c[0]-textsize.width/2),
567 (
int)(det->
c[1]+textsize.height/2)),
568 fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2);
577 std::map<int, StandaloneTagDescription> descriptions;
581 for (int32_t i = 0; i < standalone_tags.
size(); i++)
594 ROS_ASSERT(tag_description[
"size"].getType() ==
597 int id = (int)tag_description[
"id"];
599 double size = (double)tag_description[
"size"];
602 std::string frame_name;
606 ROS_ASSERT(tag_description[
"name"].getType() ==
608 frame_name = (std::string)tag_description[
"name"];
612 std::stringstream frame_name_stream;
613 frame_name_stream <<
"tag_" << id;
614 frame_name = frame_name_stream.str();
619 size <<
", frame_name: " << frame_name.c_str());
621 descriptions.insert(std::make_pair(
id, description));
631 std::vector<TagBundleDescription > descriptions;
635 for (int32_t i=0; i<tag_bundles.
size(); i++)
641 std::string bundleName;
642 if (bundle_description.
hasMember(
"name"))
644 ROS_ASSERT(bundle_description[
"name"].getType() ==
646 bundleName = (std::string)bundle_description[
"name"];
650 std::stringstream bundle_name_stream;
651 bundle_name_stream <<
"bundle_" << i;
652 bundleName = bundle_name_stream.str();
655 ROS_INFO(
"Loading tag bundle '%s'",bundle_i.
name().c_str());
657 ROS_ASSERT(bundle_description[
"layout"].getType() ==
662 for (int32_t j=0; j<member_tags.
size(); j++)
671 double size = tag[
"size"];
678 ROS_ASSERT(size == standaloneDescription->
size());
689 Eigen::Quaterniond q_tag(qw, qx, qy, qz);
691 Eigen::Matrix3d R_oi = q_tag.toRotationMatrix();
694 cv::Matx44d T_mj(R_oi(0,0), R_oi(0,1), R_oi(0,2), x,
695 R_oi(1,0), R_oi(1,1), R_oi(1,2), y,
696 R_oi(2,0), R_oi(2,1), R_oi(2,2), z,
701 ROS_INFO_STREAM(
" " << j <<
") id: " <<
id <<
", size: " << size <<
", " 702 <<
"p = [" << x <<
"," << y <<
"," << z <<
"], " 703 <<
"q = [" << qw <<
"," << qx <<
"," << qy <<
"," 706 descriptions.push_back(bundle_i);
712 std::string field)
const 718 int tmp = xmlValue[field];
723 return xmlValue[field];
729 double defaultValue)
const 737 int tmp = xmlValue[field];
742 return xmlValue[field];
754 std::map<int, StandaloneTagDescription>::iterator description_itr =
761 " but no description was found...",
id);
765 descriptionContainer = &(description_itr->second);
const std::string & getMessage() const
void tag25h7_destroy(apriltag_family_t *tf)
#define ROS_WARN_THROTTLE(rate,...)
apriltag_family_t * tag36h10_create()
apriltag_detector_t * apriltag_detector_create()
void apriltag_detections_destroy(zarray_t *detections)
Type const & getType() const
void tag36h11_destroy(apriltag_family_t *tf)
apriltag_family_t * tag16h5_create()
TFSIMD_FORCE_INLINE const tfScalar & y() const
apriltag_family_t * tag25h9_create()
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
void tag36h10_destroy(apriltag_family_t *tf)
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_WARN_STREAM(args)
TFSIMD_FORCE_INLINE const tfScalar & z() const
apriltag_family_t * tag25h7_create()
bool hasMember(const std::string &name) const
void tag25h9_destroy(apriltag_family_t *tf)
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
void apriltag_detector_destroy(apriltag_detector_t *td)
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
#define ROS_ERROR_STREAM(args)
apriltag_family_t * tag36h11_create()
void tag16h5_destroy(apriltag_family_t *tf)
zarray_t * apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)