KeyFrame.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/KeyFrame.h"
00003 #include "ptam/ShiTomasi.h"
00004 #include "ptam/SmallBlurryImage.h"
00005 #include <cvd/vision.h>
00006 #include <cvd/fast_corner.h>
00007 #include <agast/agast_corner_detect.h>
00008 //{slynen reprojection
00009 #include "ptam/MapPoint.h"
00010 #include "ptam/TrackerData.h"
00011 //}
00012 
00013 using namespace CVD;
00014 using namespace std;
00015 //using namespace GVars3;
00016 
00017 void KeyFrame::MakeKeyFrame_Lite(BasicImage<CVD::byte> &im)
00018 {
00019         // Perpares a Keyframe from an image. Generates pyramid levels, does FAST detection, etc.
00020         // Does not fully populate the keyframe struct, but only does the bits needed for the tracker;
00021         // e.g. does not perform FAST nonmax suppression. Things like that which are needed by the
00022         // mapmaker but not the tracker go in MakeKeyFrame_Rest();
00023 
00024         // adaptive thresholds
00025         static short thrs[4]={0,0,0,0};
00026         double buff;
00027 
00028         // First, copy out the image data to the pyramid's zero level.
00029         aLevels[0].im.resize(im.size());
00030         copy(im, aLevels[0].im);
00031 
00032         // Then, for each level...
00033         for(int i=0; i<LEVELS; i++)
00034         {
00035                 Level &lev = aLevels[i];
00036                 if(i!=0)
00037                 {  // .. make a half-size image from the previous level..
00038                         lev.im.resize(aLevels[i-1].im.size() / 2);
00039                         halfSample(aLevels[i-1].im, lev.im);
00040                 }
00041 
00042                 // .. and detect and store FAST corner points.
00043                 // I use a different threshold on each level; this is a bit of a hack
00044                 // whose aim is to balance the different levels' relative feature densities.
00045                 lev.vCorners.clear();
00046                 lev.vCandidates.clear();
00047                 lev.vMaxCorners.clear();
00048 
00049                 //Weiss{
00050                 void (*pFASTFunc)(const CVD::BasicImage<CVD::byte> &, std::vector<CVD::ImageRef> &,int)=NULL;
00051                 
00052                 const ptam::PtamParamsConfig& pPars = PtamParameters::varparams();
00053                 pFASTFunc=&fast_corner_detect_9_nonmax;
00054                 if (pPars.FASTMethod=="FAST9")
00055                         pFASTFunc=&fast_corner_detect_9;
00056                 else if (pPars.FASTMethod=="FAST10")
00057                         pFASTFunc=&fast_corner_detect_10;
00058                 else if (pPars.FASTMethod=="FAST9_nonmax")
00059                         pFASTFunc=&fast_corner_detect_9_nonmax;
00060                 else if (pPars.FASTMethod=="AGAST12d")
00061                         pFASTFunc=&agast::agast7_12d::agast_corner_detect12d;
00062                 else if (pPars.FASTMethod=="OAST16")
00063                         pFASTFunc=&agast::oast9_16::oast_corner_detect16;
00064 
00065                 if(i == 0)
00066                         pFASTFunc(lev.im, lev.vCorners, pPars.Thres_lvl0+thrs[i]);
00067                 if(i == 1)
00068                         pFASTFunc(lev.im, lev.vCorners, pPars.Thres_lvl1+thrs[i]);
00069                 if(i == 2)
00070                         pFASTFunc(lev.im, lev.vCorners, pPars.Thres_lvl2+thrs[i]);
00071                 if(i == 3)
00072                         pFASTFunc(lev.im, lev.vCorners, pPars.Thres_lvl3+thrs[i]);
00073 
00074                 if (pPars.AdaptiveThrs)
00075                 {
00076                         buff = lev.vCorners.size()-pPars.AdaptiveThrsMult*pPars.MaxPatchesPerFrame/pow(2.0,i);
00077                         thrs[i] = thrs[i]+(buff>0)-(buff<0);
00078         //              printf("0: %d 1: %d 2: %d 3: %d N: %d\n",thrs[0],thrs[1],thrs[2],thrs[3],lev.vCorners.size());
00079                 }
00080                 else
00081                         thrs[i]=0;
00082 //              printf("N: %d",lev.vCorners.size());
00083                 //}
00084 
00085                 // Generate row look-up-table for the FAST corner points: this speeds up
00086                 // finding close-by corner points later on.
00087                 unsigned int v=0;
00088                 lev.vCornerRowLUT.clear();
00089                 for(int y=0; y<lev.im.size().y; y++)
00090                 {
00091                         while(v < lev.vCorners.size() && y > lev.vCorners[v].y)
00092                                 v++;
00093                         lev.vCornerRowLUT.push_back(v);
00094                 }
00095         };
00096 }
00097 
00098 void KeyFrame::MakeKeyFrame_Rest()
00099 {
00100   // Fills the rest of the keyframe structure needed by the mapmaker:
00101   // FAST nonmax suppression, generation of the list of candidates for further map points,
00102   // creation of the relocaliser's SmallBlurryImage.
00103   //Weiss{
00104   //static double gvdCandidateMinSTScore = 70;
00105   
00106   const FixParams& pPars = PtamParameters::fixparams();
00107   const VarParams& pVarPars = PtamParameters::varparams();
00108   static double gvdCandidateMinSTScore = pPars.CandidateMinSTScore;
00109   //static gvar3<double> gvdCandidateMinSTScore("MapMaker.CandidateMinShiTomasiScore", 70, SILENT);
00110   //}
00111 
00112 
00113   // For each level...
00114   int startlvl=0;
00115   if(pVarPars.NoLevelZeroMapPoints)
00116     startlvl=1; // ignore level zero points for the map
00117   for(int l=startlvl; l<LEVELS; l++)
00118   {
00119     Level &lev = aLevels[l];
00120     // .. find those FAST corners which are maximal..
00121     fast_nonmax(lev.im, lev.vCorners, 10, lev.vMaxCorners);
00122     // .. and then calculate the Shi-Tomasi scores of those, and keep the ones with
00123     // a suitably high score as Candidates, i.e. points which the mapmaker will attempt
00124     // to make new map points out of.
00125     for(vector<ImageRef>::iterator i=lev.vMaxCorners.begin(); i!=lev.vMaxCorners.end(); i++)
00126     {
00127       if(!lev.im.in_image_with_border(*i, 10))
00128         continue;
00129       double dSTScore = FindShiTomasiScoreAtPoint(lev.im, 3, *i);
00130       if(dSTScore > gvdCandidateMinSTScore)
00131       {
00132         Candidate c;
00133         c.irLevelPos = *i;
00134         c.dSTScore = dSTScore;
00135         lev.vCandidates.push_back(c);
00136       }
00137     }
00138   };
00139 
00140   // Also, make a SmallBlurryImage of the keyframe: The relocaliser uses these.
00141   pSBI = new SmallBlurryImage(*this);
00142   // Relocaliser also wants the jacobians..
00143   pSBI->MakeJacs();
00144 
00145   //Weiss{
00146   static unsigned int idcounter=0;
00147   ID=idcounter;
00148   idcounter++;
00149   //}
00150 }
00151 
00152 //{slynen reprojection
00153 // checks whether to add a MapPoint to the KeyMapPoint of the KeyFrame
00154 // those points help selecting the Keyframes which are visible for reprojection
00155 void KeyFrame::AddKeyMapPoint(MapPoint::Ptr mp){
00156   vpPoints.push_back(mp);
00157   //first point init
00158   if(apCurrentBestPoints[0]==NULL){
00159     for (unsigned int i = 0;i < iBestPointsCount; i++) apCurrentBestPoints[i] = mp;
00160     return;
00161   }
00162   //apCurrentBestPoints indices:center = 0;upperright = 1;upperleft = 2;lowerleft = 3;lowerright = 4
00163   CVD::ImageRef imgRef  = aLevels[0].im.size();
00164 
00165   //all point locations in level zero coords
00166   Vector<2> v2Imcenter = makeVector(imgRef.x / 2,imgRef.y / 2);
00167   Vector<2> v2PointPos = LevelZeroPos(mp->irCenter,0);
00168   Vector<2> v2Diff = v2PointPos - v2Imcenter;   //distance to center of level zero
00169   Vector<2> v2DiffBest; //best distance for this point
00170 
00171   v2DiffBest = LevelZeroPos(apCurrentBestPoints[0]->irCenter,apCurrentBestPoints[0]->nSourceLevel) - v2Imcenter;
00172   if((v2Diff*v2Diff) < (v2DiffBest*v2DiffBest))
00173     apCurrentBestPoints[0] = mp;        //the point is closer to the center that current best center point
00174 
00175   //now check which quadrant the point is in
00176   if(v2PointPos[0] > v2Imcenter[0] &&  v2PointPos[1] > v2Imcenter[1]){
00177     v2DiffBest = LevelZeroPos(apCurrentBestPoints[1]->irCenter,apCurrentBestPoints[1]->nSourceLevel) - v2Imcenter;
00178     if((v2Diff*v2Diff) > (v2DiffBest*v2DiffBest))
00179       apCurrentBestPoints[1] = mp; //further away than current best point
00180   }else if(v2PointPos[0]<v2Imcenter[0] &&  v2PointPos[1] > v2Imcenter[1]){
00181     v2DiffBest = LevelZeroPos(apCurrentBestPoints[2]->irCenter,apCurrentBestPoints[2]->nSourceLevel) - v2Imcenter;
00182     if((v2Diff*v2Diff) > (v2DiffBest*v2DiffBest))
00183       apCurrentBestPoints[2] = mp; //further away than current best point
00184   }else if(v2PointPos[0]<v2Imcenter[0] &&  v2PointPos[1] < v2Imcenter[1]){
00185     v2DiffBest = LevelZeroPos(apCurrentBestPoints[3]->irCenter,apCurrentBestPoints[3]->nSourceLevel) - v2Imcenter;
00186     if((v2Diff*v2Diff) > (v2DiffBest*v2DiffBest))
00187       apCurrentBestPoints[3] = mp; //further away than current best point
00188   }else if(v2PointPos[0]>v2Imcenter[0] &&  v2PointPos[1] < v2Imcenter[1]){
00189     v2DiffBest = LevelZeroPos(apCurrentBestPoints[4]->irCenter,apCurrentBestPoints[4]->nSourceLevel) - v2Imcenter;
00190     if((v2Diff*v2Diff) > (v2DiffBest*v2DiffBest))
00191       apCurrentBestPoints[4] = mp; //further away than current best point
00192   }
00193 }
00194 //}
00195 
00196 // The keyframe struct is quite happy with default operator=, but Level needs its own
00197 // to override CVD's reference-counting behaviour.
00198 Level& Level::operator=(const Level &rhs)
00199 {
00200   // Operator= should physically copy pixels, not use CVD's reference-counting image copy.
00201   im.resize(rhs.im.size());
00202   copy(rhs.im, im);
00203 
00204   vCorners = rhs.vCorners;
00205   vMaxCorners = rhs.vMaxCorners;
00206   vCornerRowLUT = rhs.vCornerRowLUT;
00207   return *this;
00208 }
00209 
00210 // -------------------------------------------------------------
00211 // Some useful globals defined in LevelHelpers.h live here:
00212 Vector<3> gavLevelColors[LEVELS];
00213 
00214 // These globals are filled in here. A single static instance of this struct is run before main()
00215 struct LevelHelpersFiller // Code which should be initialised on init goes here; this runs before main()
00216 {
00217   LevelHelpersFiller()
00218   {
00219     for(int i=0; i<LEVELS; i++)
00220     {
00221       if(i==0)  gavLevelColors[i] = makeVector( 1.0, 0.0, 0.0);
00222       else if(i==1)  gavLevelColors[i] = makeVector( 1.0, 1.0, 0.0);
00223       else if(i==2)  gavLevelColors[i] = makeVector( 0.0, 1.0, 0.0);
00224       else if(i==3)  gavLevelColors[i] = makeVector( 0.0, 0.0, 0.7);
00225       else gavLevelColors[i] =  makeVector( 1.0, 1.0, 0.7); // In case I ever run with LEVELS > 4
00226     }
00227   }
00228 };
00229 static LevelHelpersFiller foo;
00230 
00231 
00232 
00233 
00234 
00235 
00236 


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