System.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/System.h"
00003 #include "ptam/OpenGL.h"
00004 #include <gvars3/instances.h>
00005 #include <stdlib.h>
00006 #include "ptam/ATANCamera.h"
00007 #include "ptam/MapMaker.h"
00008 #include "ptam/Tracker.h"
00009 //#include "ptam/ARDriver.h"
00010 #include "ptam/MapViewer.h"
00011 #include "ptam/LevelHelpers.h"
00012 #include "ptam/MapPoint.h"
00013 
00014 #include <ptam_com/ptam_info.h>
00015 #include <opencv/cv.h>
00016 #include <cvd/vision.h>
00017 
00018 using namespace CVD;
00019 using namespace std;
00020 using namespace GVars3;
00021 
00022 
00023 System::System() :
00024       nh_("vslam"), image_nh_(""), first_frame_(true)
00025 {
00026 
00027   pub_pose_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped> ("pose", 1);
00028   pub_pose_world_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped> ("pose_world", 1);
00029   pub_info_ = nh_.advertise<ptam_com::ptam_info> ("info", 1);
00030   srvPC_ = nh_.advertiseService("pointcloud", &System::pointcloudservice,this);
00031   srvKF_ = nh_.advertiseService("keyframes", &System::keyframesservice,this);
00032 
00033   sub_imu_ = nh_.subscribe("imu", 100, &System::imuCallback, this);
00034   sub_kb_input_ = nh_.subscribe("key_pressed", 100, &System::keyboardCallback, this);
00035 
00036   image_nh_.setCallbackQueue(&image_queue_);
00037   // get topic to subscribe to:
00038   std::string topic = image_nh_.resolveName("image");
00039   if (topic == "/image")
00040   {
00041     ROS_WARN("video source: image has not been remapped! Typical command-line usage:\n"
00042         "\t$ ./ptam image:=<image topic>");
00043   }
00044 
00045   image_transport::ImageTransport it(image_nh_);
00046   sub_image_ = it.subscribe(topic, 1, &System::imageCallback, this, image_transport::TransportHints("raw", ros::TransportHints().tcpNoDelay(true)));
00047   pub_preview_image_ = it.advertise("vslam/preview", 1);
00048 }
00049 
00050 
00051 void System::init(const CVD::ImageRef & size)
00052 {
00053   img_bw_.resize(size);
00054   img_rgb_.resize(size);
00055 
00056   mpCamera = new ATANCamera("Camera");
00057 
00058   mpMap = new Map;
00059   mpMapMaker = new MapMaker(*mpMap, *mpCamera, nh_);
00060   mpTracker = new Tracker(size, *mpCamera, *mpMap, *mpMapMaker);
00061 
00062   GUI.RegisterCommand("exit", GUICommandCallBack, this);
00063   GUI.RegisterCommand("quit", GUICommandCallBack, this);
00064 
00065   if(PtamParameters::fixparams().gui){
00066     mGLWindow = new GLWindow2(size, "PTAM");
00067     mpMapViewer = new MapViewer(*mpMap, *mGLWindow);
00068 
00069     GUI.ParseLine("GLWindow.AddMenu Menu Menu");
00070     GUI.ParseLine("Menu.ShowMenu Root");
00071     GUI.ParseLine("Menu.AddMenuButton Root Reset Reset Root");
00072     GUI.ParseLine("Menu.AddMenuButton Root Spacebar PokeTracker Root");
00073     GUI.ParseLine("DrawMap=0");
00074     GUI.ParseLine("Menu.AddMenuToggle Root \"View Map\" DrawMap Root");
00075   }
00076 }
00077 
00078 
00079 void System::Run()
00080 {
00081   while(ros::ok()){
00082     //    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.01));
00083     //    image_queue_.callAvailable();
00084 
00085     ros::getGlobalCallbackQueue()->callAvailable();
00086     image_queue_.callAvailable(ros::WallDuration(0.01));
00087   }
00088 }
00089 
00090 void System::imageCallback(const sensor_msgs::ImageConstPtr & img)
00091 {
00092   //    static ros::Time t = img->header.stamp;
00093 
00094 
00095   ROS_ASSERT(img->encoding == sensor_msgs::image_encodings::MONO8 && img->step == img->width);
00096 
00097   const VarParams& varParams = PtamParameters::varparams();
00098 
00099   if(first_frame_){
00100     init(CVD::ImageRef(img->width, img->height));
00101     first_frame_ = false;
00102   }
00103 
00104   TooN::SO3<double> imu_orientation;
00105   if (varParams.MotionModelSource == ptam::PtamParams_MM_IMU)
00106   {
00107     sensor_msgs::Imu imu;
00108 
00109 
00110     if (!findClosest(img->header.stamp, imu_msgs_, &imu, 0.01))
00111     {
00112       ROS_WARN("no imu match, skipping frame");
00113       return;
00114     }
00115     if (!transformQuaternion(img->header.frame_id, imu.header, imu.orientation, imu_orientation))
00116     {
00117       return;
00118     }
00119   }
00120 
00121 
00122 //  -------------------
00123   // TODO: avoid copy, by calling TrackFrame, with the ros image, because there is another copy inside TrackFrame
00124   CVD::BasicImage<CVD::byte> img_tmp((CVD::byte *)&img->data[0], CVD::ImageRef(img->width, img->height));
00125   CVD::copy(img_tmp, img_bw_);
00126 
00127   bool tracker_draw = false;
00128 
00129   static gvar3<int> gvnDrawMap("DrawMap", 0, HIDDEN | SILENT);
00130   bool bDrawMap = mpMap->IsGood() && *gvnDrawMap;
00131 
00132   if(PtamParameters::fixparams().gui){
00133     CVD::copy(img_tmp, img_rgb_);
00134 
00135     mGLWindow->SetupViewport();
00136     mGLWindow->SetupVideoOrtho();
00137     mGLWindow->SetupVideoRasterPosAndZoom();
00138     tracker_draw = !bDrawMap;
00139   }
00140 
00141   mpTracker->TrackFrame(img_bw_, tracker_draw, imu_orientation);
00142 
00143   publishPoseAndInfo(img->header);
00144 
00145   publishPreviewImage(img_bw_, img->header);
00146   std::cout << mpMapMaker->getMessageForUser();
00147 
00148   if(PtamParameters::fixparams().gui){
00149     string sCaption;
00150 
00151     if (bDrawMap)
00152       mpMapViewer->DrawMap(mpTracker->GetCurrentPose());
00153 
00154     if (bDrawMap)
00155       sCaption = mpMapViewer->GetMessageForUser();
00156     else
00157       sCaption = mpTracker->GetMessageForUser();
00158     mGLWindow->DrawCaption(sCaption);
00159     mGLWindow->DrawMenus();
00160     mGLWindow->swap_buffers();
00161     mGLWindow->HandlePendingEvents();
00162   }
00163   //    usleep(50000);
00164   //
00165   //  ros::Time t1 = img->header.stamp;
00166   //
00167   //  static unsigned long c=0;
00168   //  if((c+1)<(img->header.seq))
00169   //  {
00170   //      ROS_WARN_STREAM("missed " << img->header.seq-c+1<< " frame(s)");
00171   //      ROS_WARN_STREAM("time: " <<  (t1.toSec()-t.toSec()));
00172   //  }
00173   //  ROS_WARN_STREAM("time: " <<  1/(t1.toSec()-t.toSec()));
00174   //  c=img->header.seq;
00175   //  t = img->header.stamp;
00176 
00177 }
00178 
00179 
00180 void System::imuCallback(const sensor_msgs::ImuConstPtr & msg)
00181 {
00182   imu_msgs_.push(*msg);
00183   if (imu_msgs_.size() > 100)
00184     imu_msgs_.pop();
00185 }
00186 
00187 template<class T>
00188 bool System::findClosest(const ros::Time & timestamp, std::queue<T> & queue, T * obj, const double & max_delay)
00189 {
00190   double best_dt(1e9);
00191   double tmp_dt;
00192   //  size_t qs_before = queue.size();
00193   //  int i = 0;
00194   while (!queue.empty())
00195   {
00196     const T & curr_obj = queue.front();
00197     tmp_dt = (timestamp - curr_obj.header.stamp).toSec();
00198 
00199     if (tmp_dt < -max_delay)
00200       break;
00201     if (std::abs(tmp_dt) < best_dt)
00202     {
00203       best_dt = std::abs(tmp_dt);
00204       *obj = curr_obj;
00205       //      i++;
00206     }
00207     queue.pop();
00208   }
00209   if (best_dt > max_delay)
00210   {
00211     //    ROS_WARN("dt(%f) > 0.01 qs:%d, %d/%d", best_dt, queue.size(), qs_before, i);
00212     return false;
00213   }
00214   else
00215   {
00216     return true;
00217   };
00218 }
00219 
00220 
00221 void System::keyboardCallback(const std_msgs::StringConstPtr & kb_input){
00222   mpTracker->command(kb_input->data);
00223 }
00224 
00225 bool System::transformQuaternion(const std::string & target_frame, const std_msgs::Header & header,
00226                                  const geometry_msgs::Quaternion & _q_in, TooN::SO3<double> & r_out)
00227 {
00228   geometry_msgs::QuaternionStamped q_in, q_out;
00229   q_in.header = header;
00230   q_in.quaternion = _q_in;
00231   try
00232   {
00233     tf_sub_.transformQuaternion(target_frame, q_in, q_out);
00234     quaternionToRotationMatrix(q_out.quaternion, r_out);
00235     return true;
00236   }
00237   catch (tf::TransformException & e)
00238   {
00239     ROS_WARN_STREAM("could not transform quaternion: "<<e.what());
00240     return false;
00241   }
00242   return true;
00243 }
00244 
00245 bool System::transformPoint(const std::string & target_frame, const std_msgs::Header & header,
00246                             const geometry_msgs::Point & _p_in, TooN::Vector<3> & _p_out)
00247 {
00248   geometry_msgs::PointStamped p_in, p_out;
00249   p_in.header = header;
00250   p_in.point = _p_in;
00251   try
00252   {
00253     tf_sub_.transformPoint(target_frame, p_in, p_out);
00254     _p_out[0] = p_out.point.x;
00255     _p_out[1] = p_out.point.y;
00256     _p_out[2] = p_out.point.z;
00257     return true;
00258   }
00259   catch (tf::TransformException & e)
00260   {
00261     ROS_WARN_STREAM("could not transform point: "<<e.what());
00262     return false;
00263   }
00264   return true;
00265 }
00266 
00267 void System::publishPoseAndInfo(const std_msgs::Header & header)
00268 {
00269   
00270   double scale = PtamParameters::varparams().Scale;
00271 
00272   static float fps = 0;
00273   static double last_time = 0;
00274 
00275   std::string frame_id(header.frame_id);
00276 
00277   if (frame_id == "")
00278   {
00279     ROS_WARN_ONCE("camera frame id not set, will set it to \"ptam\"");
00280     frame_id = "ptam";
00281   }
00282 
00283   if (scale <= 0)
00284   {
00285     ROS_WARN_STREAM("scale ("<<scale<<") <= 0, set to 1");
00286     scale = 1;
00287   }
00288 
00289   if (mpTracker->getTrackingQuality() && mpMap->IsGood())
00290   {
00291     TooN::SE3<double> pose = mpTracker->GetCurrentPose();
00292     //world in the camera frame
00293     TooN::Matrix<3, 3, double> r_ptam = pose.get_rotation().get_matrix();
00294     TooN::Vector<3, double> t_ptam =  pose.get_translation();
00295 
00296     tf::StampedTransform transform_ptam(tf::Transform(tf::Matrix3x3(r_ptam(0, 0), r_ptam(0, 1), r_ptam(0, 2), r_ptam(1, 0), r_ptam(1, 1), r_ptam(1, 2), r_ptam(2, 0), r_ptam(2, 1), r_ptam(2, 2))
00297     , tf::Vector3(t_ptam[0] / scale, t_ptam[1] / scale, t_ptam[2] / scale)), header.stamp, frame_id, PtamParameters::fixparams().parent_frame);
00298 
00299     //camera in the world frame
00300     TooN::Matrix<3, 3, double> r_world = pose.get_rotation().get_matrix().T();
00301     TooN::Vector<3, double> t_world =  - r_world * pose.get_translation();
00302 
00303     tf::StampedTransform transform_world(tf::Transform(tf::Matrix3x3(r_world(0, 0), r_world(0, 1), r_world(0, 2), r_world(1, 0), r_world(1, 1), r_world(1, 2), r_world(2, 0), r_world(2, 1), r_world(2, 2))
00304         , tf::Vector3(t_world[0] / scale, t_world[1] / scale, t_world[2] / scale)), header.stamp, PtamParameters::fixparams().parent_frame, frame_id);
00305 
00306     tf_pub_.sendTransform(transform_world);
00307 
00308     if (pub_pose_.getNumSubscribers() > 0 || pub_pose_world_.getNumSubscribers() > 0)
00309     {
00310       //world in the camera frame
00311       geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
00312       const tf::Quaternion & q_tf_ptam = transform_ptam.getRotation();
00313       const tf::Vector3 & t_tf_ptam = transform_ptam.getOrigin();
00314       msg_pose->pose.pose.orientation.w = q_tf_ptam.w();
00315       msg_pose->pose.pose.orientation.x = q_tf_ptam.x();
00316       msg_pose->pose.pose.orientation.y = q_tf_ptam.y();
00317       msg_pose->pose.pose.orientation.z = q_tf_ptam.z();
00318       msg_pose->pose.pose.position.x = t_tf_ptam.x();
00319       msg_pose->pose.pose.position.y = t_tf_ptam.y();
00320       msg_pose->pose.pose.position.z = t_tf_ptam.z();
00321 
00322       TooN::Matrix<6> covar = mpTracker->GetCurrentCov();
00323       for (unsigned int i = 0; i < msg_pose->pose.covariance.size(); i++)
00324         msg_pose->pose.covariance[i] = sqrt(fabs(covar[i % 6][i / 6]));
00325 
00326       msg_pose->header = header;
00327       pub_pose_.publish(msg_pose);
00328 
00329 
00330       //camera in the world frame
00331       const tf::Quaternion & q_tf_world = transform_world.getRotation();
00332       const tf::Vector3 & t_tf_world = transform_world.getOrigin();
00333       msg_pose->pose.pose.orientation.w = q_tf_world.w();
00334       msg_pose->pose.pose.orientation.x = q_tf_world.x();
00335       msg_pose->pose.pose.orientation.y = q_tf_world.y();
00336       msg_pose->pose.pose.orientation.z = q_tf_world.z();
00337       msg_pose->pose.pose.position.x = t_tf_world.x();
00338       msg_pose->pose.pose.position.y = t_tf_world.y();
00339       msg_pose->pose.pose.position.z = t_tf_world.z();
00340 
00341       TooN::Matrix<6> CovRot;
00342       CovRot.slice(0,0,3,3) = r_ptam;
00343       CovRot.slice(3,3,3,3) = CovRot.slice(0,0,3,3);
00344 
00345       TooN::Matrix<6> covar_world = CovRot.T() * covar * CovRot;
00346 
00347       for (unsigned int i = 0; i < msg_pose->pose.covariance.size(); i++)
00348         msg_pose->pose.covariance[i] = sqrt(fabs(covar_world[i % 6][i / 6]));
00349 
00350       pub_pose_world_.publish(msg_pose);
00351 
00352     }
00353 
00354     if (pub_info_.getNumSubscribers() > 0)
00355     {
00356       ptam_com::ptam_infoPtr msg_info(new ptam_com::ptam_info);
00357       double diff = header.stamp.toSec() - last_time;
00358       if (diff < 1.0 && diff > 0.005)
00359         fps = fps * 0.8 + 0.2 / diff;
00360       if (fabs(fps) > 200)
00361         fps = 200;
00362       last_time = header.stamp.toSec();
00363 
00364       msg_info->header = header;
00365       msg_info->fps = fps;
00366       msg_info->mapQuality = mpMap->bGood;
00367       msg_info->trackingQuality = mpTracker->getTrackingQuality();
00368       msg_info->trackerMessage = mpTracker->GetMessageForUser();
00369       //      msg_info->mapViewerMessage = mpMapViewer->GetMessageForUser();
00370       msg_info->keyframes = mpMap->vpKeyFrames.size();
00371       pub_info_.publish(msg_info);
00372     }
00373   };
00374 }
00375 
00376 
00377 void System::publishPreviewImage(CVD::Image<CVD::byte> & img, const std_msgs::Header & header)
00378 {
00379   CVD::Image<TooN::Vector<2> > & grid = mpTracker->ComputeGrid();
00380   std::list<Trail> & trails = mpTracker->getTrails();
00381   bool drawGrid = mpTracker->getTrailTrackingComplete();
00382   bool drawTrails = mpTracker->getTrailTrackingStarted();
00383 
00384   if (pub_preview_image_.getNumSubscribers() > 0)
00385   {
00386     CVD::ImageRef sub_size(img.size()/2);
00387     sensor_msgs::ImagePtr img_msg(new sensor_msgs::Image);
00388     img_msg->header = header;
00389     img_msg->encoding = sensor_msgs::image_encodings::MONO8;
00390     img_msg->width = sub_size.x;
00391     img_msg->height = sub_size.y;
00392     img_msg->step = sub_size.x;
00393     img_msg->is_bigendian = 0;
00394     img_msg->data.resize(sub_size.x * sub_size.y);
00395 
00396     // subsample image
00397     CVD::BasicImage<CVD::byte> img_sub((CVD::byte *)&img_msg->data[0], sub_size);
00398     CVD::halfSample(img, img_sub);
00399 
00400     // set opencv pointer to image
00401     IplImage * ocv_img = cvCreateImageHeader(cvSize(img_sub.size().x, img_sub.size().y), IPL_DEPTH_8U, 1);
00402     ocv_img->imageData = (char*)&img_msg->data[0];
00403 
00404     int dim0 = grid.size().x;
00405     int dim1 = grid.size().y;
00406 
00407     if (drawGrid)
00408     {
00409       for (int i = 0; i < dim0; i++)
00410       {
00411         for (int j = 0; j < dim1 - 1; j++)
00412           cvLine( ocv_img, cvPoint(grid[i][j][0]/2, grid[i][j][1]/2), cvPoint(grid[i][j + 1][0]/2, grid[i][j + 1][1]/2),
00413                   CV_RGB(50, 50, 50)
00414           );
00415 
00416         for (int j = 0; j < dim1 - 1; j++)
00417           cvLine(ocv_img, cvPoint(grid[j][i][0]/2, grid[j][i][1]/2), cvPoint(grid[j + 1][i][0]/2, grid[j + 1][i][1]/2),
00418                  CV_RGB(50, 50, 50)
00419           );
00420       }
00421     }
00422 
00423     if (drawTrails)
00424     {
00425 
00426       
00427       int level = PtamParameters::fixparams().InitLevel;
00428 
00429       for (std::list<Trail>::iterator i = trails.begin(); i != trails.end(); i++)
00430       {
00431         cvLine(ocv_img, cvPoint(LevelZeroPos(i->irCurrentPos.x, level)/2, LevelZeroPos(i->irCurrentPos.y, level)/2),
00432                cvPoint(LevelZeroPos(i->irInitialPos.x, level)/2, LevelZeroPos(i->irInitialPos.y, level)/2),
00433                CV_RGB(0, 0, 0), 2);
00434       }
00435     }
00436 
00437     pub_preview_image_.publish(img_msg);
00438     cvReleaseImageHeader(&ocv_img);
00439   }
00440 }
00441 
00442 //Weiss{
00443 bool System::pointcloudservice(ptam_com::PointCloudRequest & req, ptam_com::PointCloudResponse & resp)
00444 {
00445   static unsigned int seq=0;
00446   int dimension   = 6;
00447 
00448   resp.pointcloud.header.seq=seq;
00449   seq++;
00450   resp.pointcloud.header.stamp = ros::Time::now();
00451   resp.pointcloud.height = 1;
00452   resp.pointcloud.header.frame_id = "/world";
00453   if(mpMap->bGood)
00454   {
00455     resp.pointcloud.width = mpMap->vpPoints.size();
00456     resp.pointcloud.fields.resize(dimension);
00457     resp.pointcloud.fields[0].name = "x";
00458     resp.pointcloud.fields[0].offset = 0*sizeof(uint32_t);
00459     resp.pointcloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00460     resp.pointcloud.fields[0].count = 1;
00461     resp.pointcloud.fields[1].name = "y";
00462     resp.pointcloud.fields[1].offset = 1*sizeof(uint32_t);
00463     resp.pointcloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00464     resp.pointcloud.fields[1].count = 1;
00465     resp.pointcloud.fields[2].name = "z";
00466     resp.pointcloud.fields[2].offset = 2*sizeof(uint32_t);
00467     resp.pointcloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00468     resp.pointcloud.fields[2].count = 1;
00469     resp.pointcloud.fields[3].name = "rgb";
00470     resp.pointcloud.fields[3].offset = 3*sizeof(uint32_t);
00471     resp.pointcloud.fields[3].datatype = sensor_msgs::PointField::INT32;
00472     resp.pointcloud.fields[3].count = 1;
00473     resp.pointcloud.fields[4].name = "KF";
00474     resp.pointcloud.fields[4].offset = 4*sizeof(uint32_t);
00475     resp.pointcloud.fields[4].datatype = sensor_msgs::PointField::INT32;
00476     resp.pointcloud.fields[4].count = 1;
00477     resp.pointcloud.fields[5].name = "lvl";
00478     resp.pointcloud.fields[5].offset = 5*sizeof(uint32_t);
00479     resp.pointcloud.fields[5].datatype = sensor_msgs::PointField::INT32;
00480     resp.pointcloud.fields[5].count = 1;
00481 
00482     resp.pointcloud.point_step = dimension*sizeof(uint32_t);
00483     resp.pointcloud.row_step = resp.pointcloud.point_step * resp.pointcloud.width;
00484     resp.pointcloud.data.resize(resp.pointcloud.row_step * resp.pointcloud.height);
00485     resp.pointcloud.is_dense = false;
00486 
00487 
00488     unsigned char* dat = &(resp.pointcloud.data[0]);
00489     unsigned int n=0;
00490     for(std::vector<MapPoint::Ptr>::iterator it=mpMap->vpPoints.begin(); it!=mpMap->vpPoints.end(); ++it,++n)
00491     {
00492       if(n>resp.pointcloud.width-1) break;
00493       MapPoint& p = *(*it);
00494 
00495       Vector<3,float> fvec = p.v3WorldPos;
00496       uint32_t colorlvl = 0xff<<((3-p.nSourceLevel)*8);
00497       uint32_t lvl = p.nSourceLevel;
00498       uint32_t KF = p.pPatchSourceKF->ID;
00499 
00500       memcpy(dat, &(fvec),3*sizeof(float));
00501       memcpy(dat+3*sizeof(uint32_t),&colorlvl,sizeof(uint32_t));
00502       memcpy(dat+4*sizeof(uint32_t),&lvl,sizeof(uint32_t));
00503       memcpy(dat+5*sizeof(uint32_t),&KF,sizeof(uint32_t));
00504       dat+=resp.pointcloud.point_step;
00505     }
00506   }
00507   return true;
00508 }
00509 
00510 
00511 bool System::keyframesservice(ptam_com::KeyFrame_srvRequest & req, ptam_com::KeyFrame_srvResponse & resp)
00512 {
00513   // flags:     negative number = send newest N KeyFrames
00514   //                    zero = send all available KeyFrames
00515   //                    positive number = send all KeyFrames with ID>N
00516 
00517   
00518   double scale = PtamParameters::varparams().Scale;
00519 
00520   TooN::SE3<double> pose;
00521   TooN::Matrix<3, 3, double> rot;
00522   TooN::Vector<3, double> trans;
00523   tf::Quaternion q;
00524   tf::Vector3 t;
00525   geometry_msgs::PoseWithCovarianceStamped buffpose;
00526   bool takeKF=false;
00527   int k=0;
00528   static unsigned int seq=0;
00529 
00530   if(!(mpMap->vpKeyFrames.size()>0) | !mpMap->bGood)
00531     return false;
00532 
00533   resp.KFids.reserve(mpMap->vpKeyFrames.size());
00534   resp.KFs.reserve(mpMap->vpKeyFrames.size());
00535 
00536   for(std::vector<KeyFrame::Ptr>::reverse_iterator rit=mpMap->vpKeyFrames.rbegin(); rit!=mpMap->vpKeyFrames.rend();++rit)
00537   {
00538     if((req.flags>0) & ((*rit)->ID>=req.flags))
00539       takeKF=true;
00540     else if((req.flags<0) & (k<=abs(req.flags)))
00541       takeKF=true;
00542     else if(req.flags==0)
00543       takeKF=true;
00544     else if((req.flags>0) & ((*rit)->ID<req.flags))
00545       break;
00546     else if((req.flags<0) & (k>abs(req.flags)))
00547       break;
00548 
00549     if(takeKF)
00550     {
00551       takeKF=false;
00552       resp.KFids.push_back((*rit)->ID);
00553       pose = (*rit)->se3CfromW;
00554       rot =pose.get_rotation().get_matrix();
00555       trans = pose.get_translation();
00556       tf::Transform transform(tf::Matrix3x3(rot(0, 0), rot(0, 1), rot(0, 2),
00557                                           rot(1, 0), rot(1, 1), rot(1, 2),
00558                                           rot(2, 0), rot(2, 1), rot(2, 2)),
00559                               tf::Vector3(trans[0] / scale, trans[1]/ scale, trans[2] / scale));
00560       q = transform.getRotation();
00561       t = transform.getOrigin();
00562       buffpose.header.seq=seq;
00563       buffpose.header.stamp=ros::Time::now();
00564       buffpose.pose.pose.position.x=t[0];
00565       buffpose.pose.pose.position.y=t[1];
00566       buffpose.pose.pose.position.z=t[2];
00567       buffpose.pose.pose.orientation.w=q.w();
00568       buffpose.pose.pose.orientation.x=q.x();
00569       buffpose.pose.pose.orientation.y=q.y();
00570       buffpose.pose.pose.orientation.z=q.z();
00571       memset(&(buffpose.pose.covariance[0]),0,sizeof(double)*6*6);
00572       resp.KFs.push_back(buffpose);
00573       seq++;
00574     }
00575     k++;
00576   }
00577   return true;
00578 }
00579 
00580 
00581 //}
00582 
00583 void System::quaternionToRotationMatrix(const geometry_msgs::Quaternion & q, TooN::SO3<double> & R)
00584 {
00585   // stolen from Eigen3 and adapted to TooN
00586 
00587   TooN::Matrix<3, 3, double> res;
00588 
00589   const double tx = 2 * q.x;
00590   const double ty = 2 * q.y;
00591   const double tz = 2 * q.z;
00592   const double twx = tx * q.w;
00593   const double twy = ty * q.w;
00594   const double twz = tz * q.w;
00595   const double txx = tx * q.x;
00596   const double txy = ty * q.x;
00597   const double txz = tz * q.x;
00598   const double tyy = ty * q.y;
00599   const double tyz = tz * q.y;
00600   const double tzz = tz * q.z;
00601 
00602   res(0, 0) = 1 - (tyy + tzz);
00603   res(0, 1) = txy - twz;
00604   res(0, 2) = txz + twy;
00605   res(1, 0) = txy + twz;
00606   res(1, 1) = 1 - (txx + tzz);
00607   res(1, 2) = tyz - twx;
00608   res(2, 0) = txz - twy;
00609   res(2, 1) = tyz + twx;
00610   res(2, 2) = 1 - (txx + tyy);
00611 
00612   R = res;
00613 
00614   //  R = TooN::SO3<double>::exp(TooN::makeVector<double>(q.x, q.y, q.z) * acos(q.w) * 2.0 / sqrt(q.x * q.x + q.y * q.y + q.z * q.z));
00615 }
00616 
00617 
00618 
00619 void System::GUICommandCallBack(void *ptr, string sCommand, string sParams)
00620 {
00621   if (sCommand == "quit" || sCommand == "exit")
00622     ros::shutdown();
00623 }
00624 
00625 
00626 
00627 
00628 
00629 
00630 
00631 


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33