FindMarkerBundlesNoKinect.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 using namespace alvar;
00049 using namespace std;
00050 
00051 #define MAIN_MARKER 1
00052 #define VISIBLE_MARKER 2
00053 #define GHOST_MARKER 3
00054 
00055 Camera *cam;
00056 IplImage *capture_;
00057 sensor_msgs::CvBridge bridge_;
00058 image_transport::Subscriber cam_sub_;
00059 ros::Publisher arMarkerPub_;
00060 ros::Publisher rvizMarkerPub_;
00061 ar_track_alvar::AlvarMarkers arPoseMarkers_;
00062 tf::TransformListener *tf_listener;
00063 tf::TransformBroadcaster *tf_broadcaster;
00064 MarkerDetector<MarkerData> marker_detector;
00065 MultiMarkerBundle **multi_marker_bundles=NULL;
00066 Pose *bundlePoses;
00067 int *master_id;
00068 bool *bundles_seen;
00069 std::vector<int> *bundle_indices;       
00070 bool init = true;  
00071 
00072 double marker_size;
00073 double max_new_marker_error;
00074 double max_track_error;
00075 std::string cam_image_topic; 
00076 std::string cam_info_topic; 
00077 std::string output_frame;
00078 int n_bundles = 0;   
00079 
00080 void GetMultiMarkerPoses(IplImage *image);
00081 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
00082 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);
00083 
00084 
00085 // Updates the bundlePoses of the multi_marker_bundles by detecting markers and using all markers in a bundle to infer the master tag's position
00086 void GetMultiMarkerPoses(IplImage *image) {
00087 
00088   if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)){
00089     for(int i=0; i<n_bundles; i++)
00090       multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);
00091     
00092     if(marker_detector.DetectAdditional(image, cam, false) > 0){
00093       for(int i=0; i<n_bundles; i++){
00094         if ((multi_marker_bundles[i]->SetTrackMarkers(marker_detector, cam, bundlePoses[i], image) > 0))
00095           multi_marker_bundles[i]->Update(marker_detector.markers, cam, bundlePoses[i]);
00096       }
00097     }
00098   }
00099 }
00100 
00101 
00102 // Given the pose of a marker, builds the appropriate ROS messages for later publishing 
00103 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){
00104   double px,py,pz,qx,qy,qz,qw;
00105         
00106   px = p.translation[0]/100.0;
00107   py = p.translation[1]/100.0;
00108   pz = p.translation[2]/100.0;
00109   qx = p.quaternion[1];
00110   qy = p.quaternion[2];
00111   qz = p.quaternion[3];
00112   qw = p.quaternion[0];
00113 
00114   //Get the marker pose in the camera frame
00115   btQuaternion rotation (qx,qy,qz,qw);
00116   btVector3 origin (px,py,pz);
00117   btTransform t (rotation, origin);  //transform from cam to marker
00118 
00119   btVector3 markerOrigin (0, 0, 0);
00120   btTransform m (btQuaternion::getIdentity (), markerOrigin);
00121   btTransform markerPose = t * m;
00122 
00123   //Publish the cam to marker transform for main marker in each bundle
00124   if(type==MAIN_MARKER){
00125     std::string markerFrame = "ar_marker_";
00126     std::stringstream out;
00127     out << id;
00128     std::string id_string = out.str();
00129     markerFrame += id_string;
00130     tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
00131     tf_broadcaster->sendTransform(camToMarker);
00132   }
00133 
00134   //Create the rviz visualization message
00135   tf::poseTFToMsg (markerPose, rvizMarker->pose);
00136   rvizMarker->header.frame_id = image_msg->header.frame_id;
00137   rvizMarker->header.stamp = image_msg->header.stamp;
00138   rvizMarker->id = id;
00139 
00140   rvizMarker->scale.x = 1.0 * marker_size/100.0;
00141   rvizMarker->scale.y = 1.0 * marker_size/100.0;
00142   rvizMarker->scale.z = 0.2 * marker_size/100.0;
00143 
00144   if(type==MAIN_MARKER)
00145     rvizMarker->ns = "main_shapes";
00146   else
00147     rvizMarker->ns = "basic_shapes";
00148 
00149 
00150   rvizMarker->type = visualization_msgs::Marker::CUBE;
00151   rvizMarker->action = visualization_msgs::Marker::ADD;
00152 
00153   //Determine a color and opacity, based on marker type
00154   if(type==MAIN_MARKER){
00155     rvizMarker->color.r = 1.0f;
00156     rvizMarker->color.g = 0.0f;
00157     rvizMarker->color.b = 0.0f;
00158     rvizMarker->color.a = 1.0;
00159   }
00160   else if(type==VISIBLE_MARKER){
00161     rvizMarker->color.r = 0.0f;
00162     rvizMarker->color.g = 1.0f;
00163     rvizMarker->color.b = 0.0f;
00164     rvizMarker->color.a = 0.7;
00165   }
00166   else if(type==GHOST_MARKER){
00167     rvizMarker->color.r = 0.0f;
00168     rvizMarker->color.g = 0.0f;
00169     rvizMarker->color.b = 1.0f;
00170     rvizMarker->color.a = 0.5;
00171   }
00172 
00173   rvizMarker->lifetime = ros::Duration (1.0);
00174 
00175   // Only publish the pose of the master tag in each bundle, since that's all we really care about aside from visualization 
00176   if(type==MAIN_MARKER){
00177     //Take the pose of the tag in the camera frame and convert to the output frame (usually torso_lift_link for the PR2)
00178     tf::Transform tagPoseOutput = CamToOutput * markerPose;
00179 
00180     //Create the pose marker message
00181     tf::poseTFToMsg (tagPoseOutput, ar_pose_marker->pose.pose);
00182     ar_pose_marker->header.frame_id = output_frame;
00183     ar_pose_marker->header.stamp = image_msg->header.stamp;
00184     ar_pose_marker->id = id;
00185   }
00186   else
00187     ar_pose_marker = NULL;
00188 }
00189 
00190 
00191 //Callback to handle getting video frames and processing them
00192 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
00193 {
00194   if(init){
00195     CvSize sz_ = cvSize (cam->x_res, cam->y_res);
00196     capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
00197     init = false;       
00198   }
00199 
00200   //If we've already gotten the cam info, then go ahead
00201   if(cam->getCamInfo_){
00202     try{
00203       //Get the transformation from the Camera to the output frame for this image capture
00204       tf::StampedTransform CamToOutput;
00205       try{
00206         tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, ros::Duration(1.0));
00207         tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, CamToOutput);
00208       }
00209       catch (tf::TransformException ex){
00210         ROS_ERROR("%s",ex.what());
00211       }
00212 
00213       visualization_msgs::Marker rvizMarker;
00214       ar_track_alvar::AlvarMarker ar_pose_marker;
00215       arPoseMarkers_.markers.clear ();
00216 
00217       //Convert the image
00218       capture_ = bridge_.imgMsgToCv (image_msg, "rgb8");
00219 
00220       //Get the estimated pose of the main markers by using all the markers in each bundle
00221       GetMultiMarkerPoses(capture_);
00222                 
00223       //Draw the observed markers that are visible and note which bundles have at least 1 marker seen
00224       for(int i=0; i<n_bundles; i++)
00225         bundles_seen[i] = false;
00226 
00227       for (size_t i=0; i<marker_detector.markers->size(); i++)
00228         {
00229           int id = (*(marker_detector.markers))[i].GetId();
00230 
00231           // Draw if id is valid
00232           if(id >= 0){
00233 
00234             //Mark the bundle that marker belongs to as "seen"
00235             for(int j=0; j<n_bundles; j++){
00236               for(int k=0; k<bundle_indices[j].size(); k++){
00237                 if(bundle_indices[j][k] == id){
00238                   bundles_seen[j] = true;
00239                   break;
00240                 }
00241               }
00242             }
00243 
00244             // Don't draw if it is a master tag...we do this later, a bit differently
00245             bool should_draw = true;
00246             for(int i=0; i<n_bundles; i++){
00247               if(id == master_id[i]) should_draw = false;
00248             }
00249             if(should_draw){
00250               Pose p = (*(marker_detector.markers))[i].pose;
00251               makeMarkerMsgs(VISIBLE_MARKER, id, p, image_msg, CamToOutput, &rvizMarker, &ar_pose_marker);
00252               rvizMarkerPub_.publish (rvizMarker);
00253             }
00254           }
00255         }
00256                         
00257       //Draw the main markers, whether they are visible or not -- but only if at least 1 marker from their bundle is currently seen
00258       for(int i=0; i<n_bundles; i++)
00259         {
00260           if(bundles_seen[i] == true){
00261             makeMarkerMsgs(MAIN_MARKER, master_id[i], bundlePoses[i], image_msg, CamToOutput, &rvizMarker, &ar_pose_marker);
00262             rvizMarkerPub_.publish (rvizMarker);
00263             arPoseMarkers_.markers.push_back (ar_pose_marker);
00264           }
00265         }
00266 
00267       //Publish the marker messages
00268       arMarkerPub_.publish (arPoseMarkers_);
00269     }
00270     catch (sensor_msgs::CvBridgeException & e){
00271       ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
00272     }
00273   }
00274 }
00275 
00276 
00277 
00278 int main(int argc, char *argv[])
00279 {
00280   ros::init (argc, argv, "marker_detect");
00281   ros::NodeHandle n;
00282 
00283   if(argc < 8){
00284     std::cout << std::endl;
00285     cout << "Not enough arguments provided." << endl;
00286     cout << "Usage: ./findMarkerBundles <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame> <list of bundle XML files...>" << endl;
00287     std::cout << std::endl;
00288     return 0;
00289   }
00290 
00291   // Get params from command line
00292   marker_size = atof(argv[1]);
00293   max_new_marker_error = atof(argv[2]);
00294   max_track_error = atof(argv[3]);
00295   cam_image_topic = argv[4];
00296   cam_info_topic = argv[5];
00297   output_frame = argv[6];
00298   int n_args_before_list = 7;
00299   n_bundles = argc - n_args_before_list;
00300 
00301   marker_detector.SetMarkerSize(marker_size);
00302   multi_marker_bundles = new MultiMarkerBundle*[n_bundles];     
00303   bundlePoses = new Pose[n_bundles];
00304   master_id = new int[n_bundles]; 
00305   bundle_indices = new std::vector<int>[n_bundles]; 
00306   bundles_seen = new bool[n_bundles];   
00307 
00308   // Load the marker bundle XML files
00309   for(int i=0; i<n_bundles; i++){       
00310     bundlePoses[i].Reset();             
00311     MultiMarker loadHelper;
00312     if(loadHelper.Load(argv[i + n_args_before_list], FILE_FORMAT_XML)){
00313       vector<int> id_vector = loadHelper.getIndices();
00314       multi_marker_bundles[i] = new MultiMarkerBundle(id_vector);       
00315       multi_marker_bundles[i]->Load(argv[i + n_args_before_list], FILE_FORMAT_XML);
00316       master_id[i] = multi_marker_bundles[i]->getMasterId();
00317       bundle_indices[i] = multi_marker_bundles[i]->getIndices();
00318     }
00319     else{
00320       cout<<"Cannot load file "<< argv[i + n_args_before_list] << endl; 
00321       return 0;
00322     }           
00323   }  
00324 
00325   // Set up camera, listeners, and broadcasters
00326   cam = new Camera(n, cam_info_topic);
00327   tf_listener = new tf::TransformListener(n);
00328   tf_broadcaster = new tf::TransformBroadcaster();
00329   arMarkerPub_ = n.advertise < ar_track_alvar::AlvarMarkers > ("ar_pose_marker", 0);
00330   rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00331         
00332   //Give tf a chance to catch up before the camera callback starts asking for transforms
00333   ros::Duration(1.0).sleep();
00334   ros::spinOnce();                      
00335          
00336   //Subscribe to topics and set up callbacks
00337   ROS_INFO ("Subscribing to image topic");
00338   image_transport::ImageTransport it_(n);
00339   cam_sub_ = it_.subscribe (cam_image_topic, 1, &getCapCallback);
00340 
00341   ros::spin();
00342 
00343   return 0;
00344 }


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