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