common_functions.cpp
Go to the documentation of this file.
1 
34 
35 #include "common/homography.h"
36 #include "tagStandard52h13.h"
37 #include "tagStandard41h12.h"
38 #include "tag36h11.h"
39 #include "tag25h9.h"
40 #include "tag16h5.h"
41 #include "tagCustom48h12.h"
42 #include "tagCircle21h7.h"
43 #include "tagCircle49h12.h"
44 
45 namespace apriltag_ros
46 {
47 
49  family_(getAprilTagOption<std::string>(pnh, "tag_family", "tag36h11")),
50  threads_(getAprilTagOption<int>(pnh, "tag_threads", 4)),
51  decimate_(getAprilTagOption<double>(pnh, "tag_decimate", 1.0)),
52  blur_(getAprilTagOption<double>(pnh, "tag_blur", 0.0)),
53  refine_edges_(getAprilTagOption<int>(pnh, "tag_refine_edges", 1)),
54  debug_(getAprilTagOption<int>(pnh, "tag_debug", 0)),
55  publish_tf_(getAprilTagOption<bool>(pnh, "publish_tf", false))
56 {
57  // Parse standalone tag descriptions specified by user (stored on ROS
58  // parameter server)
59  XmlRpc::XmlRpcValue standalone_tag_descriptions;
60  if(!pnh.getParam("standalone_tags", standalone_tag_descriptions))
61  {
62  ROS_WARN("No april tags specified");
63  }
64  else
65  {
66  try
67  {
69  parseStandaloneTags(standalone_tag_descriptions);
70  }
72  {
73  // in case any of the asserts in parseStandaloneTags() fail
74  ROS_ERROR_STREAM("Error loading standalone tag descriptions: " <<
75  e.getMessage().c_str());
76  }
77  }
78 
79  // parse tag bundle descriptions specified by user (stored on ROS parameter
80  // server)
81  XmlRpc::XmlRpcValue tag_bundle_descriptions;
82  if(!pnh.getParam("tag_bundles", tag_bundle_descriptions))
83  {
84  ROS_WARN("No tag bundles specified");
85  }
86  else
87  {
88  try
89  {
90  tag_bundle_descriptions_ = parseTagBundles(tag_bundle_descriptions);
91  }
93  {
94  // In case any of the asserts in parseStandaloneTags() fail
95  ROS_ERROR_STREAM("Error loading tag bundle descriptions: " <<
96  e.getMessage().c_str());
97  }
98  }
99 
100  // Optionally remove duplicate detections in scene. Defaults to removing
101  if(!pnh.getParam("remove_duplicates", remove_duplicates_))
102  {
103  ROS_WARN("remove_duplicates parameter not provided. Defaulting to true");
104  remove_duplicates_ = true;
105  }
106 
107  // Define the tag family whose tags should be searched for in the camera
108  // images
109  if (family_ == "tagStandard52h13")
110  {
111  tf_ = tagStandard52h13_create();
112  }
113  else if (family_ == "tagStandard41h12")
114  {
115  tf_ = tagStandard41h12_create();
116  }
117  else if (family_ == "tag36h11")
118  {
119  tf_ = tag36h11_create();
120  }
121  else if (family_ == "tag25h9")
122  {
123  tf_ = tag25h9_create();
124  }
125  else if (family_ == "tag16h5")
126  {
127  tf_ = tag16h5_create();
128  }
129  else if (family_ == "tagCustom48h12")
130  {
131  tf_ = tagCustom48h12_create();
132  }
133  else if (family_ == "tagCircle21h7")
134  {
135  tf_ = tagCircle21h7_create();
136  }
137  else if (family_ == "tagCircle49h12")
138  {
139  tf_ = tagCircle49h12_create();
140  }
141  else
142  {
143  ROS_WARN("Invalid tag family specified! Aborting");
144  exit(1);
145  }
146 
147  // Create the AprilTag 2 detector
148  td_ = apriltag_detector_create();
149  apriltag_detector_add_family(td_, tf_);
150  td_->quad_decimate = (float)decimate_;
151  td_->quad_sigma = (float)blur_;
152  td_->nthreads = threads_;
153  td_->debug = debug_;
154  td_->refine_edges = refine_edges_;
155 
156  detections_ = NULL;
157 
158  // Get tf frame name to use for the camera
159  if (!pnh.getParam("camera_frame", camera_tf_frame_))
160  {
161  ROS_WARN_STREAM("Camera frame not specified, using 'camera'");
162  camera_tf_frame_ = "camera";
163  }
164 }
165 
166 // destructor
168  // free memory associated with tag detector
169  apriltag_detector_destroy(td_);
170 
171  // Free memory associated with the array of tag detections
172  apriltag_detections_destroy(detections_);
173 
174  // free memory associated with tag family
175  if (family_ == "tagStandard52h13")
176  {
177  tagStandard52h13_destroy(tf_);
178  }
179  else if (family_ == "tagStandard41h12")
180  {
181  tagStandard41h12_destroy(tf_);
182  }
183  else if (family_ == "tag36h11")
184  {
185  tag36h11_destroy(tf_);
186  }
187  else if (family_ == "tag25h9")
188  {
189  tag25h9_destroy(tf_);
190  }
191  else if (family_ == "tag16h5")
192  {
193  tag16h5_destroy(tf_);
194  }
195  else if (family_ == "tagCustom48h12")
196  {
197  tagCustom48h12_destroy(tf_);
198  }
199  else if (family_ == "tagCircle21h7")
200  {
201  tagCircle21h7_destroy(tf_);
202  }
203  else if (family_ == "tagCircle49h12")
204  {
205  tagCircle49h12_destroy(tf_);
206  }
207 }
208 
209 AprilTagDetectionArray TagDetector::detectTags (
210  const cv_bridge::CvImagePtr& image,
211  const sensor_msgs::CameraInfoConstPtr& camera_info) {
212  // Convert image to AprilTag code's format
213  cv::Mat gray_image;
214  if (image->image.channels() == 1)
215  {
216  gray_image = image->image;
217  }
218  else
219  {
220  cv::cvtColor(image->image, gray_image, CV_BGR2GRAY);
221  }
222  image_u8_t apriltag_image = { .width = gray_image.cols,
223  .height = gray_image.rows,
224  .stride = gray_image.cols,
225  .buf = gray_image.data
226  };
227 
229  camera_model.fromCameraInfo(camera_info);
230 
231  // Get camera intrinsic properties for rectified image.
232  double fx = camera_model.fx(); // focal length in camera x-direction [px]
233  double fy = camera_model.fy(); // focal length in camera y-direction [px]
234  double cx = camera_model.cx(); // optical center x-coordinate [px]
235  double cy = camera_model.cy(); // optical center y-coordinate [px]
236 
237  // Run AprilTag 2 algorithm on the image
238  if (detections_)
239  {
240  apriltag_detections_destroy(detections_);
241  detections_ = NULL;
242  }
243  detections_ = apriltag_detector_detect(td_, &apriltag_image);
244 
245  // If remove_dulpicates_ is set to true, then duplicate tags are not allowed.
246  // Thus any duplicate tag IDs visible in the scene must include at least 1
247  // erroneous detection. Remove any tags with duplicate IDs to ensure removal
248  // of these erroneous detections
249  if (remove_duplicates_)
250  {
252  }
253 
254  // Compute the estimated translation and rotation individually for each
255  // detected tag
256  AprilTagDetectionArray tag_detection_array;
257  std::vector<std::string > detection_names;
258  tag_detection_array.header = image->header;
259  std::map<std::string, std::vector<cv::Point3d > > bundleObjectPoints;
260  std::map<std::string, std::vector<cv::Point2d > > bundleImagePoints;
261  for (int i=0; i < zarray_size(detections_); i++)
262  {
263  // Get the i-th detected tag
264  apriltag_detection_t *detection;
265  zarray_get(detections_, i, &detection);
266 
267  // Bootstrap this for loop to find this tag's description amongst
268  // the tag bundles. If found, add its points to the bundle's set of
269  // object-image corresponding points (tag corners) for cv::solvePnP.
270  // Don't yet run cv::solvePnP on the bundles, though, since we're still in
271  // the process of collecting all the object-image corresponding points
272  int tagID = detection->id;
273  bool is_part_of_bundle = false;
274  for (unsigned int j=0; j<tag_bundle_descriptions_.size(); j++)
275  {
276  // Iterate over the registered bundles
278 
279  if (bundle.id2idx_.find(tagID) != bundle.id2idx_.end())
280  {
281  // This detected tag belongs to the j-th tag bundle (its ID was found in
282  // the bundle description)
283  is_part_of_bundle = true;
284  std::string bundleName = bundle.name();
285 
286  //===== Corner points in the world frame coordinates
287  double s = bundle.memberSize(tagID)/2;
288  addObjectPoints(s, bundle.memberT_oi(tagID),
289  bundleObjectPoints[bundleName]);
290 
291  //===== Corner points in the image frame coordinates
292  addImagePoints(detection, bundleImagePoints[bundleName]);
293  }
294  }
295 
296  // Find this tag's description amongst the standalone tags
297  // Print warning when a tag was found that is neither part of a
298  // bundle nor standalone (thus it is a tag in the environment
299  // which the user specified no description for, or Apriltags
300  // misdetected a tag (bad ID or a false positive)).
301  StandaloneTagDescription* standaloneDescription;
302  if (!findStandaloneTagDescription(tagID, standaloneDescription,
303  !is_part_of_bundle))
304  {
305  continue;
306  }
307 
308  //=================================================================
309  // The remainder of this for loop is concerned with standalone tag
310  // poses!
311  double tag_size = standaloneDescription->size();
312 
313  // Get estimated tag pose in the camera frame.
314  //
315  // Note on frames:
316  // The raw AprilTag 2 uses the following frames:
317  // - camera frame: looking from behind the camera (like a
318  // photographer), x is right, y is up and z is towards you
319  // (i.e. the back of camera)
320  // - tag frame: looking straight at the tag (oriented correctly),
321  // x is right, y is down and z is away from you (into the tag).
322  // But we want:
323  // - camera frame: looking from behind the camera (like a
324  // photographer), x is right, y is down and z is straight
325  // ahead
326  // - tag frame: looking straight at the tag (oriented correctly),
327  // x is right, y is up and z is towards you (out of the tag).
328  // Using these frames together with cv::solvePnP directly avoids
329  // AprilTag 2's frames altogether.
330  // TODO solvePnP[Ransac] better?
331  std::vector<cv::Point3d > standaloneTagObjectPoints;
332  std::vector<cv::Point2d > standaloneTagImagePoints;
333  addObjectPoints(tag_size/2, cv::Matx44d::eye(), standaloneTagObjectPoints);
334  addImagePoints(detection, standaloneTagImagePoints);
335  Eigen::Matrix4d transform = getRelativeTransform(standaloneTagObjectPoints,
336  standaloneTagImagePoints,
337  fx, fy, cx, cy);
338  Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
339  Eigen::Quaternion<double> rot_quaternion(rot);
340 
341  geometry_msgs::PoseWithCovarianceStamped tag_pose =
342  makeTagPose(transform, rot_quaternion, image->header);
343 
344  // Add the detection to the back of the tag detection array
345  AprilTagDetection tag_detection;
346  tag_detection.pose = tag_pose;
347  tag_detection.id.push_back(detection->id);
348  tag_detection.size.push_back(tag_size);
349  tag_detection_array.detections.push_back(tag_detection);
350  detection_names.push_back(standaloneDescription->frame_name());
351  }
352 
353  //=================================================================
354  // Estimate bundle origin pose for each bundle in which at least one
355  // member tag was detected
356 
357  for (unsigned int j=0; j<tag_bundle_descriptions_.size(); j++)
358  {
359  // Get bundle name
360  std::string bundleName = tag_bundle_descriptions_[j].name();
361 
362  std::map<std::string,
363  std::vector<cv::Point3d> >::iterator it =
364  bundleObjectPoints.find(bundleName);
365  if (it != bundleObjectPoints.end())
366  {
367  // Some member tags of this bundle were detected, get the bundle's
368  // position!
370 
371  Eigen::Matrix4d transform =
372  getRelativeTransform(bundleObjectPoints[bundleName],
373  bundleImagePoints[bundleName], fx, fy, cx, cy);
374  Eigen::Matrix3d rot = transform.block(0, 0, 3, 3);
375  Eigen::Quaternion<double> rot_quaternion(rot);
376 
377  geometry_msgs::PoseWithCovarianceStamped bundle_pose =
378  makeTagPose(transform, rot_quaternion, image->header);
379 
380  // Add the detection to the back of the tag detection array
381  AprilTagDetection tag_detection;
382  tag_detection.pose = bundle_pose;
383  tag_detection.id = bundle.bundleIds();
384  tag_detection.size = bundle.bundleSizes();
385  tag_detection_array.detections.push_back(tag_detection);
386  detection_names.push_back(bundle.name());
387  }
388  }
389 
390  // If set, publish the transform /tf topic
391  if (publish_tf_) {
392  for (unsigned int i=0; i<tag_detection_array.detections.size(); i++) {
393  geometry_msgs::PoseStamped pose;
394  pose.pose = tag_detection_array.detections[i].pose.pose.pose;
395  pose.header = tag_detection_array.detections[i].pose.header;
396  tf::Stamped<tf::Transform> tag_transform;
397  tf::poseStampedMsgToTF(pose, tag_transform);
399  tag_transform.stamp_,
401  detection_names[i]));
402  }
403  }
404 
405  return tag_detection_array;
406 }
407 
408 int TagDetector::idComparison (const void* first, const void* second)
409 {
410  int id1 = ((apriltag_detection_t*) first)->id;
411  int id2 = ((apriltag_detection_t*) second)->id;
412  return (id1 < id2) ? -1 : ((id1 == id2) ? 0 : 1);
413 }
414 
416 {
417  zarray_sort(detections_, &idComparison);
418  int count = 0;
419  bool duplicate_detected = false;
420  while (true)
421  {
422  if (count > zarray_size(detections_)-1)
423  {
424  // The entire detection set was parsed
425  return;
426  }
427  apriltag_detection_t *detection;
428  zarray_get(detections_, count, &detection);
429  int id_current = detection->id;
430  // Default id_next value of -1 ensures that if the last detection
431  // is a duplicated tag ID, it will get removed
432  int id_next = -1;
433  if (count < zarray_size(detections_)-1)
434  {
435  zarray_get(detections_, count+1, &detection);
436  id_next = detection->id;
437  }
438  if (id_current == id_next || (id_current != id_next && duplicate_detected))
439  {
440  duplicate_detected = true;
441  // Remove the current tag detection from detections array
442  int shuffle = 0;
443  zarray_remove_index(detections_, count, shuffle);
444  if (id_current != id_next)
445  {
446  ROS_WARN_STREAM("Pruning tag ID " << id_current << " because it "
447  "appears more than once in the image.");
448  duplicate_detected = false; // Reset
449  }
450  continue;
451  }
452  else
453  {
454  count++;
455  }
456  }
457 }
458 
460  double s, cv::Matx44d T_oi, std::vector<cv::Point3d >& objectPoints) const
461 {
462  // Add to object point vector the tag corner coordinates in the bundle frame
463  // Going counterclockwise starting from the bottom left corner
464  objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1));
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 }
469 
471  apriltag_detection_t *detection,
472  std::vector<cv::Point2d >& imagePoints) const
473 {
474  // Add to image point vector the tag corners in the image frame
475  // Going counterclockwise starting from the bottom left corner
476  double tag_x[4] = {-1,1,1,-1};
477  double tag_y[4] = {1,1,-1,-1}; // Negated because AprilTag tag local
478  // frame has y-axis pointing DOWN
479  // while we use the tag local frame
480  // with y-axis pointing UP
481  for (int i=0; i<4; i++)
482  {
483  // Homography projection taking tag local frame coordinates to image pixels
484  double im_x, im_y;
485  homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y);
486  imagePoints.push_back(cv::Point2d(im_x, im_y));
487  }
488 }
489 
491  std::vector<cv::Point3d > objectPoints,
492  std::vector<cv::Point2d > imagePoints,
493  double fx, double fy, double cx, double cy) const
494 {
495  // perform Perspective-n-Point camera pose estimation using the
496  // above 3D-2D point correspondences
497  cv::Mat rvec, tvec;
498  cv::Matx33d cameraMatrix(fx, 0, cx,
499  0, fy, cy,
500  0, 0, 1);
501  cv::Vec4f distCoeffs(0,0,0,0); // distortion coefficients
502  // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would
503  // need to first check WHAT is a bottleneck in this code, and only
504  // do this if PnP solution is the bottleneck.
505  cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
506  cv::Matx33d R;
507  cv::Rodrigues(rvec, R);
508  Eigen::Matrix3d wRo;
509  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);
510 
511  Eigen::Matrix4d T; // homogeneous transformation matrix
512  T.topLeftCorner(3, 3) = wRo;
513  T.col(3).head(3) <<
514  tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
515  T.row(3) << 0,0,0,1;
516  return T;
517 }
518 
519 geometry_msgs::PoseWithCovarianceStamped TagDetector::makeTagPose(
520  const Eigen::Matrix4d& transform,
521  const Eigen::Quaternion<double> rot_quaternion,
522  const std_msgs::Header& header)
523 {
524  geometry_msgs::PoseWithCovarianceStamped pose;
525  pose.header = header;
526  //===== Position and orientation
527  pose.pose.pose.position.x = transform(0, 3);
528  pose.pose.pose.position.y = transform(1, 3);
529  pose.pose.pose.position.z = transform(2, 3);
530  pose.pose.pose.orientation.x = rot_quaternion.x();
531  pose.pose.pose.orientation.y = rot_quaternion.y();
532  pose.pose.pose.orientation.z = rot_quaternion.z();
533  pose.pose.pose.orientation.w = rot_quaternion.w();
534  return pose;
535 }
536 
538 {
539  for (int i = 0; i < zarray_size(detections_); i++)
540  {
541  apriltag_detection_t *det;
542  zarray_get(detections_, i, &det);
543 
544  // Check if this ID is present in config/tags.yaml
545  // Check if is part of a tag bundle
546  int tagID = det->id;
547  bool is_part_of_bundle = false;
548  for (unsigned int j=0; j<tag_bundle_descriptions_.size(); j++)
549  {
551  if (bundle.id2idx_.find(tagID) != bundle.id2idx_.end())
552  {
553  is_part_of_bundle = true;
554  break;
555  }
556  }
557  // If not part of a bundle, check if defined as a standalone tag
558  StandaloneTagDescription* standaloneDescription;
559  if (!is_part_of_bundle &&
560  !findStandaloneTagDescription(tagID, standaloneDescription, false))
561  {
562  // Neither a standalone tag nor part of a bundle, so this is a "rogue"
563  // tag, skip it.
564  continue;
565  }
566 
567  // Draw tag outline with edge colors green, blue, blue, red
568  // (going counter-clockwise, starting from lower-left corner in
569  // tag coords). cv::Scalar(Blue, Green, Red) format for the edge
570  // colors!
571  line(image->image, cv::Point((int)det->p[0][0], (int)det->p[0][1]),
572  cv::Point((int)det->p[1][0], (int)det->p[1][1]),
573  cv::Scalar(0, 0xff, 0)); // green
574  line(image->image, cv::Point((int)det->p[0][0], (int)det->p[0][1]),
575  cv::Point((int)det->p[3][0], (int)det->p[3][1]),
576  cv::Scalar(0, 0, 0xff)); // red
577  line(image->image, cv::Point((int)det->p[1][0], (int)det->p[1][1]),
578  cv::Point((int)det->p[2][0], (int)det->p[2][1]),
579  cv::Scalar(0xff, 0, 0)); // blue
580  line(image->image, cv::Point((int)det->p[2][0], (int)det->p[2][1]),
581  cv::Point((int)det->p[3][0], (int)det->p[3][1]),
582  cv::Scalar(0xff, 0, 0)); // blue
583 
584  // Print tag ID in the middle of the tag
585  std::stringstream ss;
586  ss << det->id;
587  cv::String text = ss.str();
588  int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
589  double fontscale = 0.5;
590  int baseline;
591  cv::Size textsize = cv::getTextSize(text, fontface,
592  fontscale, 2, &baseline);
593  cv::putText(image->image, text,
594  cv::Point((int)(det->c[0]-textsize.width/2),
595  (int)(det->c[1]+textsize.height/2)),
596  fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2);
597  }
598 }
599 
600 // Parse standalone tag descriptions
601 std::map<int, StandaloneTagDescription> TagDetector::parseStandaloneTags (
602  XmlRpc::XmlRpcValue& standalone_tags)
603 {
604  // Create map that will be filled by the function and returned in the end
605  std::map<int, StandaloneTagDescription> descriptions;
606  // Ensure the type is correct
607  ROS_ASSERT(standalone_tags.getType() == XmlRpc::XmlRpcValue::TypeArray);
608  // Loop through all tag descriptions
609  for (int32_t i = 0; i < standalone_tags.size(); i++)
610  {
611 
612  // i-th tag description
613  XmlRpc::XmlRpcValue& tag_description = standalone_tags[i];
614 
615  // Assert the tag description is a struct
616  ROS_ASSERT(tag_description.getType() ==
618  // Assert type of field "id" is an int
619  ROS_ASSERT(tag_description["id"].getType() ==
621  // Assert type of field "size" is a double
622  ROS_ASSERT(tag_description["size"].getType() ==
624 
625  int id = (int)tag_description["id"]; // tag id
626  // Tag size (square, side length in meters)
627  double size = (double)tag_description["size"];
628 
629  // Custom frame name, if such a field exists for this tag
630  std::string frame_name;
631  if(tag_description.hasMember("name"))
632  {
633  // Assert type of field "name" is a string
634  ROS_ASSERT(tag_description["name"].getType() ==
636  frame_name = (std::string)tag_description["name"];
637  }
638  else
639  {
640  std::stringstream frame_name_stream;
641  frame_name_stream << "tag_" << id;
642  frame_name = frame_name_stream.str();
643  }
644 
645  StandaloneTagDescription description(id, size, frame_name);
646  ROS_INFO_STREAM("Loaded tag config: " << id << ", size: " <<
647  size << ", frame_name: " << frame_name.c_str());
648  // Add this tag's description to map of descriptions
649  descriptions.insert(std::make_pair(id, description));
650  }
651 
652  return descriptions;
653 }
654 
655 // parse tag bundle descriptions
656 std::vector<TagBundleDescription > TagDetector::parseTagBundles (
657  XmlRpc::XmlRpcValue& tag_bundles)
658 {
659  std::vector<TagBundleDescription > descriptions;
661 
662  // Loop through all tag bundle descritions
663  for (int32_t i=0; i<tag_bundles.size(); i++)
664  {
665  ROS_ASSERT(tag_bundles[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
666  // i-th tag bundle description
667  XmlRpc::XmlRpcValue& bundle_description = tag_bundles[i];
668 
669  std::string bundleName;
670  if (bundle_description.hasMember("name"))
671  {
672  ROS_ASSERT(bundle_description["name"].getType() ==
674  bundleName = (std::string)bundle_description["name"];
675  }
676  else
677  {
678  std::stringstream bundle_name_stream;
679  bundle_name_stream << "bundle_" << i;
680  bundleName = bundle_name_stream.str();
681  }
682  TagBundleDescription bundle_i(bundleName);
683  ROS_INFO("Loading tag bundle '%s'",bundle_i.name().c_str());
684 
685  ROS_ASSERT(bundle_description["layout"].getType() ==
687  XmlRpc::XmlRpcValue& member_tags = bundle_description["layout"];
688 
689  // Loop through each member tag of the bundle
690  for (int32_t j=0; j<member_tags.size(); j++)
691  {
692  ROS_ASSERT(member_tags[j].getType() == XmlRpc::XmlRpcValue::TypeStruct);
693  XmlRpc::XmlRpcValue& tag = member_tags[j];
694 
695  ROS_ASSERT(tag["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
696  int id = tag["id"];
697 
698  ROS_ASSERT(tag["size"].getType() == XmlRpc::XmlRpcValue::TypeDouble);
699  double size = tag["size"];
700 
701  // Make sure that if this tag was specified also as standalone,
702  // then the sizes match
703  StandaloneTagDescription* standaloneDescription;
704  if (findStandaloneTagDescription(id, standaloneDescription, false))
705  {
706  ROS_ASSERT(size == standaloneDescription->size());
707  }
708 
709  // Get this tag's pose with respect to the bundle origin
710  double x = xmlRpcGetDoubleWithDefault(tag, "x", 0.);
711  double y = xmlRpcGetDoubleWithDefault(tag, "y", 0.);
712  double z = xmlRpcGetDoubleWithDefault(tag, "z", 0.);
713  double qw = xmlRpcGetDoubleWithDefault(tag, "qw", 1.);
714  double qx = xmlRpcGetDoubleWithDefault(tag, "qx", 0.);
715  double qy = xmlRpcGetDoubleWithDefault(tag, "qy", 0.);
716  double qz = xmlRpcGetDoubleWithDefault(tag, "qz", 0.);
717  Eigen::Quaterniond q_tag(qw, qx, qy, qz);
718  q_tag.normalize();
719  Eigen::Matrix3d R_oi = q_tag.toRotationMatrix();
720 
721  // Build the rigid transform from tag_j to the bundle origin
722  cv::Matx44d T_mj(R_oi(0,0), R_oi(0,1), R_oi(0,2), x,
723  R_oi(1,0), R_oi(1,1), R_oi(1,2), y,
724  R_oi(2,0), R_oi(2,1), R_oi(2,2), z,
725  0, 0, 0, 1);
726 
727  // Register the tag member
728  bundle_i.addMemberTag(id, size, T_mj);
729  ROS_INFO_STREAM(" " << j << ") id: " << id << ", size: " << size << ", "
730  << "p = [" << x << "," << y << "," << z << "], "
731  << "q = [" << qw << "," << qx << "," << qy << ","
732  << qz << "]");
733  }
734  descriptions.push_back(bundle_i);
735  }
736  return descriptions;
737 }
738 
740  std::string field) const
741 {
742  ROS_ASSERT((xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeDouble) ||
743  (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt));
744  if (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt)
745  {
746  int tmp = xmlValue[field];
747  return (double)tmp;
748  }
749  else
750  {
751  return xmlValue[field];
752  }
753 }
754 
756  std::string field,
757  double defaultValue) const
758 {
759  if (xmlValue.hasMember(field))
760  {
761  ROS_ASSERT((xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeDouble) ||
762  (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt));
763  if (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt)
764  {
765  int tmp = xmlValue[field];
766  return (double)tmp;
767  }
768  else
769  {
770  return xmlValue[field];
771  }
772  }
773  else
774  {
775  return defaultValue;
776  }
777 }
778 
780  int id, StandaloneTagDescription*& descriptionContainer, bool printWarning)
781 {
782  std::map<int, StandaloneTagDescription>::iterator description_itr =
784  if (description_itr == standalone_tag_descriptions_.end())
785  {
786  if (printWarning)
787  {
788  ROS_WARN_THROTTLE(10.0, "Requested description of standalone tag ID [%d],"
789  " but no description was found...",id);
790  }
791  return false;
792  }
793  descriptionContainer = &(description_itr->second);
794  return true;
795 }
796 
797 } // namespace apriltag_ros
static int idComparison(const void *first, const void *second)
const std::string & getMessage() const
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
int size() const
XmlRpcServer s
std::map< int, StandaloneTagDescription > standalone_tag_descriptions_
TagDetector(ros::NodeHandle pnh)
T getAprilTagOption(ros::NodeHandle &pnh, const std::string &param_name, const T &default_val)
Type const & getType() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
tf::TransformBroadcaster tf_pub_
#define ROS_WARN(...)
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
apriltag_detector_t * td_
#define ROS_INFO(...)
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
double xmlRpcGetDoubleWithDefault(XmlRpc::XmlRpcValue &xmlValue, std::string field, double defaultValue) const
void sendTransform(const StampedTransform &transform)
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_WARN_STREAM(args)
apriltag_family_t * tf_
std::vector< double > bundleSizes()
TFSIMD_FORCE_INLINE const tfScalar & z() const
ros::Time stamp_
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)
#define ROS_ASSERT(cond)
std::vector< TagBundleDescription > parseTagBundles(XmlRpc::XmlRpcValue &tag_bundles)


apriltag_ros
Author(s): Danylo Malyuta
autogenerated on Thu Jul 16 2020 03:53:54