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


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sat Dec 28 2013 16:46:15