Tracker.h
Go to the documentation of this file.
00001 //-*- C++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 // 
00004 // This header declares the Tracker class.
00005 // The Tracker is one of main components of the system,
00006 // and is responsible for determining the pose of a camera
00007 // from a video feed. It uses the Map to track, and communicates 
00008 // with the MapMaker (which runs in a different thread)
00009 // to help construct this map.
00010 //
00011 // Initially there is no map, so the Tracker also has a mode to 
00012 // do simple patch tracking across a stereo pair. This is handled 
00013 // by the TrackForInitialMap() method and associated sub-methods. 
00014 // Once there is a map, TrackMap() is used.
00015 //
00016 // Externally, the tracker should be used by calling TrackFrame()
00017 // with every new input video frame. This then calls either 
00018 // TrackForInitialMap() or TrackMap() as appropriate.
00019 //
00020 
00021 #ifndef __TRACKER_H
00022 #define __TRACKER_H
00023 
00024 #include "MapMaker.h"
00025 #include "ATANCamera.h"
00026 #include "MiniPatch.h"
00027 #include "Relocaliser.h"
00028 #include "../Predictor.h"
00029 
00030 #include <sstream>
00031 #include <vector>
00032 #include <list>
00033 
00034 
00035 class TrackerData;
00036 struct Trail    // This struct is used for initial correspondences of the first stereo pair.
00037 {
00038   MiniPatch mPatch;
00039   CVD::ImageRef irCurrentPos;
00040   CVD::ImageRef irInitialPos;
00041 };
00042 
00043 class Tracker
00044 {
00045 public:
00046   Tracker(CVD::ImageRef irVideoSize, const ATANCamera &c, Map &m, MapMaker &mm);
00047   
00048   // TrackFrame is the main working part of the tracker: call this every frame.
00049   void TrackFrame(CVD::Image<CVD::byte> &imFrame, bool bDraw); 
00050   void TakeKF(bool force);
00051   void tryToRecover();
00052 
00053   inline SE3<> GetCurrentPose() { return mse3CamFromWorld;}
00054   
00055   // Gets messages to be printed on-screen for the user.
00056   std::string GetMessageForUser();
00057   
00058   inline void pressSpacebar() {mbUserPressedSpacebar = true;}
00059   inline void resetMap() {Reset();}
00060   int numPointsFound;
00061   int numPointsAttempted;
00062   enum {I_FIRST, I_SECOND, I_FAILED ,T_GOOD, T_DODGY, T_LOST, T_RECOVERED_GOOD, T_RECOVERED_DODGY, NOT_TRACKING, INITIALIZING, T_TOOK_KF} lastStepResult;
00063 
00064   // kf takking parameters (settable via ros dyn. reconfigure)
00065   double minKFTimeDist;
00066 
00067 
00068   // the value of this at I_FIRST will be approx. the first keyframe's position.
00069   // the value of this at I_SECOND will be approx. the second keyframe's position (or at least the translation will be scaled respectively).
00070   inline void setPredictedCamFromW(SE3<>& camFromW) {predictedCFromW = camFromW;}
00071   inline void setLastFrameLost(bool lost, bool useGuessForRecovery = false) {lastFrameLost = lost; useGuess = useGuessForRecovery;};
00072 
00073 protected:
00074   KeyFrame mCurrentKF;            // The current working frame as a keyframe struct
00075   
00076   // The major components to which the tracker needs access:
00077   Map &mMap;                      // The map, consisting of points and keyframes
00078   MapMaker &mMapMaker;            // The class which maintains the map
00079   ATANCamera mCamera;             // Projection model
00080   Relocaliser mRelocaliser;       // Relocalisation module
00081 
00082   CVD::ImageRef mirSize;          // Image size of whole image
00083   
00084   void Reset();                   // Restart from scratch. Also tells the mapmaker to reset itself.
00085   void RenderGrid();              // Draws the reference grid
00086 
00087   // The following members are used for initial map tracking (to get the first stereo pair and correspondences):
00088   void TrackForInitialMap();      // This is called by TrackFrame if there is not a map yet.
00089   enum {TRAIL_TRACKING_NOT_STARTED, 
00090         TRAIL_TRACKING_STARTED, 
00091         TRAIL_TRACKING_COMPLETE} mnInitialStage;  // How far are we towards making the initial map?
00092   void TrailTracking_Start();     // First frame of initial trail tracking. Called by TrackForInitialMap.
00093   int  TrailTracking_Advance();   // Steady-state of initial trail tracking. Called by TrackForInitialMap.
00094   std::list<Trail> mlTrails;      // Used by trail tracking
00095   KeyFrame mFirstKF;              // First of the stereo pair
00096   KeyFrame mPreviousFrameKF;      // Used by trail tracking to check married matches
00097   SE3<> KFZeroDesiredCamFromWorld;            // ADDED: this is the pose, KFZero is supposed to have, after stereo-init.
00098 
00099   // Methods for tracking the map once it has been made:
00100   void TrackMap();                // Called by TrackFrame if there is a map.
00101   void AssessTrackingQuality();   // Heuristics to choose between good, poor, bad.
00102   void ApplyMotionModel();        // Decaying velocity motion model applied prior to TrackMap
00103   void UpdateMotionModel();       // Motion model is updated after TrackMap
00104   int SearchForPoints(std::vector<TrackerData*> &vTD, 
00105                       int nRange, 
00106                       int nFineIts);  // Finds points in the image
00107   Vector<6> CalcPoseUpdate(std::vector<TrackerData*> vTD, 
00108                            double dOverrideSigma = 0.0, 
00109                            bool bMarkOutliers = false); // Updates pose from found points.
00110   SE3<> mse3CamFromWorld;           // Camera pose: this is what the tracker updates every frame.
00111   SE3<> mse3StartPos;               // What the camera pose was at the start of the frame.
00112   Vector<6> mv6CameraVelocity;    // Motion model
00113   double mdVelocityMagnitude;     // Used to decide on coarse tracking 
00114   double mdMSDScaledVelocityMagnitude; // Velocity magnitude scaled by relative scene depth.
00115   bool mbDidCoarse;               // Did tracking use the coarse tracking stage?
00116   
00117   bool mbDraw;                    // Should the tracker draw anything to OpenGL?
00118   
00119   // Interface with map maker:
00120   int mnFrame;                    // Frames processed since last reset
00121   int mnLastKeyFrameDropped;      // Counter of last keyframe inserted.
00122   clock_t mnLastKeyFrameDroppedClock;
00123   void AddNewKeyFrame();          // Gives the current frame to the mapmaker to use as a keyframe
00124   SE3<> predictedCFromW;                  // gets filled externally, and is used by various init stages.
00125 
00126 
00127 
00128   // Tracking quality control:
00129   int manMeasAttempted[LEVELS];
00130   int manMeasFound[LEVELS];
00131   enum {BAD, DODGY, GOOD} mTrackingQuality;
00132   int mnLostFrames;
00133 
00134   // Relocalisation functions:
00135   bool AttemptRecovery();         // Called by TrackFrame if tracking is lost.
00136   bool mbJustRecoveredSoUseCoarse;// Always use coarse tracking after recovery!
00137   bool lastFrameLost;
00138   bool useGuess;
00139 
00140   // Frame-to-frame motion init:
00141   SmallBlurryImage *mpSBILastFrame;
00142   SmallBlurryImage *mpSBIThisFrame;
00143   void CalcSBIRotation();
00144   Vector<6> mv6SBIRot;
00145   bool mbUseSBIInit;
00146   
00147   // User interaction for initial tracking:
00148   bool mbUserPressedSpacebar;
00149   std::ostringstream mMessageForUser;
00150   
00151   // GUI interface:
00152   void GUICommandHandler(std::string sCommand, std::string sParams);
00153   static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams);
00154   struct Command {std::string sCommand; std::string sParams; };
00155   std::vector<Command> mvQueuedCommands;
00156 };
00157 
00158 #endif
00159 
00160 
00161 
00162 
00163 
00164 


uga_tum_ardrone
Author(s):
autogenerated on Sat Jun 8 2019 20:30:11