MapMaker.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 
00004 //
00005 // This header declares the MapMaker class
00006 // MapMaker makes and maintains the Map struct
00007 // Starting with stereo initialisation from a bunch of matches
00008 // over keyframe insertion, continual bundle adjustment and 
00009 // data-association refinement.
00010 // MapMaker runs in its own thread, although some functions
00011 // (notably stereo init) are called by the tracker and run in the 
00012 // tracker's thread.
00013 
00014 #ifndef __MAPMAKER_H
00015 #define __MAPMAKER_H
00016 #include <cvd/image.h>
00017 #include <cvd/byte.h>
00018 #include <cvd/thread.h>
00019 
00020 #include "Map.h"
00021 #include "KeyFrame.h"
00022 #include "ATANCamera.h"
00023 #include "SmallBlurryImage.h"
00024 #include <queue>
00025 #include <sstream>
00026 #include <boost/shared_ptr.hpp>
00027 #include <ptam/OctomapInterface.h>
00028 
00029 
00030 // Each MapPoint has an associated MapMakerData class
00031 // Where the mapmaker can store extra information
00032 
00033 struct MapMakerData
00034 {
00035   std::set<KeyFrame::Ptr> sMeasurementKFs;   // Which keyframes has this map point got measurements in?
00036   std::set<KeyFrame::Ptr> sNeverRetryKFs;    // Which keyframes have measurements failed enough so I should never retry?
00037   inline int GoodMeasCount()            
00038   {  return sMeasurementKFs.size(); }
00039 };
00040 
00041 // MapMaker dervives from CVD::Thread, so everything in void run() is its own thread.
00042 class MapMaker : protected CVD::Thread
00043 {
00044 public:
00045   MapMaker(Map &m, const ATANCamera &cam, ros::NodeHandle& nh);
00046   ~MapMaker();
00047 
00048   // Make a map from scratch. Called by the tracker.
00049   bool InitFromStereo(KeyFrame::Ptr kFirst, KeyFrame::Ptr kSecond,
00050                       std::vector<std::pair<CVD::ImageRef, CVD::ImageRef> > &vMatches,
00051                       SE3<> &se3CameraPos);
00052 
00053   bool InitFromStereo_OLD(KeyFrame::Ptr kFirst, KeyFrame::Ptr kSecond,  // EXPERIMENTAL HACK
00054                           std::vector<std::pair<CVD::ImageRef, CVD::ImageRef> > &vMatches,
00055                           SE3<> &se3CameraPos);
00056 
00057 
00058   void AddKeyFrame(KeyFrame::Ptr k);   // Add a key-frame to the map. Called by the tracker.
00059   void RequestReset();   // Request that the we reset. Called by the tracker.
00060   bool ResetDone();      // Returns true if the has been done.
00061   int  QueueSize() { return mvpKeyFrameQueue.size() ;} // How many KFs in the queue waiting to be added?
00062   bool NeedNewKeyFrame(KeyFrame::Ptr kCurrent);            // Is it a good camera pose to add another KeyFrame?
00063   bool IsDistanceToNearestKeyFrameExcessive(KeyFrame::Ptr kCurrent);  // Is the camera far away from the nearest KeyFrame (i.e. maybe lost?)
00064 
00065   std::string getMessageForUser(){return mMessageForUser.str();};
00066   void resetMessageForUser(){mMessageForUser.str(""); };
00067 
00068   //Weiss{ moved from protected
00069   void ApplyGlobalScaleToMap(double dScale);
00070   void ApplyGlobalTransformationToMap(SE3<> se3NewFromOld);
00071   KeyFrame::Ptr ClosestKeyFrame(KeyFrame::Ptr k);
00072   std::vector<KeyFrame::Ptr> NClosestKeyFrames(KeyFrame::Ptr k, unsigned int N);
00073   //}
00074 
00075 protected:
00076 
00077   Map &mMap;               // The map
00078   ATANCamera mCamera;      // Same as the tracker's camera: N.B. not a reference variable!
00079   virtual void run();      // The MapMaker thread code lives here
00080 
00081   //slynen octomap_interface{
00082   OctoMapInterface octomap_interface;
00083   //}
00084 
00085   // Functions for starting the map from scratch:
00086   SE3<> CalcPlaneAligner();
00087 
00088   // Map expansion functions:
00089   void AddKeyFrameFromTopOfQueue();  
00090   void ThinCandidates(KeyFrame::Ptr k, int nLevel);
00091   bool AddSomeMapPoints(int nLevel);
00092   bool AddPointEpipolar(KeyFrame::Ptr kSrc, KeyFrame::Ptr kTarget, int nLevel, int nCandidate);
00093   // Returns point in ref frame B
00094   Vector<3> ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B);
00095 
00096   // Bundle adjustment functions:
00097   void BundleAdjust(std::set<KeyFrame::Ptr>, std::set<KeyFrame::Ptr>, std::set<boost::shared_ptr<MapPoint> >, bool);
00098   void BundleAdjustAll();
00099   void BundleAdjustRecent();
00100 
00101   // Data association functions:
00102   int ReFindInSingleKeyFrame(KeyFrame::Ptr k);
00103   void ReFindFromFailureQueue();
00104   void ReFindNewlyMade();
00105   void ReFindAll();
00106   bool ReFind_Common(KeyFrame::Ptr k, boost::shared_ptr<MapPoint> p);
00107   void SubPixelRefineMatches(KeyFrame::Ptr k, int nLevel);
00108 
00109   // General Maintenance/Utility:
00110   void Reset();
00111   void HandleBadPoints();
00112   double DistToNearestKeyFrame(KeyFrame::Ptr kCurrent);
00113   double KeyFrameLinearDist(KeyFrame::Ptr k1, KeyFrame::Ptr k2);
00114   bool RefreshSceneDepth(KeyFrame::Ptr pKF);
00115 
00116 
00117   // GUI Interface:
00118   void GUICommandHandler(std::string sCommand, std::string sParams);
00119   static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams);
00120   struct Command {std::string sCommand; std::string sParams; };
00121   std::vector<Command> mvQueuedCommands;
00122 
00123 
00124   // Member variables:
00125   std::vector<KeyFrame::Ptr> mvpKeyFrameQueue;  // Queue of keyframes from the tracker waiting to be processed
00126   std::vector<std::pair<KeyFrame::Ptr, boost::shared_ptr<MapPoint> > > mvFailureQueue; // Queue of failed observations to re-find
00127   std::queue<boost::shared_ptr<MapPoint> > mqNewQueue;   // Queue of newly-made map points to re-find in other KeyFrames
00128 
00129   double mdWiggleScale;  // Metric distance between the first two KeyFrames (copied from GVar)
00130   // This sets the scale of the map
00131   //GVars3::gvar3<double> mgvdWiggleScale;   // GVar for above
00132   double mdWiggleScaleDepthNormalized;  // The above normalized against scene depth, 
00133   // this controls keyframe separation
00134 
00135   bool mbBundleConverged_Full;    // Has global bundle adjustment converged?
00136   bool mbBundleConverged_Recent;  // Has local bundle adjustment converged?
00137 
00138   // Thread interaction signalling stuff
00139   bool mbResetRequested;   // A reset has been requested
00140   bool mbResetDone;        // The reset was done.
00141   bool mbBundleAbortRequested;      // We should stop bundle adjustment
00142   bool mbBundleRunning;             // Bundle adjustment is running
00143   bool mbBundleRunningIsRecent;     //    ... and it's a local bundle adjustment.
00144 
00145 
00146   std::ostringstream mMessageForUser;
00147 };
00148 
00149 #endif
00150 
00151 
00152 
00153 
00154 
00155 
00156 
00157 
00158 
00159 
00160 
00161 
00162 
00163 
00164 
00165 
00166 
00167 


ptam
Author(s): Stephan Weiss, Markus Achtelik, Simon Lynen
autogenerated on Tue Jan 7 2014 11:12:22