44 #ifndef APRILTAG_ROS_COMMON_FUNCTIONS_H 45 #define APRILTAG_ROS_COMMON_FUNCTIONS_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> 67 #include "apriltag_ros/AprilTagDetection.h" 68 #include "apriltag_ros/AprilTagDetectionArray.h" 75 const std::string& param_name,
const T & default_val)
78 pnh.
param<T>(param_name, param_val, default_val);
95 std::string &frame_name) :
98 frame_name_(frame_name) {}
100 double size() {
return size_; }
101 int id() {
return id_; }
124 tags_.push_back(member);
125 id2idx_[
id] = tags_.size()-1;
128 std::string
name ()
const {
return name_; }
131 std::vector<int> ids;
132 for (
unsigned int i = 0; i < tags_.size(); i++) {
133 ids.push_back(tags_[i].
id);
139 std::vector<double> sizes;
140 for (
unsigned int i = 0; i < tags_.size(); i++) {
141 sizes.push_back(tags_[i].
size);
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; }
152 std::vector<TagBundleMember >
tags_;
159 static int idComparison(
const void* first,
const void* second);
162 void removeDuplicates();
171 int max_hamming_distance_ = 2;
195 std::map<int, StandaloneTagDescription> parseStandaloneTags(
197 std::vector<TagBundleDescription > parseTagBundles(
199 double xmlRpcGetDouble(
201 double xmlRpcGetDoubleWithDefault(
203 double defaultValue)
const;
205 bool findStandaloneTagDescription(
207 bool printWarning =
true);
209 geometry_msgs::PoseWithCovarianceStamped makeTagPose(
210 const Eigen::Matrix4d& transform,
211 const Eigen::Quaternion<double> rot_quaternion,
212 const std_msgs::Header& header);
215 AprilTagDetectionArray detectTags(
217 const sensor_msgs::CameraInfoConstPtr& camera_info);
226 Eigen::Matrix4d getRelativeTransform(
227 std::vector<cv::Point3d > objectPoints,
228 std::vector<cv::Point2d > imagePoints,
229 double fx,
double fy,
double cx,
double cy)
const;
231 void addImagePoints(apriltag_detection_t *detection,
232 std::vector<cv::Point2d >& imagePoints)
const;
233 void addObjectPoints(
double s, cv::Matx44d
T_oi,
234 std::vector<cv::Point3d >& objectPoints)
const;
244 #endif // APRILTAG_ROS_COMMON_FUNCTIONS_H cv::Matx44d memberT_oi(int tagID)
StandaloneTagDescription()
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 ¶m_name, const T &default_val)
tf::TransformBroadcaster tf_pub_
StandaloneTagDescription(int id, double size, std::string &frame_name)
std::vector< int > bundleIds()
std::string camera_tf_frame_
std::vector< TagBundleMember > tags_
apriltag_detector_t * td_
TagBundleDescription(std::string name)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std::vector< double > bundleSizes()
bool get_publish_tf() const
std::vector< TagBundleDescription > tag_bundle_descriptions_
std::string & frame_name()
double memberSize(int tagID)
std::map< int, int > id2idx_