apriltag_detector.cpp
Go to the documentation of this file.
00001 #include <apriltags_ros/apriltag_detector.h>
00002 #include <cv_bridge/cv_bridge.h>
00003 #include <sensor_msgs/image_encodings.h>
00004 #include <boost/foreach.hpp>
00005 #include <geometry_msgs/PoseStamped.h>
00006 #include <geometry_msgs/PoseArray.h>
00007 #include <apriltags_ros/AprilTagDetection.h>
00008 #include <apriltags_ros/AprilTagDetectionArray.h>
00009 #include <AprilTags/Tag16h5.h>
00010 #include <AprilTags/Tag25h7.h>
00011 #include <AprilTags/Tag25h9.h>
00012 #include <AprilTags/Tag36h9.h>
00013 #include <AprilTags/Tag36h11.h>
00014 #include <XmlRpcException.h>
00015 
00016 namespace apriltags_ros{
00017 
00018 AprilTagDetector::AprilTagDetector(ros::NodeHandle& nh, ros::NodeHandle& pnh): it_(nh){
00019   XmlRpc::XmlRpcValue april_tag_descriptions;
00020   if(!pnh.getParam("tag_descriptions", april_tag_descriptions)){
00021     ROS_WARN("No april tags specified");
00022   }
00023   else{
00024     try{
00025       descriptions_ = parse_tag_descriptions(april_tag_descriptions);
00026     } catch(XmlRpc::XmlRpcException e){
00027       ROS_ERROR_STREAM("Error loading tag descriptions: "<<e.getMessage());
00028     }
00029   }
00030 
00031   if(!pnh.getParam("sensor_frame_id", sensor_frame_id_)){
00032     sensor_frame_id_ = "";
00033   }
00034 
00035   AprilTags::TagCodes tag_codes = AprilTags::tagCodes36h11;
00036   tag_detector_= boost::shared_ptr<AprilTags::TagDetector>(new AprilTags::TagDetector(tag_codes));
00037   image_sub_ = it_.subscribeCamera("image_rect", 1, &AprilTagDetector::imageCb, this);
00038   image_pub_ = it_.advertise("tag_detections_image", 1);
00039   detections_pub_ = nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
00040   pose_pub_ = nh.advertise<geometry_msgs::PoseArray>("tag_detections_pose", 1);
00041 }
00042 AprilTagDetector::~AprilTagDetector(){
00043   image_sub_.shutdown();
00044 }
00045 
00046 void AprilTagDetector::imageCb(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& cam_info){
00047   cv_bridge::CvImagePtr cv_ptr;
00048   try{
00049     cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
00050   }
00051   catch (cv_bridge::Exception& e){
00052     ROS_ERROR("cv_bridge exception: %s", e.what());
00053     return;
00054   }
00055   cv::Mat gray;
00056   cv::cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
00057   std::vector<AprilTags::TagDetection>  detections = tag_detector_->extractTags(gray);
00058   ROS_DEBUG("%d tag detected", (int)detections.size());
00059 
00060   double fx = cam_info->K[0];
00061   double fy = cam_info->K[4];
00062   double px = cam_info->K[2];
00063   double py = cam_info->K[5];
00064 
00065   if(!sensor_frame_id_.empty())
00066     cv_ptr->header.frame_id = sensor_frame_id_;
00067 
00068   AprilTagDetectionArray tag_detection_array;
00069   geometry_msgs::PoseArray tag_pose_array;
00070   tag_pose_array.header = cv_ptr->header;
00071 
00072   BOOST_FOREACH(AprilTags::TagDetection detection, detections){
00073     std::map<int, AprilTagDescription>::const_iterator description_itr = descriptions_.find(detection.id);
00074     if(description_itr == descriptions_.end()){
00075       ROS_WARN_THROTTLE(10.0, "Found tag: %d, but no description was found for it", detection.id);
00076       continue;
00077     }
00078     AprilTagDescription description = description_itr->second;
00079     double tag_size = description.size();
00080 
00081     detection.draw(cv_ptr->image);
00082     Eigen::Matrix4d transform = detection.getRelativeTransform(tag_size, fx, fy, px, py);
00083     Eigen::Matrix3d rot = transform.block(0,0,3,3);
00084     Eigen::Quaternion<double> rot_quaternion = Eigen::Quaternion<double>(rot);
00085 
00086     geometry_msgs::PoseStamped tag_pose;
00087     tag_pose.pose.position.x = transform(0,3);
00088     tag_pose.pose.position.y = transform(1,3);
00089     tag_pose.pose.position.z = transform(2,3);
00090     tag_pose.pose.orientation.x = rot_quaternion.x();
00091     tag_pose.pose.orientation.y = rot_quaternion.y();
00092     tag_pose.pose.orientation.z = rot_quaternion.z();
00093     tag_pose.pose.orientation.w = rot_quaternion.w();
00094     tag_pose.header = cv_ptr->header;
00095 
00096     AprilTagDetection tag_detection;
00097     tag_detection.pose = tag_pose;
00098     tag_detection.id = detection.id;
00099     tag_detection.size = tag_size;
00100     tag_detection_array.detections.push_back(tag_detection);
00101     tag_pose_array.poses.push_back(tag_pose.pose);
00102 
00103     tf::Stamped<tf::Transform> tag_transform;
00104     tf::poseStampedMsgToTF(tag_pose, tag_transform);
00105     tf_pub_.sendTransform(tf::StampedTransform(tag_transform, tag_transform.stamp_, tag_transform.frame_id_, description.frame_name()));
00106   }
00107   detections_pub_.publish(tag_detection_array);
00108   pose_pub_.publish(tag_pose_array);
00109   image_pub_.publish(cv_ptr->toImageMsg());
00110 }
00111 
00112 
00113 std::map<int, AprilTagDescription> AprilTagDetector::parse_tag_descriptions(XmlRpc::XmlRpcValue& tag_descriptions){
00114   std::map<int, AprilTagDescription> descriptions;
00115   ROS_ASSERT(tag_descriptions.getType() == XmlRpc::XmlRpcValue::TypeArray);
00116   for (int32_t i = 0; i < tag_descriptions.size(); ++i) {
00117     XmlRpc::XmlRpcValue& tag_description = tag_descriptions[i];
00118     ROS_ASSERT(tag_description.getType() == XmlRpc::XmlRpcValue::TypeStruct);
00119     ROS_ASSERT(tag_description["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
00120     ROS_ASSERT(tag_description["size"].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00121 
00122     int id = (int)tag_description["id"];
00123     double size = (double)tag_description["size"];
00124 
00125     std::string frame_name;
00126     if(tag_description.hasMember("frame_id")){
00127       ROS_ASSERT(tag_description["frame_id"].getType() == XmlRpc::XmlRpcValue::TypeString);
00128       frame_name = (std::string)tag_description["frame_id"];
00129     }
00130     else{
00131       std::stringstream frame_name_stream;
00132       frame_name_stream << "tag_" << id;
00133       frame_name = frame_name_stream.str();
00134     }
00135     AprilTagDescription description(id, size, frame_name);
00136     ROS_INFO_STREAM("Loaded tag config: "<<id<<", size: "<<size<<", frame_name: "<<frame_name);
00137     descriptions.insert(std::make_pair(id, description));
00138   }
00139   return descriptions;
00140 }
00141 
00142 
00143 }


apriltags_ros
Author(s): Mitchell Wills
autogenerated on Thu Aug 27 2015 12:23:32