FindMarkerBundles.cpp
Go to the documentation of this file.
00001 /*
00002   Software License Agreement (BSD License)
00003 
00004   Copyright (c) 2012, Scott Niekum
00005   All rights reserved.
00006 
00007   Redistribution and use in source and binary forms, with or without
00008   modification, are permitted provided that the following conditions
00009   are met:
00010 
00011   * Redistributions of source code must retain the above copyright
00012   notice, this list of conditions and the following disclaimer.
00013   * Redistributions in binary form must reproduce the above
00014   copyright notice, this list of conditions and the following
00015   disclaimer in the documentation and/or other materials provided
00016   with the distribution.
00017   * Neither the name of the Willow Garage nor the names of its
00018   contributors may be used to endorse or promote products derived
00019   from this software without specific prior written permission.
00020 
00021   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025   COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032   POSSIBILITY OF SUCH DAMAGE.
00033 
00034   author: Scott Niekum
00035 */
00036 #include "ar_track_alvar/CvTestbed.h"
00037 #include "ar_track_alvar/MarkerDetector.h"
00038 #include "ar_track_alvar/MultiMarkerBundle.h"
00039 #include "ar_track_alvar/MultiMarkerInitializer.h"
00040 #include "ar_track_alvar/Shared.h"
00041 #include <cv_bridge/cv_bridge.h>
00042 #include <ar_track_alvar_msgs/AlvarMarker.h>
00043 #include <ar_track_alvar_msgs/AlvarMarkers.h>
00044 #include <tf/transform_listener.h>
00045 #include <tf/transform_broadcaster.h>
00046 
00047 #include <sensor_msgs/PointCloud2.h>
00048 #include <pcl_conversions/pcl_conversions.h>
00049 #include <pcl/point_types.h>
00050 #include <pcl/registration/icp.h>
00051 #include <pcl/registration/registration.h>
00052 
00053 #include <geometry_msgs/PoseStamped.h>
00054 #include <sensor_msgs/image_encodings.h>
00055 #include <ros/ros.h>
00056 #include <pcl/ModelCoefficients.h>
00057 #include <pcl/point_types.h>
00058 #include <pcl/sample_consensus/method_types.h>
00059 #include <pcl/sample_consensus/model_types.h>
00060 #include <pcl/segmentation/sac_segmentation.h>
00061 #include <pcl_ros/point_cloud.h>
00062 #include <pcl/filters/extract_indices.h>
00063 #include <boost/lexical_cast.hpp>
00064 
00065 #include <tf/tf.h>
00066 #include <Eigen/Core>
00067 #include <ar_track_alvar/filter/kinect_filtering.h>
00068 #include <ar_track_alvar/filter/medianFilter.h>
00069 
00070 
00071 #define MAIN_MARKER 1
00072 #define VISIBLE_MARKER 2
00073 #define GHOST_MARKER 3
00074 
00075 namespace gm=geometry_msgs;
00076 namespace ata=ar_track_alvar;
00077 
00078 typedef pcl::PointXYZRGB ARPoint;
00079 typedef pcl::PointCloud<ARPoint> ARCloud;
00080 
00081 using namespace alvar;
00082 using namespace std;
00083 using boost::make_shared;
00084 
00085 Camera *cam;
00086 cv_bridge::CvImagePtr cv_ptr_;
00087 image_transport::Subscriber cam_sub_;
00088 ros::Subscriber cloud_sub_;
00089 ros::Publisher arMarkerPub_;
00090 ros::Publisher rvizMarkerPub_;
00091 ros::Publisher rvizMarkerPub2_;
00092 ar_track_alvar_msgs::AlvarMarkers arPoseMarkers_;
00093 tf::TransformListener *tf_listener;
00094 tf::TransformBroadcaster *tf_broadcaster;
00095 MarkerDetector<MarkerData> marker_detector;
00096 MultiMarkerBundle **multi_marker_bundles=NULL;
00097 
00098 Pose *bundlePoses;
00099 int *master_id;
00100 int *bundles_seen;
00101 bool *master_visible;
00102 std::vector<int> *bundle_indices;       
00103 bool init = true;
00104 ata::MedianFilter **med_filts;
00105 int med_filt_size;
00106 
00107 double marker_size;
00108 double max_new_marker_error;
00109 double max_track_error;
00110 std::string cam_image_topic; 
00111 std::string cam_info_topic; 
00112 std::string output_frame;
00113 int n_bundles = 0;   
00114 
00115 //Debugging utility function
00116 void draw3dPoints(ARCloud::Ptr cloud, string frame, int color, int id, double rad)
00117 {
00118   visualization_msgs::Marker rvizMarker;
00119 
00120   rvizMarker.header.frame_id = frame;
00121   rvizMarker.header.stamp = ros::Time::now(); 
00122   rvizMarker.id = id;
00123   rvizMarker.ns = "3dpts";
00124   
00125   rvizMarker.scale.x = rad;
00126   rvizMarker.scale.y = rad;
00127   rvizMarker.scale.z = rad;
00128   
00129   rvizMarker.type = visualization_msgs::Marker::SPHERE_LIST;
00130   rvizMarker.action = visualization_msgs::Marker::ADD;
00131   
00132   if(color==1){
00133     rvizMarker.color.r = 0.0f;
00134     rvizMarker.color.g = 1.0f;
00135     rvizMarker.color.b = 1.0f;
00136     rvizMarker.color.a = 1.0;
00137   }
00138   if(color==2){
00139     rvizMarker.color.r = 1.0f;
00140     rvizMarker.color.g = 0.0f;
00141     rvizMarker.color.b = 1.0f;
00142     rvizMarker.color.a = 1.0;
00143   }
00144   if(color==3){
00145     rvizMarker.color.r = 1.0f;
00146     rvizMarker.color.g = 1.0f;
00147     rvizMarker.color.b = 0.0f;
00148     rvizMarker.color.a = 1.0;
00149   }
00150   
00151   gm::Point p;
00152   for(int i=0; i<cloud->points.size(); i++){
00153     p.x = cloud->points[i].x;
00154     p.y = cloud->points[i].y;
00155     p.z = cloud->points[i].z;
00156     rvizMarker.points.push_back(p);
00157   }
00158   
00159   rvizMarker.lifetime = ros::Duration (1.0);
00160   rvizMarkerPub2_.publish (rvizMarker);
00161 }
00162 
00163 
00164 void drawArrow(gm::Point start, tf::Matrix3x3 mat, string frame, int color, int id)
00165 {
00166   visualization_msgs::Marker rvizMarker;
00167   
00168   rvizMarker.header.frame_id = frame;
00169   rvizMarker.header.stamp = ros::Time::now(); 
00170   rvizMarker.id = id;
00171   rvizMarker.ns = "arrow";
00172   
00173   rvizMarker.scale.x = 0.01;
00174   rvizMarker.scale.y = 0.01;
00175   rvizMarker.scale.z = 0.1;
00176   
00177   rvizMarker.type = visualization_msgs::Marker::ARROW;
00178   rvizMarker.action = visualization_msgs::Marker::ADD;
00179   
00180   for(int i=0; i<3; i++){
00181     rvizMarker.points.clear();  
00182     rvizMarker.points.push_back(start);
00183     gm::Point end;
00184     end.x = start.x + mat[0][i];
00185     end.y = start.y + mat[1][i];
00186     end.z = start.z + mat[2][i];
00187     rvizMarker.points.push_back(end);
00188     rvizMarker.id += 10*i;
00189     rvizMarker.lifetime = ros::Duration (1.0);
00190 
00191     if(color==1){
00192       rvizMarker.color.r = 1.0f;
00193       rvizMarker.color.g = 0.0f;
00194       rvizMarker.color.b = 0.0f;
00195       rvizMarker.color.a = 1.0;
00196     }
00197     if(color==2){
00198       rvizMarker.color.r = 0.0f;
00199       rvizMarker.color.g = 1.0f;
00200       rvizMarker.color.b = 0.0f;
00201       rvizMarker.color.a = 1.0;
00202     }
00203     if(color==3){
00204       rvizMarker.color.r = 0.0f;
00205       rvizMarker.color.g = 0.0f;
00206       rvizMarker.color.b = 1.0f;
00207       rvizMarker.color.a = 1.0;
00208     }
00209     color += 1;
00210 
00211     rvizMarkerPub2_.publish (rvizMarker);
00212   }
00213 }
00214 
00215 
00216 // Infer the master tag corner positons from the other observed tags
00217 // Also does some of the bookkeeping for tracking that MultiMarker::_GetPose does 
00218 int InferCorners(const ARCloud &cloud, MultiMarkerBundle &master, ARCloud &bund_corners){
00219   bund_corners.clear();
00220   bund_corners.resize(4);
00221   for(int i=0; i<4; i++){
00222     bund_corners[i].x = 0;
00223     bund_corners[i].y = 0;
00224     bund_corners[i].z = 0;
00225   }
00226 
00227   // Reset the marker_status to 1 for all markers in point_cloud for tracking purposes
00228   for (size_t i=0; i<master.marker_status.size(); i++) {
00229     if (master.marker_status[i] > 0) master.marker_status[i]=1;
00230   }
00231 
00232   int n_est = 0;
00233 
00234   // For every detected marker
00235   for (size_t i=0; i<marker_detector.markers->size(); i++)
00236     {
00237       const Marker* marker = &((*marker_detector.markers)[i]);
00238       int id = marker->GetId();
00239       int index = master.get_id_index(id);
00240       int mast_id = master.master_id;
00241       if (index < 0) continue;
00242 
00243       // But only if we have corresponding points in the pointcloud
00244       if (master.marker_status[index] > 0 && marker->valid) {
00245         n_est++;
00246 
00247         std::string marker_frame = "ar_marker_";
00248         std::stringstream mark_out;
00249         mark_out << id;
00250         std::string id_string = mark_out.str();
00251         marker_frame += id_string;
00252 
00253         //Grab the precomputed corner coords and correct for the weird Alvar coord system
00254         for(int j = 0; j < 4; ++j)
00255         {
00256             tf::Vector3 corner_coord = master.rel_corners[index][j];
00257             gm::PointStamped p, output_p;
00258             p.header.frame_id = marker_frame;
00259             p.point.x = corner_coord.y()/100.0;  
00260             p.point.y = -corner_coord.x()/100.0;
00261             p.point.z = corner_coord.z()/100.0;
00262             
00263             try{
00264                 tf_listener->waitForTransform(cloud.header.frame_id, marker_frame, ros::Time(0), ros::Duration(0.1));
00265                 tf_listener->transformPoint(cloud.header.frame_id, p, output_p);                        
00266             }
00267             catch (tf::TransformException ex){
00268                 ROS_ERROR("ERROR InferCorners: %s",ex.what());
00269                 return -1;
00270             }
00271 
00272             bund_corners[j].x += output_p.point.x;
00273             bund_corners[j].y += output_p.point.y;
00274             bund_corners[j].z += output_p.point.z;
00275         }
00276         master.marker_status[index] = 2; // Used for tracking
00277       }
00278     }
00279   
00280   //Divide to take the average of the summed estimates
00281   if(n_est > 0){
00282     for(int i=0; i<4; i++){
00283         bund_corners[i].x /= n_est;
00284         bund_corners[i].y /= n_est;
00285         bund_corners[i].z /= n_est;
00286     }
00287   }
00288 
00289   return 0;
00290 }
00291 
00292 
00293 int PlaneFitPoseImprovement(int id, const ARCloud &corners_3D, ARCloud::Ptr selected_points, const ARCloud &cloud, Pose &p){
00294 
00295   ata::PlaneFitResult res = ata::fitPlane(selected_points);
00296   gm::PoseStamped pose;
00297   pose.header.stamp = pcl_conversions::fromPCL(cloud.header).stamp;
00298   pose.header.frame_id = cloud.header.frame_id;
00299   pose.pose.position = ata::centroid(*res.inliers);
00300 
00301   draw3dPoints(selected_points, cloud.header.frame_id, 1, id, 0.005);
00302           
00303   //Get 2 points that point forward in marker x direction   
00304   int i1,i2;
00305   if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) || 
00306      isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z))
00307     {
00308       if(isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z) || 
00309          isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z))
00310         {
00311           return -1;
00312         }
00313       else{
00314         i1 = 1;
00315         i2 = 2;
00316       } 
00317     }
00318   else{
00319     i1 = 0;
00320     i2 = 3;
00321   }
00322 
00323   //Get 2 points the point forward in marker y direction   
00324   int i3,i4;
00325   if(isnan(corners_3D[0].x) || isnan(corners_3D[0].y) || isnan(corners_3D[0].z) || 
00326      isnan(corners_3D[1].x) || isnan(corners_3D[1].y) || isnan(corners_3D[1].z))
00327     {   
00328       if(isnan(corners_3D[3].x) || isnan(corners_3D[3].y) || isnan(corners_3D[3].z) || 
00329          isnan(corners_3D[2].x) || isnan(corners_3D[2].y) || isnan(corners_3D[2].z))
00330         {   
00331           return -1;
00332         }
00333       else{
00334         i3 = 2;
00335         i4 = 3;
00336       } 
00337     }
00338   else{
00339     i3 = 1;
00340     i4 = 0;
00341   }
00342    
00343   ARCloud::Ptr orient_points(new ARCloud());
00344   orient_points->points.push_back(corners_3D[i1]);
00345   draw3dPoints(orient_points, cloud.header.frame_id, 3, id+1000, 0.008);
00346       
00347   orient_points->clear();
00348   orient_points->points.push_back(corners_3D[i2]);
00349   draw3dPoints(orient_points, cloud.header.frame_id, 2, id+2000, 0.008);
00350  
00351   int succ;
00352   succ = ata::extractOrientation(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], pose.pose.orientation);
00353   if(succ < 0) return -1;
00354 
00355   tf::Matrix3x3 mat;
00356   succ = ata::extractFrame(res.coeffs, corners_3D[i1], corners_3D[i2], corners_3D[i3], corners_3D[i4], mat);
00357   if(succ < 0) return -1;
00358 
00359   drawArrow(pose.pose.position, mat, cloud.header.frame_id, 1, id);
00360 
00361   p.translation[0] = pose.pose.position.x * 100.0;
00362   p.translation[1] = pose.pose.position.y * 100.0;
00363   p.translation[2] = pose.pose.position.z * 100.0;
00364   p.quaternion[1] = pose.pose.orientation.x;
00365   p.quaternion[2] = pose.pose.orientation.y;
00366   p.quaternion[3] = pose.pose.orientation.z;
00367   p.quaternion[0] = pose.pose.orientation.w; 
00368 
00369   return 0;
00370 }
00371 
00372 
00373 // Updates the bundlePoses of the multi_marker_bundles by detecting markers and
00374 // using all markers in a bundle to infer the master tag's position
00375 void GetMultiMarkerPoses(IplImage *image, ARCloud &cloud) {
00376 
00377   for(int i=0; i<n_bundles; i++){
00378     master_visible[i] = false;
00379     bundles_seen[i] = 0;
00380   }
00381   
00382   //Detect and track the markers
00383   if (marker_detector.Detect(image, cam, true, false, max_new_marker_error,
00384                              max_track_error, CVSEQ, true)) 
00385     {
00386       //printf("\n--------------------------\n\n");
00387       for (size_t i=0; i<marker_detector.markers->size(); i++)
00388         {
00389           vector<cv::Point, Eigen::aligned_allocator<cv::Point> > pixels;
00390           Marker *m = &((*marker_detector.markers)[i]);
00391           int id = m->GetId();
00392           //cout << "******* ID: " << id << endl;
00393       
00394           //Get the 3D points of the outer corners
00395           /*
00396           PointDouble corner0 = m->marker_corners_img[0];
00397           PointDouble corner1 = m->marker_corners_img[1];
00398           PointDouble corner2 = m->marker_corners_img[2];
00399           PointDouble corner3 = m->marker_corners_img[3];
00400           m->ros_corners_3D[0] = cloud(corner0.x, corner0.y);
00401           m->ros_corners_3D[1] = cloud(corner1.x, corner1.y);
00402           m->ros_corners_3D[2] = cloud(corner2.x, corner2.y);
00403           m->ros_corners_3D[3] = cloud(corner3.x, corner3.y);
00404           */
00405           
00406           //Get the 3D inner corner points - more stable than outer corners that can "fall off" object
00407           int resol = m->GetRes();
00408           int ori = m->ros_orientation;
00409       
00410           PointDouble pt1, pt2, pt3, pt4;
00411           pt4 = m->ros_marker_points_img[0];
00412           pt3 = m->ros_marker_points_img[resol-1];
00413           pt1 = m->ros_marker_points_img[(resol*resol)-resol];
00414           pt2 = m->ros_marker_points_img[(resol*resol)-1];
00415           
00416           m->ros_corners_3D[0] = cloud(pt1.x, pt1.y);
00417           m->ros_corners_3D[1] = cloud(pt2.x, pt2.y);
00418           m->ros_corners_3D[2] = cloud(pt3.x, pt3.y);
00419           m->ros_corners_3D[3] = cloud(pt4.x, pt4.y);
00420           
00421           if(ori >= 0 && ori < 4){
00422             if(ori != 0){
00423               std::rotate(m->ros_corners_3D.begin(), m->ros_corners_3D.begin() + ori, m->ros_corners_3D.end());
00424             }
00425           }
00426           else
00427             ROS_ERROR("FindMarkerBundles: Bad Orientation: %i for ID: %i", ori, id);
00428 
00429           //Check if we have spotted a master tag
00430           int master_ind = -1;
00431           for(int j=0; j<n_bundles; j++){
00432             if(id == master_id[j])
00433               master_visible[j] = true; 
00434             master_ind = j;
00435           }
00436 
00437           //Mark the bundle that marker belongs to as "seen"
00438           int bundle_ind = -1;
00439           for(int j=0; j<n_bundles; j++){
00440             for(int k=0; k<bundle_indices[j].size(); k++){
00441               if(bundle_indices[j][k] == id){
00442             bundle_ind = j;
00443             bundles_seen[j] += 1;
00444             break;
00445           }
00446         }
00447       }
00448 
00449           //Get the 3D marker points
00450           BOOST_FOREACH (const PointDouble& p, m->ros_marker_points_img)
00451             pixels.push_back(cv::Point(p.x, p.y));        
00452           ARCloud::Ptr selected_points = ata::filterCloud(cloud, pixels);
00453 
00454           //Use the kinect data to find a plane and pose for the marker
00455           int ret = PlaneFitPoseImprovement(i, m->ros_corners_3D, selected_points, cloud, m->pose);
00456             
00457           //If the plane fit fails...
00458           if(ret < 0){
00459                 //Mark this tag as invalid
00460                 m->valid = false;
00461             //If this was a master tag, reset its visibility
00462             if(master_ind >= 0)
00463               master_visible[master_ind] = false;
00464             //decrement the number of markers seen in this bundle
00465             bundles_seen[bundle_ind] -= 1;
00466               
00467           }
00468           else
00469                 m->valid = true;
00470         }       
00471 
00472       //For each master tag, infer the 3D position of its corners from other visible tags
00473       //Then, do a plane fit to those new corners       
00474       ARCloud inferred_corners;
00475       for(int i=0; i<n_bundles; i++){
00476         if(bundles_seen[i] > 0){
00477             //if(master_visible[i] == false){
00478                 if(InferCorners(cloud, *(multi_marker_bundles[i]), inferred_corners) >= 0){
00479                     ARCloud::Ptr inferred_cloud(new ARCloud(inferred_corners));
00480                     PlaneFitPoseImprovement(i+5000, inferred_corners, inferred_cloud, cloud, bundlePoses[i]);
00481                 }
00482             //}
00483             //If master is visible, use it directly instead of inferring pose
00484             //else{
00485             //    for (size_t j=0; j<marker_detector.markers->size(); j++){
00486             //        Marker *m = &((*marker_detector.markers)[j]);                     
00487             //        if(m->GetId() == master_id[i])
00488             //            bundlePoses[i] = m->pose;
00489             //    } 
00490             //}
00491             Pose ret_pose;
00492             if(med_filt_size > 0){
00493                 med_filts[i]->addPose(bundlePoses[i]);
00494                 med_filts[i]->getMedian(ret_pose);
00495                 bundlePoses[i] = ret_pose;
00496             }   
00497         }               
00498     }
00499   }
00500 }
00501 
00502 
00503 // Given the pose of a marker, builds the appropriate ROS messages for later publishing 
00504 void makeMarkerMsgs(int type, int id, Pose &p, sensor_msgs::ImageConstPtr image_msg, tf::StampedTransform &CamToOutput, visualization_msgs::Marker *rvizMarker, ar_track_alvar_msgs::AlvarMarker *ar_pose_marker, int confidence){
00505   double px,py,pz,qx,qy,qz,qw;
00506         
00507   px = p.translation[0]/100.0;
00508   py = p.translation[1]/100.0;
00509   pz = p.translation[2]/100.0;
00510   qx = p.quaternion[1];
00511   qy = p.quaternion[2];
00512   qz = p.quaternion[3];
00513   qw = p.quaternion[0];
00514 
00515   //Get the marker pose in the camera frame
00516   tf::Quaternion rotation (qx,qy,qz,qw);
00517   tf::Vector3 origin (px,py,pz);
00518   tf::Transform t (rotation, origin);  //transform from cam to marker
00519 
00520   tf::Vector3 markerOrigin (0, 0, 0);
00521   tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
00522   tf::Transform markerPose = t * m;
00523 
00524   //Publish the cam to marker transform for each marker
00525   std::string markerFrame = "ar_marker_";
00526   std::stringstream out;
00527   out << id;
00528   std::string id_string = out.str();
00529   markerFrame += id_string;
00530   tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
00531   tf_broadcaster->sendTransform(camToMarker);
00532 
00533   //Create the rviz visualization message
00534   tf::poseTFToMsg (markerPose, rvizMarker->pose);
00535   rvizMarker->header.frame_id = image_msg->header.frame_id;
00536   rvizMarker->header.stamp = image_msg->header.stamp;
00537   rvizMarker->id = id;
00538 
00539   rvizMarker->scale.x = 1.0 * marker_size/100.0;
00540   rvizMarker->scale.y = 1.0 * marker_size/100.0;
00541   rvizMarker->scale.z = 0.2 * marker_size/100.0;
00542 
00543   if(type==MAIN_MARKER)
00544     rvizMarker->ns = "main_shapes";
00545   else
00546     rvizMarker->ns = "basic_shapes";
00547 
00548 
00549   rvizMarker->type = visualization_msgs::Marker::CUBE;
00550   rvizMarker->action = visualization_msgs::Marker::ADD;
00551 
00552   //Determine a color and opacity, based on marker type
00553   if(type==MAIN_MARKER){
00554     rvizMarker->color.r = 1.0f;
00555     rvizMarker->color.g = 0.0f;
00556     rvizMarker->color.b = 0.0f;
00557     rvizMarker->color.a = 1.0;
00558   }
00559   else if(type==VISIBLE_MARKER){
00560     rvizMarker->color.r = 0.0f;
00561     rvizMarker->color.g = 1.0f;
00562     rvizMarker->color.b = 0.0f;
00563     rvizMarker->color.a = 0.7;
00564   }
00565   else if(type==GHOST_MARKER){
00566     rvizMarker->color.r = 0.0f;
00567     rvizMarker->color.g = 0.0f;
00568     rvizMarker->color.b = 1.0f;
00569     rvizMarker->color.a = 0.5;
00570   }
00571 
00572   rvizMarker->lifetime = ros::Duration (0.1);
00573 
00574   // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization 
00575   if(type==MAIN_MARKER){
00576     //Take the pose of the tag in the camera frame and convert to the output frame (usually torso_lift_link for the PR2)
00577     tf::Transform tagPoseOutput = CamToOutput * markerPose;
00578 
00579     //Create the pose marker message
00580     tf::poseTFToMsg (tagPoseOutput, ar_pose_marker->pose.pose);
00581     ar_pose_marker->header.frame_id = output_frame;
00582     ar_pose_marker->header.stamp = image_msg->header.stamp;
00583     ar_pose_marker->id = id;
00584     ar_pose_marker->confidence = confidence;
00585   }
00586   else
00587     ar_pose_marker = NULL;
00588 }
00589 
00590 
00591 
00592 //Callback to handle getting kinect point clouds and processing them
00593 void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
00594 {
00595   sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
00596 
00597   //If we've already gotten the cam info, then go ahead
00598   if(cam->getCamInfo_){
00599     try{
00600       //Get the transformation from the Camera to the output frame for this image capture
00601       tf::StampedTransform CamToOutput;
00602       try{
00603         tf_listener->waitForTransform(output_frame, msg->header.frame_id, msg->header.stamp, ros::Duration(1.0));
00604         tf_listener->lookupTransform(output_frame, msg->header.frame_id, msg->header.stamp, CamToOutput);
00605       }
00606       catch (tf::TransformException ex){
00607         ROS_ERROR("%s",ex.what());
00608       }
00609 
00610       //Init and clear visualization markers
00611       visualization_msgs::Marker rvizMarker;
00612       ar_track_alvar_msgs::AlvarMarker ar_pose_marker;
00613       arPoseMarkers_.markers.clear ();
00614 
00615       //Convert cloud to PCL 
00616       ARCloud cloud;
00617       pcl::fromROSMsg(*msg, cloud);
00618 
00619       //Get an OpenCV image from the cloud
00620       pcl::toROSMsg (*msg, *image_msg);
00621       image_msg->header.stamp = msg->header.stamp;
00622       image_msg->header.frame_id = msg->header.frame_id;
00623             
00624       //Convert the image
00625       cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
00626 
00627       //Get the estimated pose of the main markers by using all the markers in each bundle
00628 
00629       // GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
00630       // us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
00631       // do this conversion here -jbinney
00632       IplImage ipl_image = cv_ptr_->image;
00633       GetMultiMarkerPoses(&ipl_image, cloud);
00634 
00635       for (size_t i=0; i<marker_detector.markers->size(); i++)
00636         {
00637           int id = (*(marker_detector.markers))[i].GetId();     
00638 
00639           // Draw if id is valid
00640           if(id >= 0){
00641 
00642             // Don't draw if it is a master tag...we do this later, a bit differently
00643             bool should_draw = true;
00644             for(int j=0; j<n_bundles; j++){
00645               if(id == master_id[j]) should_draw = false;
00646             }
00647             if(should_draw && (*(marker_detector.markers))[i].valid){
00648               Pose p = (*(marker_detector.markers))[i].pose;
00649               makeMarkerMsgs(VISIBLE_MARKER, id, p, image_msg, CamToOutput, &rvizMarker, &ar_pose_marker, 1);
00650               rvizMarkerPub_.publish (rvizMarker);
00651             }
00652           }
00653         }
00654                         
00655       //Draw the main markers, whether they are visible or not -- but only if at least 1 marker from their bundle is currently seen
00656       for(int i=0; i<n_bundles; i++)
00657         {
00658           if(bundles_seen[i] > 0){
00659             makeMarkerMsgs(MAIN_MARKER, master_id[i], bundlePoses[i], image_msg, CamToOutput, &rvizMarker, &ar_pose_marker, bundles_seen[i]);
00660             rvizMarkerPub_.publish (rvizMarker);
00661             arPoseMarkers_.markers.push_back (ar_pose_marker);
00662           }
00663         }
00664 
00665       //Publish the marker messages
00666       arMarkerPub_.publish (arPoseMarkers_);
00667     }
00668     catch (cv_bridge::Exception& e){
00669       ROS_ERROR ("ar_track_alvar: Image error: %s", image_msg->encoding.c_str ());
00670     }
00671   }
00672 }
00673 
00674 
00675 //Create a ROS frame out of the known corners of a tag in the weird marker coord frame used by Alvar markers (x right y forward z up)
00676 //p0-->p1 should point in Alvar's pos X direction
00677 //p1-->p2 should point in Alvar's pos Y direction
00678 int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1,
00679                          const CvPoint3D64f& p2, const CvPoint3D64f& p3,
00680                          tf::Transform &retT)
00681   {
00682     const tf::Vector3 q0(p0.x, p0.y, p0.z);
00683     const tf::Vector3 q1(p1.x, p1.y, p1.z);
00684     const tf::Vector3 q2(p2.x, p2.y, p2.z);
00685     const tf::Vector3 q3(p3.x, p3.y, p3.z);
00686   
00687     // (inverse) matrix with the given properties
00688     const tf::Vector3 v = (q1-q0).normalized();
00689     const tf::Vector3 w = (q2-q1).normalized();
00690     const tf::Vector3 n = v.cross(w);
00691     tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);
00692     m = m.inverse();
00693     
00694     //Translate to quaternion
00695     if(m.determinant() <= 0)
00696         return -1;
00697   
00698     //Use Eigen for this part instead, because the ROS version of bullet appears to have a bug
00699     Eigen::Matrix3f eig_m;
00700     for(int i=0; i<3; i++){
00701         for(int j=0; j<3; j++){
00702             eig_m(i,j) = m[i][j];
00703         }
00704     }
00705     Eigen::Quaternion<float> eig_quat(eig_m);
00706     
00707     // Translate back to bullet
00708     tfScalar ex = eig_quat.x();
00709     tfScalar ey = eig_quat.y();
00710     tfScalar ez = eig_quat.z();
00711     tfScalar ew = eig_quat.w();
00712     tf::Quaternion quat(ex,ey,ez,ew);
00713     quat = quat.normalized();
00714     
00715     double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0;
00716     double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0;
00717     double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0;
00718     tf::Vector3 origin (qx,qy,qz);
00719     
00720     tf::Transform tform (quat, origin);  //transform from master to marker
00721     retT = tform;
00722     
00723     return 0;
00724   }
00725 
00726 
00727 //Find the coordinates of the Master marker with respect to the coord frame of each of it's child markers
00728 //This data is used for later estimation of the Master marker pose from the child poses
00729 int calcAndSaveMasterCoords(MultiMarkerBundle &master)
00730 {
00731     int mast_id = master.master_id;
00732     std::vector<tf::Vector3> rel_corner_coords;
00733     
00734     //Go through all the markers associated with this bundle
00735     for (size_t i=0; i<master.marker_indices.size(); i++){
00736         int mark_id = master.marker_indices[i];
00737         rel_corner_coords.clear();
00738         
00739         //Get the coords of the corners of the child marker in the master frame
00740         CvPoint3D64f mark_corners[4];
00741         for(int j=0; j<4; j++){
00742             mark_corners[j] = master.pointcloud[master.pointcloud_index(mark_id, j)];
00743         }
00744         
00745         //Use them to find a transform from the master frame to the child frame
00746         tf::Transform tform;
00747         makeMasterTransform(mark_corners[0], mark_corners[1], mark_corners[2], mark_corners[3], tform);
00748     
00749         //Finally, find the coords of the corners of the master in the child frame
00750         for(int j=0; j<4; j++){
00751             
00752             CvPoint3D64f corner_coord = master.pointcloud[master.pointcloud_index(mast_id, j)];
00753             double px = corner_coord.x;
00754             double py = corner_coord.y;
00755             double pz = corner_coord.z;
00756         
00757             tf::Vector3 corner_vec (px, py, pz);
00758             tf::Vector3 ans = (tform.inverse()) * corner_vec;
00759             rel_corner_coords.push_back(ans);
00760         }
00761         
00762         master.rel_corners.push_back(rel_corner_coords);
00763     }
00764     
00765     return 0;
00766 }
00767 
00768 
00769 int main(int argc, char *argv[])
00770 {
00771   ros::init (argc, argv, "marker_detect");
00772   ros::NodeHandle n;
00773 
00774   if(argc < 9){
00775     std::cout << std::endl;
00776     cout << "Not enough arguments provided." << endl;
00777     cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <median filt size> <list of bundle XML files...>" << endl;
00778     std::cout << std::endl;
00779     return 0;
00780   }
00781 
00782   // Get params from command line
00783   marker_size = atof(argv[1]);
00784   max_new_marker_error = atof(argv[2]);
00785   max_track_error = atof(argv[3]);
00786   cam_image_topic = argv[4]; 
00787   cam_info_topic = argv[5];
00788   output_frame = argv[6];
00789   med_filt_size = atoi(argv[7]);
00790   int n_args_before_list = 8;
00791   n_bundles = argc - n_args_before_list;
00792 
00793   marker_detector.SetMarkerSize(marker_size);
00794   multi_marker_bundles = new MultiMarkerBundle*[n_bundles];     
00795   bundlePoses = new Pose[n_bundles];
00796   master_id = new int[n_bundles]; 
00797   bundle_indices = new std::vector<int>[n_bundles]; 
00798   bundles_seen = new int[n_bundles]; 
00799   master_visible = new bool[n_bundles];
00800         
00801   //Create median filters
00802   med_filts = new ata::MedianFilter*[n_bundles];
00803   for(int i=0; i<n_bundles; i++)
00804     med_filts[i] = new ata::MedianFilter(med_filt_size);
00805 
00806   // Load the marker bundle XML files
00807   for(int i=0; i<n_bundles; i++){       
00808     bundlePoses[i].Reset();             
00809     MultiMarker loadHelper;
00810     if(loadHelper.Load(argv[i + n_args_before_list], FILE_FORMAT_XML)){
00811       vector<int> id_vector = loadHelper.getIndices();
00812       multi_marker_bundles[i] = new MultiMarkerBundle(id_vector);       
00813       multi_marker_bundles[i]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML);
00814       master_id[i] = multi_marker_bundles[i]->getMasterId();
00815       bundle_indices[i] = multi_marker_bundles[i]->getIndices();
00816       calcAndSaveMasterCoords(*(multi_marker_bundles[i]));
00817     }
00818     else{
00819       cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl; 
00820       return 0;
00821     }           
00822   }  
00823 
00824   // Set up camera, listeners, and broadcasters
00825   cam = new Camera(n, cam_info_topic);
00826   tf_listener = new tf::TransformListener(n);
00827   tf_broadcaster = new tf::TransformBroadcaster();
00828   arMarkerPub_ = n.advertise < ar_track_alvar_msgs::AlvarMarkers > ("ar_pose_marker", 0);
00829   rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00830   rvizMarkerPub2_ = n.advertise < visualization_msgs::Marker > ("ARmarker_points", 0);
00831         
00832   //Give tf a chance to catch up before the camera callback starts asking for transforms
00833   ros::Duration(1.0).sleep();
00834   ros::spinOnce();                      
00835          
00836   //Subscribe to topics and set up callbacks
00837   ROS_INFO ("Subscribing to image topic");
00838   cloud_sub_ = n.subscribe(cam_image_topic, 1, &getPointCloudCallback);
00839 
00840   ros::spin();
00841 
00842   return 0;
00843 }
00844 
00845 


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02