tracker.cpp
Go to the documentation of this file.
00001 #include <stdexcept>
00002 
00003 #include <boost/filesystem/fstream.hpp>
00004 #include <boost/format.hpp>
00005 #include <boost/scope_exit.hpp>
00006 #include <boost/version.hpp>
00007 
00008 #include <dynamic_reconfigure/server.h>
00009 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00010 #include <image_proc/advertisement_checker.h>
00011 #include <image_transport/image_transport.h>
00012 #include <ros/param.h>
00013 #include <ros/ros.h>
00014 #include <ros/transport_hints.h>
00015 #include <sensor_msgs/Image.h>
00016 #include <std_msgs/String.h>
00017 #include <tf/transform_broadcaster.h>
00018 
00019 #include <boost/bind.hpp>
00020 #include <visp/vpExponentialMap.h>
00021 #include <visp/vpImage.h>
00022 #include <visp/vpImageConvert.h>
00023 #include <visp/vpCameraParameters.h>
00024 
00025 #include "tracker.hh"
00026 
00027 #include "conversion.hh"
00028 #include "callbacks.hh"
00029 #include "file.hh"
00030 #include "names.hh"
00031 
00032 // TODO:
00033 // - add a topic allowing to suggest an estimation of the cMo
00034 // - handle automatic reset when tracking is lost.
00035 
00036 namespace visp_tracker
00037 {
00038   bool
00039   Tracker::initCallback(visp_tracker::Init::Request& req,
00040                         visp_tracker::Init::Response& res)
00041   {
00042     ROS_INFO("Initialization request received.");
00043 
00044     res.initialization_succeed = false;
00045 
00046     // If something goes wrong, rollback all changes.
00047     BOOST_SCOPE_EXIT((&res)(tracker_)(&state_)
00048                      (&lastTrackedImage_)(&trackerType_))
00049     {
00050       if(!res.initialization_succeed)
00051       {
00052         tracker_->resetTracker();
00053         state_ = WAITING_FOR_INITIALIZATION;
00054         lastTrackedImage_ = 0;
00055 
00056       }
00057     } BOOST_SCOPE_EXIT_END;
00058 
00059     std::string fullModelPath;
00060     boost::filesystem::ofstream modelStream;
00061 
00062     // Load model from parameter.
00063     if (!makeModelFile(modelStream, fullModelPath))
00064       return true;
00065 
00066     tracker_->resetTracker();
00067 
00068     // Common parameters
00069     convertInitRequestToVpMbTracker(req, tracker_);
00070 
00071     if(trackerType_!="klt"){ // for mbt and hybrid
00072       convertInitRequestToVpMe(req, tracker_, movingEdge_);
00073     }
00074 
00075     if(trackerType_!="mbt"){ // for klt and hybrid
00076       convertInitRequestToVpKltOpencv(req, tracker_, kltTracker_);
00077     }
00078 
00079     if(trackerType_=="mbt+klt"){ // Hybrid Tracker reconfigure
00080       visp_tracker::ModelBasedSettingsConfig config;
00081       convertVpMbTrackerToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsConfig>(tracker_, config);
00082       reconfigureSrv_->updateConfig(config);
00083       convertVpMeToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsConfig>(movingEdge_, tracker_, config);
00084       reconfigureSrv_->updateConfig(config);
00085       convertVpKltOpencvToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsConfig>(kltTracker_, tracker_, config);
00086       reconfigureSrv_->updateConfig(config);
00087     }
00088     else if(trackerType_=="mbt"){ // Edge Tracker reconfigure
00089       visp_tracker::ModelBasedSettingsEdgeConfig config;
00090       convertVpMbTrackerToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsEdgeConfig>(tracker_, config);
00091       reconfigureEdgeSrv_->updateConfig(config);
00092       convertVpMeToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsEdgeConfig>(movingEdge_, tracker_, config);
00093       reconfigureEdgeSrv_->updateConfig(config);
00094     }
00095     else{ // KLT Tracker reconfigure
00096       visp_tracker::ModelBasedSettingsKltConfig config;
00097       convertVpMbTrackerToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsKltConfig>(tracker_, config);
00098       reconfigureKltSrv_->updateConfig(config);
00099       convertVpKltOpencvToModelBasedSettingsConfig<visp_tracker::ModelBasedSettingsKltConfig>(kltTracker_, tracker_, config);
00100       reconfigureKltSrv_->updateConfig(config);
00101     }
00102 
00103     state_ = WAITING_FOR_INITIALIZATION;
00104     lastTrackedImage_ = 0;
00105 
00106     // Load the model.
00107     try
00108     {
00109       ROS_DEBUG_STREAM("Trying to load the model Tracker: " << fullModelPath);
00110       tracker_->loadModel(fullModelPath.c_str());
00111       modelStream.close();
00112     }
00113     catch(...)
00114     {
00115       ROS_ERROR_STREAM("Failed to load the model: " << fullModelPath);
00116       return true;
00117     }
00118     ROS_DEBUG("Model has been successfully loaded.");
00119 
00120     // Load the initial cMo.
00121     transformToVpHomogeneousMatrix(cMo_, req.initial_cMo);
00122 
00123     // Enable covariance matrix.
00124     tracker_->setCovarianceComputation(true);
00125 
00126     // Try to initialize the tracker.
00127     ROS_INFO_STREAM("Initializing tracker with cMo:\n" << cMo_);
00128     try
00129     {
00130       // Bug between setPose() and initFromPose() not present here due to previous call to resetTracker()
00131       tracker_->initFromPose(image_, cMo_);
00132       ROS_INFO("Tracker successfully initialized.");
00133 
00134       //movingEdge.print();
00135       ROS_INFO_STREAM(convertVpMbTrackerToRosMessage(tracker_));
00136       // - Moving edges.
00137       if(trackerType_!="klt")
00138         ROS_INFO_STREAM(convertVpMeToRosMessage(tracker_, movingEdge_));
00139 
00140       if(trackerType_!="mbt")
00141         ROS_INFO_STREAM(convertVpKltOpencvToRosMessage(tracker_,kltTracker_));
00142     }
00143     catch(const std::string& str)
00144     {
00145       ROS_ERROR_STREAM("Tracker initialization has failed: " << str);
00146     }
00147 
00148     // Initialization is valid.
00149     res.initialization_succeed = true;
00150     state_ = TRACKING;
00151     return true;
00152   }
00153 
00154   void
00155   Tracker::updateMovingEdgeSites(visp_tracker::MovingEdgeSitesPtr sites)
00156   {
00157     if (!sites)
00158       return;
00159 
00160     std::list<vpMbtDistanceLine*> linesList;
00161 
00162     if(trackerType_!="klt") { // For mbt and hybrid
00163       dynamic_cast<vpMbEdgeTracker*>(tracker_)->getLline(linesList, 0);
00164 
00165       std::list<vpMbtDistanceLine*>::iterator linesIterator = linesList.begin();
00166 
00167       bool noVisibleLine = true;
00168       for (; linesIterator != linesList.end(); ++linesIterator)
00169       {
00170         vpMbtDistanceLine* line = *linesIterator;
00171 
00172 #if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0
00173         if (line && line->isVisible() && ! line->meline.empty())
00174 #else
00175         if (line && line->isVisible() && line->meline)
00176 #endif
00177         {
00178 #if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0
00179           for(unsigned int a = 0 ; a < line->meline.size() ; a++)
00180           {
00181             if(line->meline[a] != NULL) {
00182               std::list<vpMeSite>::const_iterator sitesIterator = line->meline[a]->getMeList().begin();
00183               if (line->meline[a]->getMeList().empty())
00184                 ROS_DEBUG_THROTTLE(10, "no moving edge for a line");
00185               for (; sitesIterator != line->meline[a]->getMeList().end(); ++sitesIterator)
00186               {
00187 #elif VISP_VERSION_INT >= VP_VERSION_INT(2,10,0) // ViSP >= 2.10.0
00188           std::list<vpMeSite>::const_iterator sitesIterator = line->meline->getMeList().begin();
00189           if (line->meline->getMeList().empty())
00190             ROS_DEBUG_THROTTLE(10, "no moving edge for a line");
00191           for (; sitesIterator != line->meline->getMeList().end(); ++sitesIterator)
00192           {
00193 #else
00194           std::list<vpMeSite>::const_iterator sitesIterator = line->meline->list.begin();
00195           if (line->meline->list.empty())
00196             ROS_DEBUG_THROTTLE(10, "no moving edge for a line");
00197           for (; sitesIterator != line->meline->list.end(); ++sitesIterator)
00198           {
00199 #endif
00200             visp_tracker::MovingEdgeSite movingEdgeSite;
00201             movingEdgeSite.x = sitesIterator->ifloat;
00202             movingEdgeSite.y = sitesIterator->jfloat;
00203 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) // ViSP < 2.10.0
00204             movingEdgeSite.suppress = sitesIterator->suppress;
00205 #endif
00206             sites->moving_edge_sites.push_back (movingEdgeSite);
00207           }
00208           noVisibleLine = false;
00209         }
00210 #if VISP_VERSION_INT >= VP_VERSION_INT(3,0,0) // ViSP >= 3.0.0
00211       }
00212     }
00213 #endif
00214       }
00215       if (noVisibleLine)
00216         ROS_DEBUG_THROTTLE(10, "no distance lines");
00217     }
00218   }
00219 
00220   void
00221   Tracker::updateKltPoints(visp_tracker::KltPointsPtr klt)
00222   {
00223     if (!klt)
00224       return;
00225 
00226 #if VISP_VERSION_INT < VP_VERSION_INT(2,10,0) // ViSP < 2.10.0
00227     vpMbHiddenFaces<vpMbtKltPolygon> *poly_lst;
00228     std::map<int, vpImagePoint> *map_klt;
00229 
00230     if(trackerType_!="mbt") { // For klt and hybrid
00231       poly_lst = &dynamic_cast<vpMbKltTracker*>(tracker_)->getFaces();
00232 
00233       for(unsigned int i = 0 ; i < poly_lst->size() ; i++)
00234       {
00235         if((*poly_lst)[i])
00236         {
00237           map_klt = &((*poly_lst)[i]->getCurrentPoints());
00238 
00239           if(map_klt->size() > 3)
00240           {
00241             for (std::map<int, vpImagePoint>::iterator it=map_klt->begin(); it!=map_klt->end(); ++it)
00242             {
00243               visp_tracker::KltPoint kltPoint;
00244               kltPoint.id = it->first;
00245               kltPoint.i = it->second.get_i();
00246               kltPoint.j = it->second.get_j();
00247               klt->klt_points_positions.push_back (kltPoint);
00248             }
00249           }
00250         }
00251       }
00252     }
00253 #else // ViSP >= 2.10.0
00254     std::list<vpMbtDistanceKltPoints*> *poly_lst;
00255     std::map<int, vpImagePoint> *map_klt;
00256 
00257     if(trackerType_!="mbt") { // For klt and hybrid
00258       poly_lst = &dynamic_cast<vpMbKltTracker*>(tracker_)->getFeaturesKlt();
00259 
00260       for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=poly_lst->begin(); it!=poly_lst->end(); ++it){
00261         map_klt = &((*it)->getCurrentPoints());
00262 
00263         if((*it)->polygon->isVisible()){
00264           if(map_klt->size() > 3)
00265           {
00266             for (std::map<int, vpImagePoint>::iterator it=map_klt->begin(); it!=map_klt->end(); ++it)
00267             {
00268               visp_tracker::KltPoint kltPoint;
00269               kltPoint.id = it->first;
00270               kltPoint.i = it->second.get_i();
00271               kltPoint.j = it->second.get_j();
00272               klt->klt_points_positions.push_back (kltPoint);
00273             }
00274           }
00275         }
00276       }
00277     }
00278 #endif
00279   }
00280 
00281   void Tracker::checkInputs()
00282   {
00283     ros::V_string topics;
00284     topics.push_back(rectifiedImageTopic_);
00285     checkInputs_.start(topics, 60.0);
00286   }
00287 
00288   Tracker::Tracker(ros::NodeHandle& nh,
00289                    ros::NodeHandle& privateNh,
00290                    volatile bool& exiting,
00291                    unsigned queueSize)
00292     : exiting_ (exiting),
00293       queueSize_(queueSize),
00294       nodeHandle_(nh),
00295       nodeHandlePrivate_(privateNh),
00296       imageTransport_(nodeHandle_),
00297       state_(WAITING_FOR_INITIALIZATION),
00298       image_(),
00299       cameraPrefix_(),
00300       rectifiedImageTopic_(),
00301       cameraInfoTopic_(),
00302       modelPath_(),
00303       cameraSubscriber_(),
00304       mutex_ (),
00305       //reconfigureSrv_(mutex_, nodeHandlePrivate_),
00306       reconfigureSrv_(NULL),
00307       reconfigureKltSrv_(NULL),
00308       reconfigureEdgeSrv_(NULL),
00309       resultPublisher_(),
00310       transformationPublisher_(),
00311       movingEdgeSitesPublisher_(),
00312       kltPointsPublisher_(),
00313       initService_(),
00314       header_(),
00315       info_(),
00316       kltTracker_(),
00317       movingEdge_(),
00318       cameraParameters_(),
00319       lastTrackedImage_(),
00320       checkInputs_(nodeHandle_, ros::this_node::getName()),
00321       cMo_ (),
00322       listener_ (),
00323       worldFrameId_ (),
00324       compensateRobotMotion_ (false),
00325       transformBroadcaster_ (),
00326       childFrameId_ (),
00327       objectPositionHintSubscriber_ (),
00328       objectPositionHint_ ()
00329   {
00330     // Set cMo to identity.
00331     cMo_.eye();
00332     //tracker_ = new vpMbEdgeTracker();
00333 
00334     // Parameters.
00335     nodeHandlePrivate_.param<std::string>("camera_prefix", cameraPrefix_, "");
00336     nodeHandlePrivate_.param<std::string>("tracker_type", trackerType_, "mbt");
00337     if(trackerType_=="mbt")
00338       tracker_ = new vpMbEdgeTracker();
00339     else if(trackerType_=="klt")
00340       tracker_ = new vpMbKltTracker();
00341     else
00342       tracker_ = new vpMbEdgeKltTracker();
00343 
00344     if (cameraPrefix_.empty ())
00345       {
00346         ROS_FATAL
00347           ("The camera_prefix parameter not set.\n"
00348            "Please relaunch the tracker while setting this parameter, i.e.\n"
00349            "$ rosrun visp_tracker tracker _camera_prefix:=/my/camera");
00350         ros::shutdown ();
00351         return;
00352       }
00353     // Create global /camera_prefix param to avoid to remap in the launch files the tracker_client and tracker_viewer nodes
00354     nodeHandle_.setParam("camera_prefix", cameraPrefix_);
00355 
00356     nodeHandle_.param<std::string>("frame_id", childFrameId_, "object_position");
00357 
00358     // Robot motion compensation.
00359     nodeHandle_.param<std::string>("world_frame_id", worldFrameId_, "/odom");
00360     nodeHandle_.param<bool>
00361       ("compensate_robot_motion", compensateRobotMotion_, false);
00362 
00363     // Compute topic and services names.
00364     rectifiedImageTopic_ =
00365       ros::names::resolve(cameraPrefix_ + "/image_rect");
00366 
00367     // Check for subscribed topics.
00368     checkInputs();
00369 
00370     // Result publisher.
00371     resultPublisher_ =
00372       nodeHandle_.advertise<geometry_msgs::PoseWithCovarianceStamped>
00373       (visp_tracker::object_position_covariance_topic, queueSize_);
00374 
00375     transformationPublisher_ =
00376       nodeHandle_.advertise<geometry_msgs::TransformStamped>
00377       (visp_tracker::object_position_topic, queueSize_);
00378 
00379     // Moving edge sites_ publisher.
00380     movingEdgeSitesPublisher_ =
00381       nodeHandle_.advertise<visp_tracker::MovingEdgeSites>
00382       (visp_tracker::moving_edge_sites_topic, queueSize_);
00383 
00384     // Klt_points_ publisher.
00385     kltPointsPublisher_ =
00386       nodeHandle_.advertise<visp_tracker::KltPoints>
00387       (visp_tracker::klt_points_topic, queueSize_);
00388 
00389     // Camera subscriber.
00390     cameraSubscriber_ =
00391       imageTransport_.subscribeCamera
00392       (rectifiedImageTopic_, queueSize_,
00393        bindImageCallback(image_, header_, info_));
00394 
00395     // Object position hint subscriber.
00396     typedef boost::function<
00397     void (const geometry_msgs::TransformStampedConstPtr&)>
00398       objectPositionHintCallback_t;
00399     objectPositionHintCallback_t callback =
00400       boost::bind (&Tracker::objectPositionHintCallback, this, _1);
00401     objectPositionHintSubscriber_ =
00402       nodeHandle_.subscribe<geometry_msgs::TransformStamped>
00403       ("object_position_hint", queueSize_, callback);
00404 
00405     // Initialization.
00406     // No more necessary as it is done via the reconfigure server
00407 //    movingEdge_.initMask();
00408 //    if(trackerType_!="klt"){
00409 //      vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker_);
00410 //      t->setMovingEdge(movingEdge_);
00411 //    }
00412     
00413 //    if(trackerType_!="mbt"){
00414 //      vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker_);
00415 //      t->setKltOpencv(kltTracker_);
00416 //    }
00417     
00418     // Dynamic reconfigure.
00419     if(trackerType_=="mbt+klt"){ // Hybrid Tracker reconfigure
00420       reconfigureSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
00421       reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t::CallbackType f =
00422         boost::bind(&reconfigureCallbackAndInitViewer,
00423                     boost::ref(nodeHandle_), boost::ref(tracker_),
00424                     boost::ref(image_), boost::ref(movingEdge_), boost::ref(kltTracker_),
00425                     boost::ref(mutex_), _1, _2);
00426       reconfigureSrv_->setCallback(f);
00427     }
00428     else if(trackerType_=="mbt"){ // Edge Tracker reconfigure
00429       reconfigureEdgeSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
00430       reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t::CallbackType f =
00431         boost::bind(&reconfigureEdgeCallbackAndInitViewer,
00432                     boost::ref(nodeHandle_), boost::ref(tracker_),
00433                     boost::ref(image_), boost::ref(movingEdge_),
00434                     boost::ref(mutex_), _1, _2);
00435       reconfigureEdgeSrv_->setCallback(f);
00436     }
00437     else{ // KLT Tracker reconfigure
00438       reconfigureKltSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
00439       reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t::CallbackType f =
00440         boost::bind(&reconfigureKltCallbackAndInitViewer,
00441                     boost::ref(nodeHandle_), boost::ref(tracker_),
00442                     boost::ref(image_), boost::ref(kltTracker_),
00443                     boost::ref(mutex_), _1, _2);
00444       reconfigureKltSrv_->setCallback(f);
00445     }
00446     
00447     // Wait for the image to be initialized.
00448     waitForImage();
00449     if (this->exiting())
00450       return;
00451     if (!image_.getWidth() || !image_.getHeight())
00452       throw std::runtime_error("failed to retrieve image");
00453 
00454     // Tracker initialization.
00455     initializeVpCameraFromCameraInfo(cameraParameters_, info_);
00456 
00457     // Double check camera parameters.
00458     if (cameraParameters_.get_px () == 0.
00459         || cameraParameters_.get_px () == 1.
00460         || cameraParameters_.get_py () == 0.
00461         || cameraParameters_.get_py () == 1.
00462         || cameraParameters_.get_u0 () == 0.
00463         || cameraParameters_.get_u0 () == 1.
00464         || cameraParameters_.get_v0 () == 0.
00465         || cameraParameters_.get_v0 () == 1.)
00466       ROS_WARN ("Dubious camera parameters detected.\n"
00467                 "\n"
00468                 "It seems that the matrix P from your camera\n"
00469                 "calibration topics is wrong.\n"
00470                 "The tracker will continue anyway, but you\n"
00471                 "should double check your calibration data,\n"
00472                 "especially if the model re-projection fails.\n"
00473                 "\n"
00474                 "This warning is triggered is px, py, u0 or v0\n"
00475                 "is set to 0. or 1. (exactly).");
00476 
00477     tracker_->setCameraParameters(cameraParameters_);
00478     tracker_->setDisplayFeatures(false);
00479 
00480     ROS_INFO_STREAM(cameraParameters_);
00481 
00482     // Service declaration.
00483     initCallback_t initCallback =
00484       boost::bind(&Tracker::initCallback, this, _1, _2);
00485 
00486     initService_ = nodeHandle_.advertiseService
00487       (visp_tracker::init_service, initCallback);
00488   }
00489   
00490   Tracker::~Tracker()
00491   {
00492     delete tracker_;  
00493 
00494     if(reconfigureSrv_ != NULL)
00495       delete reconfigureSrv_;
00496 
00497     if(reconfigureKltSrv_ != NULL)
00498       delete reconfigureKltSrv_;
00499 
00500     if(reconfigureEdgeSrv_ != NULL)
00501       delete reconfigureEdgeSrv_;
00502   }
00503 
00504   void Tracker::spin()
00505   {    
00506       ros::Rate loopRateTracking(100);
00507       tf::Transform transform;
00508       std_msgs::Header lastHeader;
00509 
00510       while (!exiting())
00511       {
00512           // When a camera sequence is played several times,
00513           // the seq id will decrease, in this case we want to
00514           // continue the tracking.
00515           if (header_.seq < lastHeader.seq)
00516               lastTrackedImage_ = 0;
00517 
00518           if (lastTrackedImage_ < header_.seq)
00519           {
00520               lastTrackedImage_ = header_.seq;
00521 
00522               // If we can estimate the camera displacement using tf,
00523               // we update the cMo to compensate for robot motion.
00524               if (compensateRobotMotion_)
00525                   try
00526               {
00527                   tf::StampedTransform stampedTransform;
00528                   listener_.lookupTransform
00529                           (header_.frame_id, // camera frame name
00530                            header_.stamp,    // current image time
00531                            header_.frame_id, // camera frame name
00532                            lastHeader.stamp, // last processed image time
00533                            worldFrameId_,    // frame attached to the environment
00534                            stampedTransform
00535                            );
00536                   vpHomogeneousMatrix newMold;
00537                   transformToVpHomogeneousMatrix (newMold, stampedTransform);
00538                   cMo_ = newMold * cMo_;
00539 
00540                   mutex_.lock();
00541                   tracker_->setPose(image_, cMo_);
00542                   mutex_.unlock();
00543               }
00544               catch(tf::TransformException& e)
00545               {
00546                 mutex_.unlock();
00547               }
00548 
00549               // If we are lost but an estimation of the object position
00550               // is provided, use it to try to reinitialize the system.
00551               if (state_ == LOST)
00552               {
00553                   // If the last received message is recent enough,
00554                   // use it otherwise do nothing.
00555                   if (ros::Time::now () - objectPositionHint_.header.stamp
00556                           < ros::Duration (1.))
00557                       transformToVpHomogeneousMatrix
00558                               (cMo_, objectPositionHint_.transform);
00559 
00560                   mutex_.lock();
00561                   tracker_->setPose(image_, cMo_);
00562                   mutex_.unlock();
00563               }
00564 
00565               // We try to track the image even if we are lost,
00566               // in the case the tracker recovers...
00567               if (state_ == TRACKING || state_ == LOST)
00568                   try
00569               {
00570                   mutex_.lock();
00571                   // tracker_->setPose(image_, cMo_); // Removed as it is not necessary when the pose is not modified from outside.
00572                   tracker_->track(image_);
00573                   tracker_->getPose(cMo_);
00574                   mutex_.unlock();
00575               }
00576               catch(...)
00577               {
00578                   mutex_.unlock();
00579                   ROS_WARN_THROTTLE(10, "tracking lost");
00580                   state_ = LOST;
00581               }
00582 
00583               // Publish the tracking result.
00584               if (state_ == TRACKING)
00585               {
00586                   geometry_msgs::Transform transformMsg;
00587                   vpHomogeneousMatrixToTransform(transformMsg, cMo_);
00588 
00589                   // Publish position.
00590                   if (transformationPublisher_.getNumSubscribers        () > 0)
00591                   {
00592                       geometry_msgs::TransformStampedPtr objectPosition
00593                               (new geometry_msgs::TransformStamped);
00594                       objectPosition->header = header_;
00595                       objectPosition->child_frame_id = childFrameId_;
00596                       objectPosition->transform = transformMsg;
00597                       transformationPublisher_.publish(objectPosition);
00598                   }
00599 
00600                   // Publish result.
00601                   if (resultPublisher_.getNumSubscribers        () > 0)
00602                   {
00603                       geometry_msgs::PoseWithCovarianceStampedPtr result
00604                               (new geometry_msgs::PoseWithCovarianceStamped);
00605                       result->header = header_;
00606                       result->pose.pose.position.x =
00607                               transformMsg.translation.x;
00608                       result->pose.pose.position.y =
00609                               transformMsg.translation.y;
00610                       result->pose.pose.position.z =
00611                               transformMsg.translation.z;
00612 
00613                       result->pose.pose.orientation.x =
00614                               transformMsg.rotation.x;
00615                       result->pose.pose.orientation.y =
00616                               transformMsg.rotation.y;
00617                       result->pose.pose.orientation.z =
00618                               transformMsg.rotation.z;
00619                       result->pose.pose.orientation.w =
00620                               transformMsg.rotation.w;
00621                       const vpMatrix& covariance =
00622                               tracker_->getCovarianceMatrix();
00623                       for (unsigned i = 0; i < covariance.getRows(); ++i)
00624                           for (unsigned j = 0; j < covariance.getCols(); ++j)
00625                           {
00626                               unsigned idx = i * covariance.getCols() + j;
00627                               if (idx >= 36)
00628                                   continue;
00629                               result->pose.covariance[idx] = covariance[i][j];
00630                           }
00631                       resultPublisher_.publish(result);
00632                   }
00633 
00634                   // Publish moving edge sites.
00635                   if (movingEdgeSitesPublisher_.getNumSubscribers       () > 0)
00636                   {
00637                       visp_tracker::MovingEdgeSitesPtr sites
00638                               (new visp_tracker::MovingEdgeSites);
00639                       updateMovingEdgeSites(sites);
00640                       sites->header = header_;
00641                       movingEdgeSitesPublisher_.publish(sites);
00642                   }
00643                   // Publish KLT points.
00644                   if (kltPointsPublisher_.getNumSubscribers     () > 0)
00645                   {
00646                       visp_tracker::KltPointsPtr klt
00647                               (new visp_tracker::KltPoints);
00648                       updateKltPoints(klt);
00649                       klt->header = header_;
00650                       kltPointsPublisher_.publish(klt);
00651                   }
00652 
00653                   // Publish to tf.
00654                   transform.setOrigin
00655                           (tf::Vector3(transformMsg.translation.x,
00656                                        transformMsg.translation.y,
00657                                        transformMsg.translation.z));
00658                   transform.setRotation
00659                           (tf::Quaternion
00660                            (transformMsg.rotation.x,
00661                             transformMsg.rotation.y,
00662                             transformMsg.rotation.z,
00663                             transformMsg.rotation.w));
00664                   transformBroadcaster_.sendTransform
00665                           (tf::StampedTransform
00666                            (transform,
00667                             header_.stamp,
00668                             header_.frame_id,
00669                             childFrameId_));
00670               }
00671           }
00672 
00673           lastHeader = header_;
00674           spinOnce();
00675           loopRateTracking.sleep();
00676 
00677       }
00678   }
00679 
00680   // Make sure that we have an image *and* associated calibration
00681   // data.
00682   void
00683   Tracker::waitForImage()
00684   {
00685     ros::Rate loop_rate(10);
00686     while (!exiting()
00687            && (!image_.getWidth() || !image_.getHeight())
00688            && (!info_ || info_->K[0] == 0.))
00689     {
00690       //ROS_INFO_THROTTLE(1, "waiting for a rectified image...");
00691       spinOnce();
00692       loop_rate.sleep();
00693     }
00694   }
00695 
00696   void
00697   Tracker::objectPositionHintCallback
00698   (const geometry_msgs::TransformStampedConstPtr& transform)
00699   {
00700     objectPositionHint_ = *transform;
00701   }
00702 
00703 } // end of namespace visp_tracker.


visp_tracker
Author(s): Thomas Moulard
autogenerated on Sun Feb 19 2017 03:28:46