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


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