Tracker.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/OpenGL.h"
00003 #include "ptam/Tracker.h"
00004 #include "ptam/MEstimator.h"
00005 #include "ptam/ShiTomasi.h"
00006 #include "ptam/SmallMatrixOpts.h"
00007 #include "ptam/PatchFinder.h"
00008 #include "ptam/TrackerData.h"
00009 #include <ptam/Params.h>
00010 
00011 #include <cvd/utility.h>
00012 #include <cvd/gl_helpers.h>
00013 #include <cvd/fast_corner.h>
00014 #include <cvd/vision.h>
00015 #include <TooN/wls.h>
00016 #include <TooN/SVD.h>
00017 #include <gvars3/instances.h>
00018 #include <gvars3/GStringUtil.h>
00019 
00020 #include <opencv/cv.h>
00021 
00022 #include <fstream>
00023 #include <fcntl.h>
00024 #include <math.h>
00025 #include <vector>
00026 
00027 using namespace CVD;
00028 using namespace std;
00029 using namespace GVars3;
00030 
00031 // The constructor mostly sets up interal reference variables
00032 // to the other classes..
00033 Tracker::Tracker(ImageRef irVideoSize, const ATANCamera &c, Map &m, MapMaker &mm) : 
00034                                                           mMap(m),
00035                                                           mMapMaker(mm),
00036                                                           mCamera(c),
00037                                                           mRelocaliser(mMap, mCamera),
00038                                                           mirSize(irVideoSize)
00039 {
00040   mCurrentKF.reset(new KeyFrame);
00041   mCurrentKF->bFixed = false;
00042   GUI.RegisterCommand("Reset", GUICommandCallBack, this);
00043   GUI.RegisterCommand("KeyPress", GUICommandCallBack, this);
00044   GUI.RegisterCommand("PokeTracker", GUICommandCallBack, this);
00045   TrackerData::irImageSize = mirSize;
00046 
00047   //Weiss{
00048   mOldKF.reset(new KeyFrame);
00049   mOldKF->dSceneDepthMedian=1;
00050   mOldKF->se3CfromW = SE3<>::exp(TooN::makeVector(0,0,1,M_PI,0,0));
00051   mAutoreset=false;
00052   //}
00053 
00054   mpSBILastFrame = NULL;
00055   mpSBIThisFrame = NULL;
00056 
00057   // Most of the initialisation is done in Reset()
00058   Reset();
00059 }
00060 
00061 // Resets the tracker, wipes the map.
00062 // This is the main Reset-handler-entry-point of the program! Other classes' resets propagate from here.
00063 // It's always called in the Tracker's thread, often as a GUI command.
00064 void Tracker::Reset()
00065 {
00066   // Tell the MapMaker to reset itself..
00067   // this may take some time, since the mapmaker thread may have to wait
00068   // for an abort-check during calculation, so sleep while waiting.
00069   // MapMaker will also clear the map.
00070   mMapMaker.RequestReset();
00071   while(!mMapMaker.ResetDone())
00072 #ifndef WIN32
00073     usleep(10);
00074 #else
00075   Sleep(1);
00076 #endif
00077 
00078   mbDidCoarse = false;
00079   mbUserPressedSpacebar = false;
00080   mTrackingQuality = GOOD;
00081   mnLostFrames = 0;
00082   mdMSDScaledVelocityMagnitude = 0;
00083   mCurrentKF->dSceneDepthMean = 1.0;
00084   mCurrentKF->dSceneDepthSigma = 1.0;
00085   //Weiss{
00086   mCurrentKF->dSceneDepthMedian = 1.0;
00087   //}
00088   mnInitialStage = TRAIL_TRACKING_NOT_STARTED;
00089   mlTrails.clear();
00090   mCamera.SetImageSize(mirSize);
00091   mCurrentKF->mMeasurements.clear();
00092   mnLastKeyFrameDropped = -20;
00093   mnFrame=0;
00094   mv6CameraVelocity = Zeros;
00095   mbJustRecoveredSoUseCoarse = false;
00096 }
00097 
00098 void Tracker::TrackFrame(CVD::Image<CVD::byte> &imFrame, bool bDraw, const TooN::SO3<> & imu){
00099   mso3CurrentImu = imu;
00100   TrackFrame(imFrame, bDraw);
00101   mso3LastImu = mso3CurrentImu;
00102 }
00103 
00104 
00105 // TrackFrame is called by System.cc with each incoming video frame.
00106 // It figures out what state the tracker is in, and calls appropriate internal tracking
00107 // functions. bDraw tells the tracker wether it should output any GL graphics
00108 // or not (it should not draw, for example, when AR stuff is being shown.)
00109 void Tracker::TrackFrame(Image<CVD::byte> &imFrame, bool bDraw)
00110 {
00111   mbDraw = bDraw;
00112   mMessageForUser.str("");   // Wipe the user message clean
00113   mMapMaker.resetMessageForUser();
00114 
00115   // Take the input video image, and convert it into the tracker's keyframe struct
00116   // This does things like generate the image pyramid and find FAST corners
00117 
00118   mCurrentKF.reset(new KeyFrame);
00119 
00120   mCurrentKF->mMeasurements.clear();
00121   mCurrentKF->MakeKeyFrame_Lite(imFrame);
00122 
00123   // Update the small images for the rotation estimator
00124   //Weiss{
00125   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00126   double gvdSBIBlur = pPars.RotationEstimatorBlur;
00127   int gvnUseSBI = pPars.UseRotationEstimator;
00128   //static gvar3<double> gvdSBIBlur("Tracker.RotationEstimatorBlur", 0.75, SILENT);
00129   //static gvar3<int> gvnUseSBI("Tracker.UseRotationEstimator", 1, SILENT);
00130   //}
00131   mbUseSBIInit = gvnUseSBI;
00132   if(!mpSBIThisFrame)
00133   {
00134     mpSBIThisFrame = new SmallBlurryImage(*mCurrentKF, gvdSBIBlur);
00135     mpSBILastFrame = new SmallBlurryImage(*mCurrentKF, gvdSBIBlur);
00136   }
00137   else
00138   {
00139     delete  mpSBILastFrame;
00140     mpSBILastFrame = mpSBIThisFrame;
00141     mpSBIThisFrame = new SmallBlurryImage(*mCurrentKF, gvdSBIBlur);
00142   }
00143 
00144   // From now on we only use the keyframe struct!
00145   mnFrame++;
00146 
00147   if(mbDraw)
00148   {
00149     glDrawPixels(mCurrentKF->aLevels[0].im);
00150     if(GV2.GetInt("Tracker.DrawFASTCorners",0, SILENT))
00151     {
00152       glColor3f(1,0,1);  glPointSize(1); glBegin(GL_POINTS);
00153       for(unsigned int i=0; i<mCurrentKF->aLevels[0].vCorners.size(); i++)
00154         glVertex(mCurrentKF->aLevels[0].vCorners[i]);
00155       glEnd();
00156     }
00157   }
00158 
00159   // Decide what to do - if there is a map, try to track the map ...
00160   if(mMap.IsGood())
00161   {
00162     if(mnLostFrames < 3)  // .. but only if we're not lost!
00163     {
00164       if(mbUseSBIInit)
00165         CalcSBIRotation();
00166       ApplyMotionModel();       //
00167       TrackMap();               //  These three lines do the main tracking work.
00168       UpdateMotionModel();      //
00169 
00170       AssessTrackingQuality();  //  Check if we're lost or if tracking is poor.
00171 
00172       { // Provide some feedback for the user:
00173         mMessageForUser << "Tracking Map, quality ";
00174         if(mTrackingQuality == GOOD)  mMessageForUser << "good.";
00175         if(mTrackingQuality == DODGY) mMessageForUser << "poor.";
00176         if(mTrackingQuality == BAD)   mMessageForUser << "bad.";
00177         mMessageForUser << " Found:";
00178         for(int i=0; i<LEVELS; i++) mMessageForUser << " " << manMeasFound[i] << "/" << manMeasAttempted[i];
00179         //          mMessageForUser << " Found " << mnMeasFound << " of " << mnMeasAttempted <<". (";
00180         mMessageForUser << " Map: " << mMap.vpPoints.size() << "P, " << mMap.vpKeyFrames.size() << "KF";
00181       }
00182 
00183       // Heuristics to check if a key-frame should be added to the map:
00184       if(mTrackingQuality == GOOD &&
00185           mMapMaker.NeedNewKeyFrame(mCurrentKF) &&
00186           //                                    mnFrame - mnLastKeyFrameDropped > 5  &&
00187           mMapMaker.QueueSize() < 5)
00188       {
00189         mMessageForUser << " Adding key-frame.";
00190         AddNewKeyFrame();
00191       };
00192 
00193     }
00194     else  // what if there is a map, but tracking has been lost?
00195     {
00196       mMessageForUser << "** Attempting recovery **.";
00197       if(AttemptRecovery())
00198       {
00199         TrackMap();
00200         AssessTrackingQuality();
00201       }
00202 
00203     }
00204     if(mbDraw)
00205       RenderGrid();
00206 
00207     //Weiss{
00208     if (mAutoreset)
00209       Reset();
00210     //}
00211   }
00212   else // If there is no map, try to make one.
00213     //Weiss{
00214   {
00215     
00216     int level = PtamParameters::fixparams().InitLevel;
00217     const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00218     if (pPars.AutoInit)
00219     {
00220       if(mnInitialStage == TRAIL_TRACKING_NOT_STARTED)
00221         mbUserPressedSpacebar=true;
00222       else if(mnInitialStage == TRAIL_TRACKING_STARTED)
00223       {
00224         // calc rotation aid of SBI
00225         SmallBlurryImage* pSBIsecond = new SmallBlurryImage(*mCurrentKF);
00226         Vector<3> rotvec = CalcSBIRotation(mFirstKF->pSBI,pSBIsecond);
00227         SO3<> rotmat = SO3<>::exp(rotvec);
00228         std::vector<double> mediandistvec;
00229 
00230         unsigned short k=0;
00231         std::vector<double> medianvec;
00232         medianvec.resize(mlTrails.size());
00233         for(list<Trail>::iterator i = mlTrails.begin(); i!=mlTrails.end();++i)
00234         {
00235           Trail &trail = *i;
00236           // compensate pixel disparity with rotation!!
00237           Vector<2> firstvec = LevelZeroPos(trail.irInitialPos, level);
00238           Vector<2> z1vec =  mCamera.UnProject(LevelZeroPos(trail.irCurrentPos, level));
00239           Vector<3> dirvec = makeVector(z1vec[0],z1vec[1],1);
00240           dirvec = rotmat.inverse()*dirvec;
00241           z1vec = mCamera.Project(makeVector(dirvec[0]/dirvec[2],dirvec[1]/dirvec[2]));
00242           mediandistvec.push_back(sqrt((z1vec[0]-firstvec[0])*(z1vec[0]-firstvec[0])+(z1vec[1]-firstvec[1])*(z1vec[1]-firstvec[1])));
00243           ++k;
00244         }
00245         delete pSBIsecond;
00246 
00247         if(k>0) // otherwise "if (mediandist>pPars.AutoInitPixel)" segfaults...
00248         {
00249           std::vector<double>::iterator first = mediandistvec.begin();
00250           std::vector<double>::iterator last = mediandistvec.end();
00251           std::vector<double>::iterator middle = first + std::floor((last - first) / 2);
00252           std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
00253           double mediandist = *middle;
00254           if (mediandist>pPars.AutoInitPixel)
00255             mbUserPressedSpacebar=true;
00256         }
00257       }
00258     }
00259     //}
00260     TrackForInitialMap();
00261   }
00262   //}
00263 
00264   // GUI interface
00265   while(!mvQueuedCommands.empty())
00266   {
00267     GUICommandHandler(mvQueuedCommands.begin()->sCommand, mvQueuedCommands.begin()->sParams);
00268     mvQueuedCommands.erase(mvQueuedCommands.begin());
00269   }
00270 };
00271 
00272 
00273 //Weiss{ returns a rodriguez vector of an estimated rotation between 2 SBIs
00274 Vector<3> Tracker::CalcSBIRotation(SmallBlurryImage *SBI1, SmallBlurryImage *SBI2)
00275 {
00276   SBI1->MakeJacs();
00277   pair<SE2<>, double> result_pair;
00278   result_pair = SBI2->IteratePosRelToTarget(*SBI1, 6);
00279   SE3<> se3Adjust = SmallBlurryImage::SE3fromSE2(result_pair.first, mCamera);
00280   return se3Adjust.ln().slice<3,3>();
00281 }
00282 //}
00283 
00284 
00285 // Try to relocalise in case tracking was lost.
00286 // Returns success or failure as a bool.
00287 // Actually, the SBI relocaliser will almost always return true, even if
00288 // it has no idea where it is, so graphics will go a bit 
00289 // crazy when lost. Could use a tighter SSD threshold and return more false,
00290 // but the way it is now gives a snappier response and I prefer it.
00291 bool Tracker::AttemptRecovery()
00292 {
00293   bool bRelocGood = mRelocaliser.AttemptRecovery(*mCurrentKF);
00294   if(!bRelocGood)
00295     return false;
00296 
00297   SE3<> se3Best = mRelocaliser.BestPose();
00298   mse3CamFromWorld = mse3StartPos = se3Best;
00299   mv6CameraVelocity = Zeros;
00300   mbJustRecoveredSoUseCoarse = true;
00301   return true;
00302 }
00303 
00304 // Draw the reference grid to give the user an idea of wether tracking is OK or not.
00305 void Tracker::RenderGrid()
00306 {
00307 
00308   ComputeGrid();
00309   int dim0 = mProjVertices.size().x;
00310   int dim1 = mProjVertices.size().y;
00311 
00312   // The colour of the ref grid shows if the coarse stage of tracking was used
00313   // (it's turned off when the camera is sitting still to reduce jitter.)
00314   if(mbDidCoarse)
00315     glColor4f(.0, 0.5, .0, 0.6);
00316   else
00317     glColor4f(0,0,0,0.6);
00318 
00319   glEnable(GL_LINE_SMOOTH);
00320   glEnable(GL_BLEND);
00321   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00322   glLineWidth(2);
00323   for(int i=0; i<dim0; i++)
00324   {
00325     glBegin(GL_LINE_STRIP);
00326     for(int j=0; j<dim1; j++)
00327       glVertex(mProjVertices[i][j]);
00328     glEnd();
00329 
00330     glBegin(GL_LINE_STRIP);
00331     for(int j=0; j<dim1; j++)
00332       glVertex(mProjVertices[j][i]);
00333     glEnd();
00334   };
00335 
00336   glLineWidth(1);
00337   glColor3f(1,0,0);
00338 }
00339 
00340 // Draw the reference grid to give the user an idea of wether tracking is OK or not.
00341 CVD::Image<TooN::Vector<2> > &  Tracker::ComputeGrid()
00342 {
00343   // The colour of the ref grid shows if the coarse stage of tracking was used
00344   // (it's turned off when the camera is sitting still to reduce jitter.)
00345   // The grid is projected manually, i.e. GL receives projected 2D coords to draw.
00346   int nHalfCells = 8;
00347   int nTot = nHalfCells * 2 + 1;
00348   //  Image<Vector<2> >  imVertices(ImageRef(nTot,nTot));
00349   mProjVertices.resize(ImageRef(nTot,nTot));
00350   for(int i=0; i<nTot; i++)
00351     for(int j=0; j<nTot; j++)
00352     {
00353       Vector<3> v3;
00354       v3[0] = (i - nHalfCells) * 0.1;
00355       v3[1] = (j - nHalfCells) * 0.1;
00356       v3[2] = 0.0;
00357       Vector<3> v3Cam = mse3CamFromWorld * v3;
00358       if(v3Cam[2] < 0.001)
00359         v3Cam[2] = 0.001;
00360       mProjVertices[i][j] = mCamera.Project(project(v3Cam));
00361     }
00362   return mProjVertices;
00363 }
00364 
00365 // GUI interface. Stuff commands onto the back of a queue so the tracker handles
00366 // them in its own thread at the end of each frame. Note the charming lack of
00367 // any thread safety (no lock on mvQueuedCommands).
00368 void Tracker::GUICommandCallBack(void* ptr, string sCommand, string sParams)
00369 {
00370   Command c;
00371   c.sCommand = sCommand;
00372   c.sParams = sParams;
00373   ((Tracker*) ptr)->mvQueuedCommands.push_back(c);
00374 }
00375 
00376 // This is called in the tracker's own thread.
00377 void Tracker::GUICommandHandler(string sCommand, string sParams)  // Called by the callback func..
00378 {
00379   if(sCommand=="Reset")
00380   {
00381     Reset();
00382     return;
00383   }
00384 
00385   // KeyPress commands are issued by GLWindow
00386   if(sCommand=="KeyPress")
00387   {
00388     if(sParams == "Space")
00389     {
00390       mbUserPressedSpacebar = true;
00391     }
00392     else if(sParams == "r")
00393     {
00394       mAutoreset=false;
00395       ROS_WARN_STREAM("Forcing map reset because of user input! Autoreset set to " << mAutoreset);
00396       Reset();
00397     }
00398     else if(sParams == "a")     // autoreset test button
00399     {
00400       mAutoreset=true;
00401       ROS_WARN_STREAM("Forcing map reset because of user input! Autoreset set to " << mAutoreset);
00402       Reset();
00403     }
00404     else if(sParams == "q" || sParams == "Escape")
00405     {
00406       GUI.ParseLine("quit");
00407     }
00408     return;
00409   }
00410   if((sCommand=="PokeTracker"))
00411   {
00412     mbUserPressedSpacebar = true;
00413     return;
00414   }
00415 
00416 
00417   cout << "! Tracker::GUICommandHandler: unhandled command "<< sCommand << endl;
00418   exit(1);
00419 }; 
00420 
00421 // Routine for establishing the initial map. This requires two spacebar presses from the user
00422 // to define the first two key-frames. Salient points are tracked between the two keyframes
00423 // using cheap frame-to-frame tracking (which is very brittle - quick camera motion will
00424 // break it.) The salient points are stored in a list of `Trail' data structures.
00425 // What action TrackForInitialMap() takes depends on the mnInitialStage enum variable..
00426 void Tracker::TrackForInitialMap()
00427 {
00428   // MiniPatch tracking threshhold.
00429   //Weiss{
00430   
00431   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00432   int gvnMaxSSD = pPars.MiniPatchMaxSSD;
00433   //static gvar3<int> gvnMaxSSD("Tracker.MiniPatchMaxSSD", 100000, SILENT);
00434   int level = PtamParameters::fixparams().InitLevel;
00435   ImageRef tmpInit, tmpCurr;
00436   //}
00437   MiniPatch::mnMaxSSD = gvnMaxSSD;
00438 
00439   // What stage of initial tracking are we at?
00440   if(mnInitialStage == TRAIL_TRACKING_NOT_STARTED)
00441   {
00442     if(mbUserPressedSpacebar)  // First spacebar = this is the first keyframe
00443     {
00444       mbUserPressedSpacebar = false;
00445       TrailTracking_Start();
00446       mnInitialStage = TRAIL_TRACKING_STARTED;
00447     }
00448     else
00449       mMessageForUser << "Point camera at planar scene and press spacebar to start tracking for initial map." << endl;
00450     return;
00451   };
00452 
00453   if(mnInitialStage == TRAIL_TRACKING_STARTED)
00454   {
00455     int nGoodTrails = TrailTracking_Advance();  // This call actually tracks the trails
00456     if(nGoodTrails < 10) // if most trails have been wiped out, no point continuing.
00457     {
00458       Reset();
00459       return;
00460     }
00461 
00462     // If the user pressed spacebar here, use trails to run stereo and make the intial map..
00463     if(mbUserPressedSpacebar)
00464     {
00465       mbUserPressedSpacebar = false;
00466       vector<pair<ImageRef, ImageRef> > vMatches;   // This is the format the mapmaker wants for the stereo pairs
00467       for(list<Trail>::iterator i = mlTrails.begin(); i!=mlTrails.end(); i++){
00468         tmpInit.x = LevelZeroPos(i->irInitialPos.x, level);
00469         tmpInit.y = LevelZeroPos(i->irInitialPos.y, level);
00470         tmpCurr.x = LevelZeroPos(i->irCurrentPos.x, level);
00471         tmpCurr.y = LevelZeroPos(i->irCurrentPos.y, level);
00472 
00473         vMatches.push_back(pair<ImageRef, ImageRef>(tmpInit, tmpCurr));
00474       }
00475 
00476       //Weiss{
00477       //mMapMaker.InitFromStereo(mFirstKF, mCurrentKF, vMatches, mse3CamFromWorld);  // This will take some time!
00478 
00479       bool initret=false;
00480       initret=mMapMaker.InitFromStereo(mFirstKF, mCurrentKF, vMatches, mse3CamFromWorld);  // This will take some time!
00481 
00482       if(initret)
00483       {
00484         if (mAutoreset)
00485         {
00486           std::vector<double> medianvec1, medianvec2;
00487 
00488 
00489           for(unsigned int ip=0;ip<mMap.vpPoints.size();ip++)
00490           {
00491             //                                          if(mMap.vpPoints[ip]->pPatchSourceKF==mMap.vpKeyFrames[0])
00492             medianvec1.push_back((mMap.vpKeyFrames[0]->se3CfromW*mMap.vpPoints[ip]->v3WorldPos)[2]);
00493             //                                          else //if(mMap.vpPoints[ip]->pPatchSourceKF==mMap.vpKeyFrames[1])
00494             medianvec2.push_back((mMap.vpKeyFrames[1]->se3CfromW*mMap.vpPoints[ip]->v3WorldPos)[2]);
00495           }
00496           std::vector<double>::iterator first = medianvec1.begin();
00497           std::vector<double>::iterator last = medianvec1.end();
00498           std::vector<double>::iterator middle = first + std::floor((last - first) / 2);
00499           std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
00500           mMap.vpKeyFrames[0]->dSceneDepthMedian = *middle;
00501 
00502           first = medianvec2.begin();
00503           last = medianvec2.end();
00504           middle = first + std::floor((last - first) / 2);
00505           std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
00506           mMap.vpKeyFrames[1]->dSceneDepthMedian = *middle;
00507 
00508           double scale = mOldKF->dSceneDepthMedian/mMap.vpKeyFrames[0]->dSceneDepthMedian;
00509           mMapMaker.ApplyGlobalScaleToMap(scale);
00510           SE3<> transf = mOldKF->se3CfromW.inverse()*mMap.vpKeyFrames[0]->se3CfromW;
00511           mMapMaker.ApplyGlobalTransformationToMap(transf);
00512           mse3CamFromWorld.get_translation() = mse3CamFromWorld.get_translation()*scale;
00513           mse3CamFromWorld = mse3CamFromWorld*transf.inverse();
00514 
00515           mAutoreset=false;
00516         }
00517         mnInitialStage = TRAIL_TRACKING_COMPLETE;
00518 
00519         ROS_INFO_STREAM("Initialized map on level "<<level);
00520 
00521       }
00522       else
00523       {
00524         Reset();
00525         return;
00526       }
00527       //}
00528     }
00529     else
00530       mMessageForUser << "Translate the camera slowly sideways, and press spacebar again to perform stereo init." << endl;
00531   }
00532 }
00533 
00534 // The current frame is to be the first keyframe!
00535 void Tracker::TrailTracking_Start()
00536 {
00537   mCurrentKF->MakeKeyFrame_Rest();  // This populates the Candidates list, which is Shi-Tomasi thresholded.
00538 
00539   mFirstKF.reset(new KeyFrame); //TODO check if we need to copy the data, or
00540   *mFirstKF = *mCurrentKF; //copy data
00541 
00542   vector<pair<double,ImageRef> > vCornersAndSTScores;
00543 
00544   
00545   vCornersAndSTScores.reserve(1000);
00546   int level = PtamParameters::fixparams().InitLevel;
00547   for(unsigned int i=0; i<mCurrentKF->aLevels[level].vCandidates.size(); i++)  // Copy candidates into a trivially sortable vector
00548   {                                                                     // so that we can choose the image corners with max ST score
00549     Candidate &c = mCurrentKF->aLevels[level].vCandidates[i];
00550     if(!mCurrentKF->aLevels[level].im.in_image_with_border(c.irLevelPos, MiniPatch::mnHalfPatchSize))
00551       continue;
00552     vCornersAndSTScores.push_back(pair<double,ImageRef>(-1.0 * c.dSTScore, c.irLevelPos)); // negative so highest score first in sorted list
00553   };
00554   sort(vCornersAndSTScores.begin(), vCornersAndSTScores.end());  // Sort according to Shi-Tomasi score
00555   int nToAdd = GV2.GetInt("MaxInitialTrails", 1000, SILENT);
00556   for(unsigned int i = 0; i<vCornersAndSTScores.size() && nToAdd > 0; i++)
00557   {
00558     if(!mCurrentKF->aLevels[level].im.in_image_with_border(vCornersAndSTScores[i].second, MiniPatch::mnHalfPatchSize))
00559       continue;
00560     Trail t;
00561     t.mPatch.SampleFromImage(vCornersAndSTScores[i].second, mCurrentKF->aLevels[level].im);
00562     t.irInitialPos = vCornersAndSTScores[i].second;
00563     t.irCurrentPos = t.irInitialPos;
00564     mlTrails.push_back(t);
00565     nToAdd--;
00566   }
00567   mPreviousFrameKF.reset(new KeyFrame);
00568   *mPreviousFrameKF = *mFirstKF;  // Always store the previous frame so married-matching can work.
00569 }
00570 
00571 // Steady-state trail tracking: Advance from the previous frame, remove duds.
00572 int Tracker::TrailTracking_Advance()
00573 {
00574   int nGoodTrails = 0;
00575   if(mbDraw)
00576   {
00577     glPointSize(5);
00578     glLineWidth(2);
00579     glEnable(GL_POINT_SMOOTH);
00580     glEnable(GL_LINE_SMOOTH);
00581     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00582     glEnable(GL_BLEND);
00583     glBegin(GL_LINES);
00584   }
00585   
00586   int level = PtamParameters::fixparams().InitLevel;
00587   MiniPatch BackwardsPatch;
00588   Level &lCurrentFrame = mCurrentKF->aLevels[level];
00589   Level &lPreviousFrame = mPreviousFrameKF->aLevels[level];
00590 
00591 
00592   for(list<Trail>::iterator i = mlTrails.begin(); i!=mlTrails.end();)
00593   {
00594     list<Trail>::iterator next = i; next++;
00595 
00596     Trail &trail = *i;
00597     ImageRef irStart = trail.irCurrentPos;
00598     ImageRef irEnd = irStart;
00599     bool bFound = trail.mPatch.FindPatch(irEnd, lCurrentFrame.im, 10, lCurrentFrame.vCorners);
00600     if(bFound)
00601     {
00602       // Also find backwards in a married-matches check
00603       BackwardsPatch.SampleFromImage(irEnd, lCurrentFrame.im);
00604       ImageRef irBackWardsFound = irEnd;
00605       bFound = BackwardsPatch.FindPatch(irBackWardsFound, lPreviousFrame.im, 10, lPreviousFrame.vCorners);
00606       if((irBackWardsFound - irStart).mag_squared() > 2)
00607         bFound = false;
00608 
00609       trail.irCurrentPos = irEnd;
00610       nGoodTrails++;
00611     }
00612     if(mbDraw)
00613     {
00614       if(!bFound)
00615         glColor3f(0,1,1); // Failed trails flash purple before dying.
00616       else
00617         glColor3f(1,1,0);
00618       glVertex(LevelZeroPos(trail.irInitialPos, level));
00619       if(bFound) glColor3f(1,0,0);
00620       glVertex(LevelZeroPos(trail.irCurrentPos, level));
00621     }
00622     if(!bFound) // Erase from list of trails if not found this frame.
00623     {
00624       mlTrails.erase(i);
00625     }
00626     i = next;
00627   }
00628   if(mbDraw)
00629     glEnd();
00630 
00631   mPreviousFrameKF = mCurrentKF;
00632   return nGoodTrails;
00633 }
00634 
00635 // TrackMap is the main purpose of the Tracker.
00636 // It first projects all map points into the image to find a potentially-visible-set (PVS);
00637 // Then it tries to find some points of the PVS in the image;
00638 // Then it updates camera pose according to any points found.
00639 // Above may happen twice if a coarse tracking stage is performed.
00640 // Finally it updates the tracker's current-frame-KeyFrame struct with any
00641 // measurements made.
00642 // A lot of low-level functionality is split into helper classes:
00643 // class TrackerData handles the projection of a MapPoint and stores intermediate results;
00644 // class PatchFinder finds a projected MapPoint in the current-frame-KeyFrame.
00645 void Tracker::TrackMap()
00646 {
00647   // Some accounting which will be used for tracking quality assessment:
00648   for(int i=0; i<LEVELS; i++)
00649     manMeasAttempted[i] = manMeasFound[i] = 0;
00650 
00651   // The Potentially-Visible-Set (PVS) is split into pyramid levels.
00652   vector<TrackerData*> avPVS[LEVELS];
00653   for(int i=0; i<LEVELS; i++)
00654     avPVS[i].reserve(500);
00655 
00656   //slynen{ reprojection keyfeatures
00657 #ifdef KF_REPROJ
00658   //possibly visible Keyframes
00659   vector<KeyFrame::Ptr> vpPVKeyFrames;
00660   //for all keyframes
00661   if(mTrackingQuality == GOOD){ //if tracking quality is not good, we use all KF
00662     for(unsigned int k=0; k<mMap.vpKeyFrames.size(); k++)
00663     {   //project best points
00664       int foundKFPoints = 0;
00665       if (!mMap.vpKeyFrames.at(k)->vpPoints.size()) continue; //if this keyframe doesn't yet contain any points
00666 
00667       for(unsigned int j=0; j<mMap.vpKeyFrames.at(k)->iBestPointsCount; j++)
00668       {
00669         if(mMap.vpKeyFrames.at(k)->apCurrentBestPoints[j]==NULL) continue;
00670         MapPoint::Ptr p = (mMap.vpKeyFrames.at(k)->apCurrentBestPoints[j]);
00671         // Ensure that this map point has an associated TrackerData struct.
00672         if(!p->pTData) p->pTData = new TrackerData(p);
00673         TrackerData &TData = *p->pTData;
00674 
00675         // Project according to current view, and if it's not in the image, skip.
00676         TData.Project(mse3CamFromWorld, mCamera);
00677         if(TData.bInImage)
00678           foundKFPoints++;
00679       };
00680       //have at least some points of this keyframe been found?
00681       if(foundKFPoints>1) vpPVKeyFrames.push_back(mMap.vpKeyFrames.at(k));
00682     };
00683   };
00684   //if we didn't find any Keyframes or tracking quality is bad
00685   //we fall back to reprojecting all keyframes, thus all points
00686   if (vpPVKeyFrames.size() < 1) {
00687     for(unsigned int k=0; k<mMap.vpKeyFrames.size(); k++){
00688       vpPVKeyFrames.push_back(mMap.vpKeyFrames.at(k));
00689     }
00690   }
00691   for(unsigned int k=0; k<vpPVKeyFrames.size(); k++){//for all possibly visible keyframes
00692     for(unsigned int i=0; i<vpPVKeyFrames.at(k)->vpPoints.size(); i++)// For all points in the visible keyframes..
00693     {
00694       MapPoint::Ptr p= (vpPVKeyFrames.at(k)->vpPoints.at(i));
00695       if(p->bAlreadyProjected) continue;//check whether we already projected that point from another KF
00696       p->bAlreadyProjected=true;
00697       if(!p->pTData)
00698         p->pTData = new TrackerData(p);// Ensure that this map point has an associated TrackerData struct.
00699       TrackerData &TData = *p->pTData;
00700       TData.Project(mse3CamFromWorld, mCamera);// Project according to current view, and if it's not in the image, skip.
00701       if(!TData.bInImage)
00702         continue;
00703 
00704       TData.GetDerivsUnsafe(mCamera);// Calculate camera projection derivatives of this point.
00705 
00706       // And check what the PatchFinder (included in TrackerData) makes of the mappoint in this view..
00707 
00708       TData.nSearchLevel = TData.Finder.CalcSearchLevelAndWarpMatrix(TData.Point, mse3CamFromWorld, TData.m2CamDerivs);
00709 
00710       if(TData.nSearchLevel == -1)
00711         continue;   // a negative search pyramid level indicates an inappropriate warp for this view, so skip.
00712 
00713       // Otherwise, this point is suitable to be searched in the current image! Add to the PVS.
00714       TData.bSearched = false;
00715       TData.bFound = false;
00716       avPVS[TData.nSearchLevel].push_back(&TData);
00717     };
00718   };
00719 
00720   //reset alreadyprojected marker
00721   for(unsigned int k=0; k<vpPVKeyFrames.size(); k++)
00722     for(unsigned int i=0; i<vpPVKeyFrames.at(k)->vpPoints.size(); i++)
00723       vpPVKeyFrames.at(k)->vpPoints.at(i)->bAlreadyProjected = false;
00724 #else
00725 
00726   // For all points in the map..
00727   for(unsigned int i=0; i<mMap.vpPoints.size(); i++)
00728   {
00729     MapPoint &p= *(mMap.vpPoints[i]);
00730     // Ensure that this map point has an associated TrackerData struct.
00731     if(!p.pTData) p.pTData = new TrackerData(&p);
00732     TrackerData &TData = *p.pTData;
00733 
00734     // Project according to current view, and if it's not in the image, skip.
00735     TData.Project(mse3CamFromWorld, mCamera);
00736     if(!TData.bInImage)
00737       continue;
00738 
00739     // Calculate camera projection derivatives of this point.
00740     TData.GetDerivsUnsafe(mCamera);
00741 
00742     // And check what the PatchFinder (included in TrackerData) makes of the mappoint in this view..
00743     TData.nSearchLevel = TData.Finder.CalcSearchLevelAndWarpMatrix(TData.Point, mse3CamFromWorld, TData.m2CamDerivs);
00744     if(TData.nSearchLevel == -1)
00745       continue;   // a negative search pyramid level indicates an inappropriate warp for this view, so skip.
00746 
00747     // Otherwise, this point is suitable to be searched in the current image! Add to the PVS.
00748     TData.bSearched = false;
00749     TData.bFound = false;
00750     avPVS[TData.nSearchLevel].push_back(&TData);
00751   };
00752   //slynen{ reprojection
00753 #endif
00754   //}
00755 
00756   // Next: A large degree of faffing about and deciding which points are going to be measured!
00757   // First, randomly shuffle the individual levels of the PVS.
00758   for(int i=0; i<LEVELS; i++)
00759     random_shuffle(avPVS[i].begin(), avPVS[i].end());
00760 
00761   // The next two data structs contain the list of points which will next
00762   // be searched for in the image, and then used in pose update.
00763   vector<TrackerData*> vNextToSearch;
00764   vector<TrackerData*> vIterationSet;
00765 
00766   // Tunable parameters to do with the coarse tracking stage:
00767 
00768   //Weiss{
00769   
00770   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00771   unsigned int gvnCoarseMin = pPars.CoarseMin;
00772   unsigned int gvnCoarseMax = pPars.CoarseMax;
00773   unsigned int gvnCoarseRange = pPars.CoarseRange;
00774   int gvnCoarseSubPixIts = pPars.CoarseSubPixIts;
00775   int gvnCoarseDisabled = pPars.DisableCoarse;
00776   double gvdCoarseMinVel = pPars.CoarseMinVelocity;
00777   //static gvar3<unsigned int> gvnCoarseMin("Tracker.CoarseMin", 20, SILENT);   // Min number of large-scale features for coarse stage
00778   //static gvar3<unsigned int> gvnCoarseMax("Tracker.CoarseMax", 60, SILENT);   // Max number of large-scale features for coarse stage
00779   //static gvar3<unsigned int> gvnCoarseRange("Tracker.CoarseRange", 30, SILENT);       // Pixel search radius for coarse features
00780   //static gvar3<int> gvnCoarseSubPixIts("Tracker.CoarseSubPixIts", 8, SILENT); // Max sub-pixel iterations for coarse features
00781   //static gvar3<int> gvnCoarseDisabled("Tracker.DisableCoarse", 0, SILENT);    // Set this to 1 to disable coarse stage (except after recovery)
00782   //static gvar3<double> gvdCoarseMinVel("Tracker.CoarseMinVelocity", 0.006, SILENT);  // Speed above which coarse stage is used.
00783   //}
00784   unsigned int nCoarseMax = gvnCoarseMax;
00785   unsigned int nCoarseRange = gvnCoarseRange;
00786 
00787   mbDidCoarse = false;
00788 
00789   // Set of heuristics to check if we should do a coarse tracking stage.
00790   bool bTryCoarse = true;
00791   if(gvnCoarseDisabled ||
00792       mdMSDScaledVelocityMagnitude < gvdCoarseMinVel  ||
00793       nCoarseMax == 0)
00794     bTryCoarse = false;
00795   if(mbJustRecoveredSoUseCoarse)
00796   {
00797     bTryCoarse = true;
00798     nCoarseMax *=2;
00799     nCoarseRange *=2;
00800     mbJustRecoveredSoUseCoarse = false;
00801   };
00802 
00803   // If we do want to do a coarse stage, also check that there's enough high-level
00804   // PV map points. We use the lowest-res two pyramid levels (LEVELS-1 and LEVELS-2),
00805   // with preference to LEVELS-1.
00806   if(bTryCoarse && avPVS[LEVELS-1].size() + avPVS[LEVELS-2].size() > gvnCoarseMin )
00807   {
00808     // Now, fill the vNextToSearch struct with an appropriate number of
00809     // TrackerDatas corresponding to coarse map points! This depends on how many
00810     // there are in different pyramid levels compared to CoarseMin and CoarseMax.
00811 
00812     if(avPVS[LEVELS-1].size() <= nCoarseMax)
00813     { // Fewer than CoarseMax in LEVELS-1? then take all of them, and remove them from the PVS list.
00814       vNextToSearch = avPVS[LEVELS-1];
00815       avPVS[LEVELS-1].clear();
00816     }
00817     else
00818     { // ..otherwise choose nCoarseMax at random, again removing from the PVS list.
00819       for(unsigned int i=0; i<nCoarseMax; i++)
00820         vNextToSearch.push_back(avPVS[LEVELS-1][i]);
00821       avPVS[LEVELS-1].erase(avPVS[LEVELS-1].begin(), avPVS[LEVELS-1].begin() + nCoarseMax);
00822     }
00823 
00824     // If didn't source enough from LEVELS-1, get some from LEVELS-2... same as above.
00825     if(vNextToSearch.size() < nCoarseMax)
00826     {
00827       unsigned int nMoreCoarseNeeded = nCoarseMax - vNextToSearch.size();
00828       if(avPVS[LEVELS-2].size() <= nMoreCoarseNeeded)
00829       {
00830         vNextToSearch = avPVS[LEVELS-2];
00831         avPVS[LEVELS-2].clear();
00832       }
00833       else
00834       {
00835         for(unsigned int i=0; i<nMoreCoarseNeeded; i++)
00836           vNextToSearch.push_back(avPVS[LEVELS-2][i]);
00837         avPVS[LEVELS-2].erase(avPVS[LEVELS-2].begin(), avPVS[LEVELS-2].begin() + nMoreCoarseNeeded);
00838       }
00839     }
00840     // Now go and attempt to find these points in the image!
00841     unsigned int nFound = SearchForPoints(vNextToSearch, nCoarseRange, gvnCoarseSubPixIts);
00842     vIterationSet = vNextToSearch;  // Copy over into the to-be-optimised list.
00843     if(nFound >= gvnCoarseMin)  // Were enough found to do any meaningful optimisation?
00844     {
00845       mbDidCoarse = true;
00846       for(int iter = 0; iter<10; iter++) // If so: do ten Gauss-Newton pose updates iterations.
00847       {
00848         if(iter != 0)
00849         { // Re-project the points on all but the first iteration.
00850           for(unsigned int i=0; i<vIterationSet.size(); i++)
00851             if(vIterationSet[i]->bFound)
00852               vIterationSet[i]->ProjectAndDerivs(mse3CamFromWorld, mCamera);
00853         }
00854         for(unsigned int i=0; i<vIterationSet.size(); i++)
00855           if(vIterationSet[i]->bFound)
00856             vIterationSet[i]->CalcJacobian();
00857         double dOverrideSigma = 0.0;
00858         // Hack: force the MEstimator to be pretty brutal
00859         // with outliers beyond the fifth iteration.
00860         if(iter > 5)
00861           dOverrideSigma = 1.0;
00862 
00863         // Calculate and apply the pose update...
00864         Vector<6> v6Update =
00865             CalcPoseUpdate(vIterationSet, dOverrideSigma);
00866         mse3CamFromWorld = SE3<>::exp(v6Update) * mse3CamFromWorld;
00867       };
00868     }
00869   };
00870 
00871   // So, at this stage, we may or may not have done a coarse tracking stage.
00872   // Now do the fine tracking stage. This needs many more points!
00873 
00874   int nFineRange = 10;  // Pixel search range for the fine stage.
00875   if(mbDidCoarse)       // Can use a tighter search if the coarse stage was already done.
00876     nFineRange = 5;
00877 
00878   // What patches shall we use this time? The high-level ones are quite important,
00879   // so do all of these, with sub-pixel refinement.
00880   {
00881     int l = LEVELS - 1;
00882     for(unsigned int i=0; i<avPVS[l].size(); i++)
00883       avPVS[l][i]->ProjectAndDerivs(mse3CamFromWorld, mCamera);
00884     SearchForPoints(avPVS[l], nFineRange, 8);
00885     for(unsigned int i=0; i<avPVS[l].size(); i++)
00886       vIterationSet.push_back(avPVS[l][i]);  // Again, plonk all searched points onto the (maybe already populate) vIterationSet.
00887   };
00888 
00889   // All the others levels: Initially, put all remaining potentially visible patches onto vNextToSearch.
00890   vNextToSearch.clear();
00891   for(int l=LEVELS - 2; l>=0; l--)
00892     for(unsigned int i=0; i<avPVS[l].size(); i++)
00893       vNextToSearch.push_back(avPVS[l][i]);
00894 
00895   // But we haven't got CPU to track _all_ patches in the map - arbitrarily limit
00896   // ourselves to 1000, and choose these randomly.
00897   //Weiss{
00898   int gvnMaxPatchesPerFrame = pPars.MaxPatchesPerFrame;
00899   //static gvar3<int> gvnMaxPatchesPerFrame("Tracker.MaxPatchesPerFrame", 1000, SILENT);
00900   //}
00901   int nFinePatchesToUse = gvnMaxPatchesPerFrame - vIterationSet.size();
00902   if(((int) vNextToSearch.size() > nFinePatchesToUse) && (nFinePatchesToUse>0))
00903   {
00904     random_shuffle(vNextToSearch.begin(), vNextToSearch.end());
00905     vNextToSearch.resize(nFinePatchesToUse); // Chop!
00906   };
00907 
00908   // If we did a coarse tracking stage: re-project and find derivs of fine points
00909   if(mbDidCoarse)
00910     for(unsigned int i=0; i<vNextToSearch.size(); i++)
00911       vNextToSearch[i]->ProjectAndDerivs(mse3CamFromWorld, mCamera);
00912 
00913   // Find fine points in image:
00914   SearchForPoints(vNextToSearch, nFineRange, 0);
00915   // And attach them all to the end of the optimisation-set.
00916   for(unsigned int i=0; i<vNextToSearch.size(); i++)
00917     vIterationSet.push_back(vNextToSearch[i]);
00918 
00919   // Again, ten gauss-newton pose update iterations.
00920   Vector<6> v6LastUpdate;
00921   v6LastUpdate = Zeros;
00922   for(int iter = 0; iter<10; iter++)
00923   {
00924     bool bNonLinearIteration; // For a bit of time-saving: don't do full nonlinear
00925     // reprojection at every iteration - it really isn't necessary!
00926     if(iter == 0 || iter == 4 || iter == 9)
00927       bNonLinearIteration = true;   // Even this is probably overkill, the reason we do many
00928     else                            // iterations is for M-Estimator convergence rather than
00929       bNonLinearIteration = false;  // linearisation effects.
00930 
00931     if(iter != 0)   // Either way: first iteration doesn't need projection update.
00932     {
00933       if(bNonLinearIteration)
00934       {
00935         for(unsigned int i=0; i<vIterationSet.size(); i++)
00936           if(vIterationSet[i]->bFound)
00937             vIterationSet[i]->ProjectAndDerivs(mse3CamFromWorld, mCamera);
00938       }
00939       else
00940       {
00941         for(unsigned int i=0; i<vIterationSet.size(); i++)
00942           if(vIterationSet[i]->bFound)
00943             vIterationSet[i]->LinearUpdate(v6LastUpdate);
00944       };
00945     }
00946 
00947     if(bNonLinearIteration)
00948       for(unsigned int i=0; i<vIterationSet.size(); i++)
00949         if(vIterationSet[i]->bFound)
00950           vIterationSet[i]->CalcJacobian();
00951 
00952     // Again, an M-Estimator hack beyond the fifth iteration.
00953     double dOverrideSigma = 0.0;
00954     if(iter > 5)
00955       dOverrideSigma = 16.0;
00956 
00957     // Calculate and update pose; also store update vector for linear iteration updates.
00958     Vector<6> v6Update =
00959         CalcPoseUpdate(vIterationSet, dOverrideSigma, iter==9);
00960     mse3CamFromWorld = SE3<>::exp(v6Update) * mse3CamFromWorld;
00961     v6LastUpdate = v6Update;
00962   };
00963 
00964   if(mbDraw)
00965   {
00966     glPointSize(6);
00967     glEnable(GL_BLEND);
00968     glEnable(GL_POINT_SMOOTH);
00969     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00970     glBegin(GL_POINTS);
00971     for(vector<TrackerData*>::reverse_iterator it = vIterationSet.rbegin();
00972         it!= vIterationSet.rend();
00973         it++)
00974     {
00975       if(! (*it)->bFound)
00976         continue;
00977       glColor(gavLevelColors[(*it)->nSearchLevel]);
00978       glVertex((*it)->v2Image);
00979     }
00980     glEnd();
00981     glDisable(GL_BLEND);
00982   }
00983 
00984   // Update the current keyframe with info on what was found in the frame.
00985   // Strictly speaking this is unnecessary to do every frame, it'll only be
00986   // needed if the KF gets added to MapMaker. Do it anyway.
00987   // Export pose to current keyframe:
00988   mCurrentKF->se3CfromW = mse3CamFromWorld;
00989 
00990   // Record successful measurements. Use the KeyFrame-Measurement struct for this.
00991   mCurrentKF->mMeasurements.clear();
00992   for(vector<TrackerData*>::iterator it = vIterationSet.begin();
00993       it!= vIterationSet.end();
00994       it++)
00995   {
00996     if(! (*it)->bFound)
00997       continue;
00998     Measurement m;
00999     m.Source = Measurement::SRC_TRACKER;
01000     m.v2RootPos = (*it)->v2Found;
01001     m.nLevel = (*it)->nSearchLevel;
01002     m.bSubPix = (*it)->bDidSubPix;
01003     mCurrentKF->mMeasurements[((*it)->Point)] = m;
01004   }
01005 
01006   // Finally, find the mean scene depth from tracked features
01007   {
01008     double dSum = 0;
01009     double dSumSq = 0;
01010     int nNum = 0;
01011     std::vector<double> buffvec;
01012     for(vector<TrackerData*>::iterator it = vIterationSet.begin();
01013         it!= vIterationSet.end();
01014         it++)
01015       if((*it)->bFound)
01016       {
01017         double z = (*it)->v3Cam[2];
01018         dSum+= z;
01019         dSumSq+= z*z;
01020         //Weiss{
01021         buffvec.push_back(z);
01022         //}
01023         nNum++;
01024       };
01025     if(nNum > 20)
01026     {
01027       mCurrentKF->dSceneDepthMean = dSum/nNum;
01028       mCurrentKF->dSceneDepthSigma = sqrt((dSumSq / nNum) - (mCurrentKF->dSceneDepthMean) * (mCurrentKF->dSceneDepthMean));
01029       //Weiss{
01030       std::vector<double>::iterator first = buffvec.begin();
01031       std::vector<double>::iterator last = buffvec.end();
01032       std::vector<double>::iterator middle = first + std::floor((last - first) / 2);
01033       std::nth_element(first, middle, last); // can specify comparator as optional 4th arg
01034       mCurrentKF->dSceneDepthMedian = *middle;
01035       //}
01036     }
01037   }
01038 }
01039 
01040 // Find points in the image. Uses the PatchFiner struct stored in TrackerData
01041 int Tracker::SearchForPoints(vector<TrackerData*> &vTD, int nRange, int nSubPixIts)
01042 {
01043   int nFound = 0;
01044   for(unsigned int i=0; i<vTD.size(); i++)   // for each point..
01045   {
01046     // First, attempt a search at pixel locations which are FAST corners.
01047     // (PatchFinder::FindPatchCoarse)
01048     TrackerData &TD = *vTD[i];
01049     PatchFinder &Finder = TD.Finder;
01050     Finder.MakeTemplateCoarseCont(TD.Point);
01051     if(Finder.TemplateBad())
01052     {
01053       TD.bInImage = TD.bPotentiallyVisible = TD.bFound = false;
01054       continue;
01055     }
01056     manMeasAttempted[Finder.GetLevel()]++;  // Stats for tracking quality assessmenta
01057 
01058     bool bFound =
01059         Finder.FindPatchCoarse(ir(TD.v2Image), mCurrentKF, nRange);
01060     TD.bSearched = true;
01061     if(!bFound)
01062     {
01063       TD.bFound = false;
01064       continue;
01065     }
01066 
01067     TD.bFound = true;
01068     TD.dSqrtInvNoise = (1.0 / Finder.GetLevelScale());
01069 
01070     nFound++;
01071     manMeasFound[Finder.GetLevel()]++;
01072 
01073     // Found the patch in coarse search - are Sub-pixel iterations wanted too?
01074     if(nSubPixIts > 0)
01075     {
01076       TD.bDidSubPix = true;
01077       Finder.MakeSubPixTemplate();
01078       bool bSubPixConverges=Finder.IterateSubPixToConvergence(*mCurrentKF, nSubPixIts);
01079       if(!bSubPixConverges)
01080       { // If subpix doesn't converge, the patch location is probably very dubious!
01081         TD.bFound = false;
01082         nFound--;
01083         manMeasFound[Finder.GetLevel()]--;
01084         continue;
01085       }
01086       TD.v2Found = Finder.GetSubPixPos();
01087     }
01088     else
01089     {
01090       TD.v2Found = Finder.GetCoarsePosAsVector();
01091       TD.bDidSubPix = false;
01092     }
01093   }
01094 
01095   return nFound;
01096 };
01097 
01098 //Calculate a pose update 6-vector from a bunch of image measurements.
01099 //User-selectable M-Estimator.
01100 //Normally this robustly estimates a sigma-squared for all the measurements
01101 //to reduce outlier influence, but this can be overridden if
01102 //dOverrideSigma is positive. Also, bMarkOutliers set to true
01103 //records any instances of a point being marked an outlier measurement
01104 //by the Tukey MEstimator.
01105 Vector<6> Tracker::CalcPoseUpdate(vector<TrackerData*> vTD, double dOverrideSigma, bool bMarkOutliers)
01106 {
01107         // Which M-estimator are we using?
01108         int nEstimator = 0;
01109         //Weiss{
01110         //static std::string gvsEstimator = "Tukey";
01111         const FixParams& pPars = PtamParameters::fixparams();
01112         static std::string gvsEstimator = pPars.TrackerMEstimator;
01113         //}
01114         if(gvsEstimator == "Tukey")
01115                 nEstimator = 0;
01116         else if(gvsEstimator == "Cauchy")
01117                 nEstimator = 1;
01118         else if(gvsEstimator == "Huber")
01119                 nEstimator = 2;
01120         else
01121         {
01122                 cout << "Invalid TrackerMEstimator, choices are Tukey, Cauchy, Huber" << endl;
01123                 nEstimator = 0;
01124                 gvsEstimator = "Tukey";
01125         };
01126 
01127         // Find the covariance-scaled reprojection error for each measurement.
01128         // Also, store the square of these quantities for M-Estimator sigma squared estimation.
01129         vector<double> vdErrorSquared;
01130         for(unsigned int f=0; f<vTD.size(); f++)
01131         {
01132                 TrackerData &TD = *vTD[f];
01133                 if(!TD.bFound)
01134                         continue;
01135                 TD.v2Error_CovScaled = TD.dSqrtInvNoise* (TD.v2Found - TD.v2Image);
01136                 vdErrorSquared.push_back(static_cast<double>(TD.v2Error_CovScaled * TD.v2Error_CovScaled));
01137         };
01138 
01139         // No valid measurements? Return null update.
01140         if(vdErrorSquared.size() == 0)
01141                 return makeVector( 0,0,0,0,0,0);
01142 
01143         // What is the distribution of errors?
01144         double dSigmaSquared;
01145         if(dOverrideSigma > 0)
01146                 dSigmaSquared = dOverrideSigma; // Bit of a waste having stored the vector of square errors in this case!
01147         else
01148         {
01149                 if (nEstimator == 0)
01150                         dSigmaSquared = Tukey::FindSigmaSquared(vdErrorSquared);
01151                 else if(nEstimator == 1)
01152                         dSigmaSquared = Cauchy::FindSigmaSquared(vdErrorSquared);
01153                 else
01154                         dSigmaSquared = Huber::FindSigmaSquared(vdErrorSquared);
01155         }
01156 
01157         // The TooN WLSCholesky class handles reweighted least squares.
01158         // It just needs errors and jacobians.
01159         WLS<6> wls;
01160         wls.add_prior(100.0); // Stabilising prior
01161         for(unsigned int f=0; f<vTD.size(); f++)
01162         {
01163                 TrackerData &TD = *vTD[f];
01164                 if(!TD.bFound)
01165                         continue;
01166                 Vector<2> &v2 = TD.v2Error_CovScaled;
01167                 double dErrorSq = v2 * v2;
01168                 double dWeight;
01169 
01170                 if(nEstimator == 0)
01171                         dWeight= Tukey::Weight(dErrorSq, dSigmaSquared);
01172                 else if(nEstimator == 1)
01173                         dWeight= Cauchy::Weight(dErrorSq, dSigmaSquared);
01174                 else
01175                         dWeight= Huber::Weight(dErrorSq, dSigmaSquared);
01176 
01177                 // Inlier/outlier accounting, only really works for cut-off estimators such as Tukey.
01178                 if(dWeight == 0.0)
01179                 {
01180                         if(bMarkOutliers)
01181                                 TD.Point->nMEstimatorOutlierCount++;
01182                         continue;
01183                 }
01184                 else
01185                         if(bMarkOutliers)
01186                                 TD.Point->nMEstimatorInlierCount++;
01187 
01188                 Matrix<2,6> &m26Jac = TD.m26Jacobian;
01189                 wls.add_mJ(v2[0], TD.dSqrtInvNoise * m26Jac[0], dWeight); // These two lines are currently
01190                 wls.add_mJ(v2[1], TD.dSqrtInvNoise * m26Jac[1], dWeight); // the slowest bit of poseits
01191         }
01192 
01193         wls.compute();
01194         //Weiss{
01195         if (bMarkOutliers)      //only true for last iteration, see code above... (iter==9)
01196                 mmCovariances=TooN::SVD<6>(wls.get_C_inv()).get_pinv();
01197         //}
01198         return wls.get_mu();
01199 }
01200 
01201 
01202 // Just add the current velocity to the current pose.
01203 // N.b. this doesn't actually use time in any way, i.e. it assumes
01204 // a one-frame-per-second camera. Skipped frames etc
01205 // are not handled properly here.
01206 void Tracker::ApplyMotionModel()
01207 {
01208   const VarParams& vp = PtamParameters::varparams();
01209   mse3StartPos = mse3CamFromWorld;
01210   Vector<6> v6Velocity = mv6CameraVelocity;
01211   if(mbUseSBIInit && vp.MotionModelSource == ptam::PtamParams_MM_CONSTANT)
01212   {
01213     v6Velocity.slice<3,3>() = mv6SBIRot.slice<3,3>();
01214     v6Velocity[0] = 0.0;
01215     v6Velocity[1] = 0.0;
01216   }
01217   else if(vp.MotionModelSource == ptam::PtamParams_MM_IMU)
01218   {
01219     //
01220     v6Velocity.slice<3,3>() = (mso3LastImu*mso3CurrentImu.inverse()).ln();
01221     //                v6Velocity[0] = 0.0;
01222     //                v6Velocity[1] = 0.0;
01223   }
01224   //    std::cout<<"motion:"<<std::endl<<v6Velocity.slice<3,3>()<<std::endl
01225   //            <<(mso3CurrentImu.inverse()*mso3LastImu).ln()<<std::endl
01226   //            <<(mso3CurrentImu*mso3LastImu.inverse()).ln()<<std::endl
01227   //            <<(mso3LastImu.inverse()*mso3CurrentImu).ln()<<std::endl
01228   //            <<(mso3LastImu*mso3CurrentImu.inverse()).ln()<<std::endl
01229   //;
01230   mse3CamFromWorld = SE3<>::exp(v6Velocity) * mse3StartPos;
01231 };
01232 
01233 
01234 // The motion model is entirely the tracker's, and is kept as a decaying
01235 // constant velocity model.
01236 void Tracker::UpdateMotionModel()
01237 {
01238   SE3<> se3NewFromOld = mse3CamFromWorld * mse3StartPos.inverse();
01239   Vector<6> v6Motion = SE3<>::ln(se3NewFromOld);
01240   Vector<6> v6OldVel = mv6CameraVelocity;
01241 
01242   mv6CameraVelocity = 0.9 * (0.5 * v6Motion + 0.5 * v6OldVel);
01243   mdVelocityMagnitude = sqrt(mv6CameraVelocity * mv6CameraVelocity);
01244 
01245   // Also make an estimate of this which has been scaled by the mean scene depth.
01246   // This is used to decide if we should use a coarse tracking stage.
01247   // We can tolerate more translational vel when far away from scene!
01248   Vector<6> v6 = mv6CameraVelocity;
01249   v6.slice<0,3>() *= 1.0 / mCurrentKF->dSceneDepthMean;
01250   mdMSDScaledVelocityMagnitude = sqrt(v6*v6);
01251 }
01252 
01253 // Time to add a new keyframe? The MapMaker handles most of this.
01254 void Tracker::AddNewKeyFrame()
01255 {
01256   mMapMaker.AddKeyFrame(mCurrentKF);
01257   mnLastKeyFrameDropped = mnFrame;
01258 }
01259 
01260 // Some heuristics to decide if tracking is any good, for this frame.
01261 // This influences decisions to add key-frames, and eventually
01262 // causes the tracker to attempt relocalisation.
01263 void Tracker::AssessTrackingQuality()
01264 {
01265   int nTotalAttempted = 0;
01266   int nTotalFound = 0;
01267   int nLargeAttempted = 0;
01268   int nLargeFound = 0;
01269 
01270   for(int i=0; i<LEVELS; i++)
01271   {
01272     nTotalAttempted += manMeasAttempted[i];
01273     nTotalFound += manMeasFound[i];
01274     if(i>=2) nLargeAttempted += manMeasAttempted[i];
01275     if(i>=2) nLargeFound += manMeasFound[i];
01276   }
01277 
01278   //Weiss{
01279   
01280   const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
01281   if(nTotalFound <= pPars.TrackingQualityFoundPixels || nTotalAttempted == 0)
01282     mTrackingQuality = BAD;
01283   //}
01284   else
01285   {
01286     double dTotalFracFound = (double) nTotalFound / nTotalAttempted;
01287     double dLargeFracFound;
01288     if(nLargeAttempted > 10)
01289       dLargeFracFound = (double) nLargeFound / nLargeAttempted;
01290     else
01291       dLargeFracFound = dTotalFracFound;
01292 
01293     //Weiss{
01294     double gvdQualityGood = pPars.TrackingQualityGood;
01295     double gvdQualityLost = pPars.TrackingQualityLost;
01296     //static gvar3<double> gvdQualityGood("Tracker.TrackingQualityGood", 0.3, SILENT);
01297     //static gvar3<double> gvdQualityLost("Tracker.TrackingQualityLost", 0.13, SILENT);
01298     //}
01299 
01300     if(dTotalFracFound > gvdQualityGood)
01301       mTrackingQuality = GOOD;
01302     else if(dLargeFracFound < gvdQualityLost)
01303       mTrackingQuality = BAD;
01304     else
01305       mTrackingQuality = DODGY;
01306   }
01307 
01308   if(mTrackingQuality == DODGY)
01309   {
01310     // Further heuristics to see if it's actually bad, not just dodgy...
01311     // If the camera pose estimate has run miles away, it's probably bad.
01312     if(mMapMaker.IsDistanceToNearestKeyFrameExcessive(mCurrentKF))
01313       mTrackingQuality = BAD;
01314   }
01315 
01316   if(mTrackingQuality==BAD)
01317   {
01318     //Weiss{
01319     
01320     const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
01321     if (pPars.AutoInit & mMap.IsGood())
01322     {
01323       mAutoreset=true;
01324       ROS_WARN_STREAM("Forcing map reset because of bad tracking! Autoreset set to " << mAutoreset);
01325     }
01326     //}
01327     mnLostFrames++;
01328   }
01329   else
01330   {
01331     mnLostFrames = 0;
01332     //Weiss{
01333     if(!mAutoreset){
01334       mOldKF.reset(new KeyFrame);
01335       *mOldKF = *mCurrentKF;
01336     }
01337     //}
01338   }
01339 }
01340 
01341 string Tracker::GetMessageForUser()
01342 {
01343   return mMessageForUser.str();
01344 }
01345 
01346 void Tracker::CalcSBIRotation()
01347 {
01348   mpSBILastFrame->MakeJacs();
01349   pair<SE2<>, double> result_pair;
01350   result_pair = mpSBIThisFrame->IteratePosRelToTarget(*mpSBILastFrame, 6);
01351   SE3<> se3Adjust = SmallBlurryImage::SE3fromSE2(result_pair.first, mCamera);
01352   mv6SBIRot = se3Adjust.ln();
01353 }
01354 
01355 
01356 ImageRef TrackerData::irImageSize;  // Static member of TrackerData lives here
01357 
01358 
01359 //Achtelik{
01360 void Tracker::command(const std::string & cmd){
01361 
01362   if(PtamParameters::fixparams().gui)
01363     this->GUICommandCallBack(this, "KeyPress", cmd);
01364   else
01365     this->GUICommandHandler("KeyPress", cmd);
01366 }
01367 //}
01368 
01369 
01370 
01371 
01372 
01373 
01374 
01375 


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