TrainMarkerBundle.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 MultiMarkerInitializer *multi_marker_init=NULL;
00066 MultiMarkerBundle *multi_marker_bundle=NULL;
00067 int auto_count;
00068 bool auto_collect;
00069 
00070 bool init=true;
00071 bool add_measurement=false;
00072 bool optimize = false;
00073 bool optimize_done = false;
00074 
00075 double marker_size;
00076 double max_new_marker_error;
00077 double max_track_error;
00078 std::string cam_image_topic; 
00079 std::string cam_info_topic; 
00080 std::string output_frame;
00081 int nof_markers;  
00082 
00083 double GetMultiMarkerPose(IplImage *image, Pose &pose);
00084 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg);
00085 int keyCallback(int key);
00086 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);
00087 
00088 
00089 double GetMultiMarkerPose(IplImage *image, Pose &pose) {
00090     static bool init=true;
00091 
00092     if (init) {
00093         init=false;
00094         vector<int> id_vector;
00095         for(int i = 0; i < nof_markers; ++i)
00096             id_vector.push_back(i);
00097         // We make the initialization for MultiMarkerBundle using MultiMarkerInitializer
00098         // Each marker needs to be visible in at least two images and at most 64 image are used.
00099         multi_marker_init = new MultiMarkerInitializer(id_vector, 2, 64);
00100         pose.Reset();
00101         multi_marker_init->PointCloudAdd(id_vector[0], marker_size, pose);
00102         multi_marker_bundle = new MultiMarkerBundle(id_vector);
00103                 marker_detector.SetMarkerSize(marker_size); 
00104     }
00105 
00106     double error = -1;
00107     if (!optimize_done) {
00108         if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) {
00109                 error = multi_marker_init->Update(marker_detector.markers, cam, pose);
00110         }
00111     } 
00112         else {
00113         if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) {
00114             error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
00115             if ((multi_marker_bundle->SetTrackMarkers(marker_detector, cam, pose, image) > 0) && (marker_detector.DetectAdditional(image, cam, false) > 0))
00116             {    
00117                 error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
00118             }
00119         }
00120     }
00121 
00122    if (add_measurement) {
00123                 cout << "Markers seen: " << marker_detector.markers->size() << "\n";
00124         if (marker_detector.markers->size() >= 2) {
00125             cout<<"Adding measurement..."<<endl;
00126             multi_marker_init->MeasurementsAdd(marker_detector.markers);
00127         }
00128                 else{
00129                         cout << "Not enough markers to capture measurement\n";
00130         }
00131                 add_measurement=false;
00132         }
00133 
00134     if (optimize) {
00135         cout<<"Initializing..."<<endl;
00136         if (!multi_marker_init->Initialize(cam)) {
00137             cout<<"Initialization failed, add some more measurements."<<endl;
00138 
00139         } else {
00140             // Reset the bundle adjuster.
00141             multi_marker_bundle->Reset();
00142             multi_marker_bundle->MeasurementsReset();
00143             // Copy all measurements into the bundle adjuster.
00144             for (int i = 0; i < multi_marker_init->getMeasurementCount(); ++i) {
00145                 Pose p2;
00146                 multi_marker_init->getMeasurementPose(i, cam, p2);
00147                 const std::vector<MultiMarkerInitializer::MarkerMeasurement> markers = multi_marker_init->getMeasurementMarkers(i);
00148                 multi_marker_bundle->MeasurementsAdd(&markers, p2);
00149             }
00150             // Initialize the bundle adjuster with initial marker poses.
00151             multi_marker_bundle->PointCloudCopy(multi_marker_init);
00152             cout<<"Optimizing..."<<endl;
00153             if (multi_marker_bundle->Optimize(cam, 0.01, 20)) {
00154                 cout<<"Optimizing done"<<endl;
00155                 optimize_done=true;
00156 
00157             } else {
00158                 cout<<"Optimizing FAILED!"<<endl;
00159             }
00160         }
00161         optimize=false;
00162     }
00163     return error;
00164 }
00165 
00166 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){
00167         double px,py,pz,qx,qy,qz,qw;
00168         
00169         px = p.translation[0]/100.0;
00170         py = p.translation[1]/100.0;
00171         pz = p.translation[2]/100.0;
00172         qx = p.quaternion[1];
00173         qy = p.quaternion[2];
00174         qz = p.quaternion[3];
00175         qw = p.quaternion[0];
00176 
00177         //Get the marker pose in the camera frame
00178         btQuaternion rotation (qx,qy,qz,qw);
00179         btVector3 origin (px,py,pz);
00180         btTransform t (rotation, origin);
00181 
00182         btVector3 markerOrigin (0, 0, 0);
00183         btTransform m (btQuaternion::getIdentity (), markerOrigin);
00184         btTransform markerPose = t * m;
00185 
00186         //Publish the transform from the camera to the marker           
00187         if(type==MAIN_MARKER){
00188                 std::string markerFrame = "ar_marker_";
00189                 std::stringstream out;
00190                 out << id;
00191                 std::string id_string = out.str();
00192                 markerFrame += id_string;
00193                 tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
00194         tf_broadcaster->sendTransform(camToMarker);
00195         }
00196 
00197         //Create the rviz visualization message
00198         tf::poseTFToMsg (markerPose, rvizMarker->pose);
00199         rvizMarker->header.frame_id = image_msg->header.frame_id;
00200         rvizMarker->header.stamp = image_msg->header.stamp;
00201         rvizMarker->id = id;
00202 
00203         rvizMarker->scale.x = 1.0 * marker_size/100.0;
00204         rvizMarker->scale.y = 1.0 * marker_size/100.0;
00205         rvizMarker->scale.z = 0.2 * marker_size/100.0;
00206         rvizMarker->ns = "basic_shapes";
00207         rvizMarker->type = visualization_msgs::Marker::CUBE;
00208         rvizMarker->action = visualization_msgs::Marker::ADD;
00209 
00210         //Determine a color and opacity, based on marker type
00211         if(type==MAIN_MARKER){
00212                 rvizMarker->color.r = 1.0f;
00213                 rvizMarker->color.g = 0.0f;
00214                 rvizMarker->color.b = 0.0f;
00215                 rvizMarker->color.a = 1.0;
00216         }
00217         else if(type==VISIBLE_MARKER){
00218                 rvizMarker->color.r = 0.0f;
00219                 rvizMarker->color.g = 1.0f;
00220                 rvizMarker->color.b = 0.0f;
00221                 rvizMarker->color.a = 1.0;
00222         }
00223         else if(type==GHOST_MARKER){
00224                 rvizMarker->color.r = 0.0f;
00225                 rvizMarker->color.g = 0.0f;
00226                 rvizMarker->color.b = 1.0f;
00227                 rvizMarker->color.a = 0.5;
00228         }
00229 
00230         rvizMarker->lifetime = ros::Duration (1.0);
00231 
00232         //Get the pose of the tag in the camera frame, then convert to the output frame (usually torso)
00233         tf::Transform tagPoseOutput = CamToOutput * markerPose;
00234 
00235         //Create the pose marker message
00236         tf::poseTFToMsg (tagPoseOutput, ar_pose_marker->pose.pose);
00237         ar_pose_marker->header.frame_id = output_frame;
00238         ar_pose_marker->header.stamp = image_msg->header.stamp;
00239         ar_pose_marker->id = id;
00240 }
00241 
00242 
00243 void getCapCallback (const sensor_msgs::ImageConstPtr & image_msg)
00244 {
00245         if(init){
00246                 CvSize sz_ = cvSize (cam->x_res, cam->y_res);
00247         capture_ = cvCreateImage (sz_, IPL_DEPTH_8U, 4);
00248                 init = false;   
00249         }
00250 
00251         //Check if automatic measurement collection should be triggered
00252         if(auto_collect){
00253                 auto_count++;
00254                 add_measurement = true;
00255                 if(auto_count >= 5)
00256                         auto_collect = false;
00257         }
00258 
00259         //If we've already gotten the cam info, then go ahead
00260         if(cam->getCamInfo_){
00261                 try{
00262                         //Get the transformation from the Camera to the output frame for this image capture
00263                         tf::StampedTransform CamToOutput;
00264                 try{
00265                                 tf_listener->waitForTransform(image_msg->header.frame_id, output_frame, image_msg->header.stamp, ros::Duration(1.0));
00266                                 tf_listener->lookupTransform(image_msg->header.frame_id, output_frame, image_msg->header.stamp, CamToOutput);                           
00267                         }
00268                 catch (tf::TransformException ex){
00269                         ROS_ERROR("%s",ex.what());
00270                 }
00271 
00272                 visualization_msgs::Marker rvizMarker;
00273                 ar_track_alvar::AlvarMarker ar_pose_marker;
00274                 arPoseMarkers_.markers.clear ();
00275 
00276                 //Convert the image
00277                 capture_ = bridge_.imgMsgToCv (image_msg, "rgb8");
00278 
00279                 //Get the estimated pose of the main marker using the whole bundle
00280                 static Pose bundlePose;
00281                 double error = GetMultiMarkerPose(capture_, bundlePose);
00282 
00283                         if (optimize_done){
00284                         //Draw the main marker
00285                         makeMarkerMsgs(MAIN_MARKER, 0, bundlePose, image_msg, CamToOutput, &rvizMarker, &ar_pose_marker);
00286                         rvizMarkerPub_.publish (rvizMarker);
00287                         arPoseMarkers_.markers.push_back (ar_pose_marker);
00288                         }
00289                 //Now grab the poses of the other markers that are visible
00290                         for (size_t i=0; i<marker_detector.markers->size(); i++)
00291                         {
00292                         int id = (*(marker_detector.markers))[i].GetId();
00293 
00294                         //Don't do the main marker (id=0) if we've already drawn it
00295                         if(id > 0 || ((!optimize_done) && id==0)){
00296                                 Pose p = (*(marker_detector.markers))[i].pose;
00297                                 makeMarkerMsgs(VISIBLE_MARKER, id, p, image_msg, CamToOutput, &rvizMarker, &ar_pose_marker);
00298                                 rvizMarkerPub_.publish (rvizMarker);
00299                                 arPoseMarkers_.markers.push_back (ar_pose_marker);
00300                         }
00301                         }
00302                         arMarkerPub_.publish (arPoseMarkers_);
00303                 }
00304         catch (sensor_msgs::CvBridgeException & e){
00305                 ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
00306         }
00307         }
00308 
00309         //Sleep if we are auto collecting
00310         if(auto_collect)
00311                 usleep(1000000);
00312 }
00313 
00314 
00315 //Do something based on keystrokes from menu
00316 int keyProcess(int key)
00317 {
00318     if(key == 'r')
00319     {
00320         cout<<"Reseting multi marker"<<endl;
00321             multi_marker_init->Reset();
00322             multi_marker_init->MeasurementsReset();
00323             multi_marker_bundle->Reset();
00324             multi_marker_bundle->MeasurementsReset();
00325             add_measurement = false;
00326             optimize        = false;
00327             optimize_done   = false;
00328     }
00329     else if(key == 'l')
00330     {
00331         if(multi_marker_bundle->Load("mmarker.xml", FILE_FORMAT_XML))
00332         {
00333             cout<<"Multi marker loaded"<<endl;
00334             multi_marker_init->PointCloudCopy(multi_marker_bundle);
00335             optimize_done = true;
00336         }
00337         else
00338             cout<<"Cannot load multi marker"<<endl;
00339     }
00340     else if(key == 's')
00341     {
00342         if(multi_marker_bundle->Save("mmarker.xml", FILE_FORMAT_XML))
00343             cout<<"Multi marker saved"<<endl;
00344         else
00345             cout<<"Cannot save multi marker"<<endl;
00346     }
00347     else if(key == 'p')
00348     {
00349         add_measurement=true;
00350     }
00351         else if(key == 'a')
00352     {
00353         auto_count = 0;
00354                 auto_collect = true;
00355     }
00356     else if(key == 'o')
00357     {
00358         optimize=true;
00359     }
00360         else if(key == 'q')
00361     {
00362         exit(0);
00363     }
00364     else return key;
00365 
00366     return 0;
00367 }
00368 
00369 
00370 int main(int argc, char *argv[])
00371 {
00372         ros::init (argc, argv, "marker_detect");
00373         ros::NodeHandle n;
00374         
00375         if(argc < 8){
00376                 std::cout << std::endl;
00377                 cout << "Not enough arguments provided." << endl;
00378                 cout << "Usage: ./trainMarkerBundle <num of markers> <marker size in cm> <max new marker error> <max track error> <cam image topic> <cam info topic> <output frame>" << endl;
00379                 std::cout << std::endl;
00380                 return 0;
00381         }
00382 
00383         // Get params from command line
00384         nof_markers = atoi(argv[1]);
00385         marker_size = atof(argv[2]);
00386         max_new_marker_error = atof(argv[3]);
00387         max_track_error = atof(argv[4]);
00388         cam_image_topic = argv[5];
00389         cam_info_topic = argv[6];
00390     output_frame = argv[7];
00391         marker_detector.SetMarkerSize(marker_size);
00392 
00393         cam = new Camera(n, cam_info_topic);
00394         tf_listener = new tf::TransformListener(n);
00395         tf_broadcaster = new tf::TransformBroadcaster();
00396         arMarkerPub_ = n.advertise < ar_track_alvar::AlvarMarkers > ("ar_pose_marker", 0);
00397         rvizMarkerPub_ = n.advertise < visualization_msgs::Marker > ("visualization_marker", 0);
00398         
00399         //Give tf a chance to catch up before the camera callback starts asking for transforms
00400         ros::Duration(1.0).sleep();
00401         ros::spinOnce();
00402 
00403         //Subscribe to camera message
00404         ROS_INFO ("Subscribing to image topic");
00405         image_transport::ImageTransport it_(n);
00406         cam_sub_ = it_.subscribe (cam_image_topic, 1, &getCapCallback);
00407 
00408     // Output usage message
00409     std::cout << std::endl;
00410     std::cout << "Keyboard Shortcuts:" << std::endl;
00411     std::cout << "  l: load marker configuration from mmarker.txt" << std::endl;
00412     std::cout << "  s: save marker configuration to mmarker.txt" << std::endl;
00413     std::cout << "  r: reset marker configuration" << std::endl;
00414     std::cout << "  p: add measurement" << std::endl;
00415         std::cout << "  a: auto add measurements (captures once a second for 5 seconds)" << std::endl;
00416     std::cout << "  o: optimize bundle" << std::endl;
00417     std::cout << "  q: quit" << std::endl;
00418     std::cout << std::endl;
00419     std::cout << "Please type commands with the openCV window selected" << std::endl;
00420         std::cout << std::endl;
00421 
00422         cvNamedWindow("Command input window", CV_WINDOW_AUTOSIZE); 
00423 
00424         while(1){
00425                 int key = cvWaitKey(20);
00426                 if(key >= 0)
00427                         keyProcess(key);
00428                 ros::spinOnce();
00429         }
00430 
00431     return 0;
00432 }


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