common_functions.h
Go to the documentation of this file.
1 
44 #ifndef APRILTAG_ROS_COMMON_FUNCTIONS_H
45 #define APRILTAG_ROS_COMMON_FUNCTIONS_H
46 
47 #include <string>
48 #include <sstream>
49 #include <vector>
50 #include <map>
51 
52 #include <ros/ros.h>
53 #include <ros/console.h>
54 #include <XmlRpcException.h>
55 #include <cv_bridge/cv_bridge.h>
56 #include <eigen3/Eigen/Dense>
57 #include <eigen3/Eigen/Geometry>
58 #include <opencv2/opencv.hpp>
59 #include <opencv2/imgproc/imgproc.hpp>
60 #include <opencv2/core/core.hpp>
64 
65 #include <apriltag.h>
66 
67 #include "apriltag_ros/AprilTagDetection.h"
68 #include "apriltag_ros/AprilTagDetectionArray.h"
69 
70 namespace apriltag_ros
71 {
72 
73 template<typename T>
75  const std::string& param_name, const T & default_val)
76 {
77  T param_val;
78  pnh.param<T>(param_name, param_val, default_val);
79  return param_val;
80 }
81 
82 // Stores the properties of a tag member of a bundle
84 {
85  int id; // Payload ID
86  double size; // [m] Side length
87  cv::Matx44d T_oi; // Rigid transform from tag i frame to bundle origin frame
88 };
89 
91 {
92  public:
95  std::string &frame_name) :
96  id_(id),
97  size_(size),
98  frame_name_(frame_name) {}
99 
100  double size() { return size_; }
101  int id() { return id_; }
102  std::string& frame_name() { return frame_name_; }
103 
104  private:
105  // Tag description
106  int id_;
107  double size_;
108  std::string frame_name_;
109 };
110 
112 {
113  public:
114  std::map<int, int > id2idx_; // (id2idx_[<tag ID>]=<index in tags_>) mapping
115 
116  TagBundleDescription(std::string name) :
117  name_(name) {}
118 
119  void addMemberTag(int id, double size, cv::Matx44d T_oi) {
120  TagBundleMember member;
121  member.id = id;
122  member.size = size;
123  member.T_oi = T_oi;
124  tags_.push_back(member);
125  id2idx_[id] = tags_.size()-1;
126  }
127 
128  std::string name () const { return name_; }
129  // Get IDs of bundle member tags
130  std::vector<int> bundleIds () {
131  std::vector<int> ids;
132  for (unsigned int i = 0; i < tags_.size(); i++) {
133  ids.push_back(tags_[i].id);
134  }
135  return ids;
136  }
137  // Get sizes of bundle member tags
138  std::vector<double> bundleSizes () {
139  std::vector<double> sizes;
140  for (unsigned int i = 0; i < tags_.size(); i++) {
141  sizes.push_back(tags_[i].size);
142  }
143  return sizes;
144  }
145  int memberID (int tagID) { return tags_[id2idx_[tagID]].id; }
146  double memberSize (int tagID) { return tags_[id2idx_[tagID]].size; }
147  cv::Matx44d memberT_oi (int tagID) { return tags_[id2idx_[tagID]].T_oi; }
148 
149  private:
150  // Bundle description
151  std::string name_;
152  std::vector<TagBundleMember > tags_;
153 };
154 
156 {
157  private:
158  // Detections sorting
159  static int idComparison(const void* first, const void* second);
160 
161  // Remove detections of tags with the same ID
162  void removeDuplicates();
163 
164  // AprilTag 2 code's attributes
165  std::string family_;
166  int threads_;
167  double decimate_;
168  double blur_;
170  int debug_;
171 
172  // AprilTag 2 objects
173  apriltag_family_t *tf_;
174  apriltag_detector_t *td_;
175  zarray_t *detections_;
176 
177  // Other members
178  std::map<int, StandaloneTagDescription> standalone_tag_descriptions_;
179  std::vector<TagBundleDescription > tag_bundle_descriptions_;
184  std::string camera_tf_frame_;
185 
186  public:
187 
189  ~TagDetector();
190 
191  // Store standalone and bundle tag descriptions
192  std::map<int, StandaloneTagDescription> parseStandaloneTags(
193  XmlRpc::XmlRpcValue& standalone_tag_descriptions);
194  std::vector<TagBundleDescription > parseTagBundles(
195  XmlRpc::XmlRpcValue& tag_bundles);
196  double xmlRpcGetDouble(
197  XmlRpc::XmlRpcValue& xmlValue, std::string field) const;
198  double xmlRpcGetDoubleWithDefault(
199  XmlRpc::XmlRpcValue& xmlValue, std::string field,
200  double defaultValue) const;
201 
202  bool findStandaloneTagDescription(
203  int id, StandaloneTagDescription*& descriptionContainer,
204  bool printWarning = true);
205 
206  geometry_msgs::PoseWithCovarianceStamped makeTagPose(
207  const Eigen::Matrix4d& transform,
208  const Eigen::Quaternion<double> rot_quaternion,
209  const std_msgs::Header& header);
210 
211  // Detect tags in an image
212  AprilTagDetectionArray detectTags(
213  const cv_bridge::CvImagePtr& image,
214  const sensor_msgs::CameraInfoConstPtr& camera_info);
215 
216  // Get the pose of the tag in the camera frame
217  // Returns homogeneous transformation matrix [R,t;[0 0 0 1]] which
218  // takes a point expressed in the tag frame to the same point
219  // expressed in the camera frame. As usual, R is the (passive)
220  // rotation from the tag frame to the camera frame and t is the
221  // vector from the camera frame origin to the tag frame origin,
222  // expressed in the camera frame.
223  Eigen::Matrix4d getRelativeTransform(
224  std::vector<cv::Point3d > objectPoints,
225  std::vector<cv::Point2d > imagePoints,
226  double fx, double fy, double cx, double cy) const;
227 
228  void addImagePoints(apriltag_detection_t *detection,
229  std::vector<cv::Point2d >& imagePoints) const;
230  void addObjectPoints(double s, cv::Matx44d T_oi,
231  std::vector<cv::Point3d >& objectPoints) const;
232 
233  // Draw the detected tags' outlines and payload values on the image
234  void drawDetections(cv_bridge::CvImagePtr image);
235 
236  bool get_publish_tf() const { return publish_tf_; }
237 };
238 
239 } // namespace apriltag_ros
240 
241 #endif // APRILTAG_ROS_COMMON_FUNCTIONS_H
void addMemberTag(int id, double size, cv::Matx44d T_oi)
std::map< int, StandaloneTagDescription > standalone_tag_descriptions_
T getAprilTagOption(ros::NodeHandle &pnh, const std::string &param_name, const T &default_val)
tf::TransformBroadcaster tf_pub_
StandaloneTagDescription(int id, double size, std::string &frame_name)
std::vector< TagBundleMember > tags_
apriltag_detector_t * td_
bool param(const std::string &param_name, T &param_val, const T &default_val) const
apriltag_family_t * tf_
std::vector< double > bundleSizes()
std::vector< TagBundleDescription > tag_bundle_descriptions_


apriltag_ros
Author(s): Danylo Malyuta
autogenerated on Thu Oct 22 2020 03:55:58