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


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