PatchFinder.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 // 
00004 // This header declares the PatchFinder class.
00005 // This is quite a low-level class.
00006 // 
00007 // The purpose of the PatchFinder is to find a map point in a new view. 
00008 // This proceeds in multiple stages:
00009 // 1. A warping matrix appropriate for the current view is calculated,
00010 // 2. A `coarse' matching template of the map point is generated by warping;
00011 // 3. The new view is searched for this template at corner locations;
00012 // 4. An inverse-composition (`fine') matching template is generated;
00013 // 5. A sub-pixel accurate position is calculated using inverse composition.
00014 //
00015 // To clarify points 1 and 2 above: Each map point does _not_ store a `patch' 
00016 // in the style of many other SLAM implementations! Each map point came from 
00017 // a key-frame, and that key-frame is stored in the map. Each map point only
00018 // stores the information on where it came from in what key-frame. Patches 
00019 // are then generated from the pixels of the source key-frame. According to the
00020 // relative camera poses now and of the source key-frame, a warp has to be 
00021 // applied to generate an NxN pixel square template appropriate for the current 
00022 // view. Thus, the matching template is always NxN, but the region of source 
00023 // pixels used can change size and shape! This is all described in the 
00024 // Klein & Murray ISMAR 2007.
00025 //
00026 // Most of the above stages are optional, and there are different versions
00027 // of stages 1/2, including some for operating when there is not yet a 3D map 
00028 // point. The class provides no safety checks to ensure that the operations
00029 // are carried out in the right order - it's the caller's responsibility.
00030 //
00031 // The patch finder uses zero-mean SSD as its difference metric.
00032 //
00033 // Although PatchFinder can use arbitrary-sized search templates (it's determined
00034 // at construction), the use of 8x8 pixel templates (the default) is highly 
00035 // recommended, as the coarse search for this size is SSE-optimised.
00036 
00037 #ifndef __PATCHFINDER_H
00038 #define __PATCHFINDER_H
00039 
00040 #include <TooN/TooN.h>
00041 using namespace TooN;
00042 #include <TooN/se3.h>
00043 #include <cvd/image.h>
00044 #include <cvd/byte.h>
00045 #include "MapPoint.h"
00046 #include "LevelHelpers.h"
00047 
00048 class PatchFinder
00049 {
00050 public:
00051   // Constructor defines size of search patch.
00052   PatchFinder(int nPatchSize = 8);
00053   
00054   // Step 1 Function.
00055   // This calculates the warping matrix appropriate for observing point p
00056   // from the current view (described as an SE3.) This also needs the camera
00057   // projection derivates at level zero for the point's projection in the new view.
00058   // It also calculates which pyramid level we should search in, and this is
00059   // returned as an int. Negative level returned denotes an inappropriate 
00060   // transformation.
00061   int CalcSearchLevelAndWarpMatrix(MapPoint &p, SE3<> se3CFromW, Matrix<2> &m2CamDerivs);
00062   inline int GetLevel() { return mnSearchLevel; }
00063   inline int GetLevelScale() { return LevelScale(mnSearchLevel); }
00064   
00065   // Step 2 Functions
00066   // Generates the NxN search template either from the pre-calculated warping matrix,
00067   // or an identity transformation.
00068   void MakeTemplateCoarseCont(MapPoint &p); // If the warping matrix has already been pre-calced, use this.
00069   void MakeTemplateCoarse(MapPoint &p, SE3<> se3CFromW, Matrix<2> &m2CamDerivs); // This also calculates the warp.
00070   void MakeTemplateCoarseNoWarp(MapPoint &p);  // Identity warp: just copies pixels from the source KF.
00071   void MakeTemplateCoarseNoWarp(KeyFrame &k, int nLevel, CVD::ImageRef irLevelPos); // Identity warp if no MapPoint struct exists yet.
00072 
00073   // If the template making failed (i.e. it needed pixels outside the source image),
00074   // this bool will return false.
00075   inline bool TemplateBad()      { return mbTemplateBad;} 
00076   
00077   // Step 3 Functions
00078   // This is the raison d'etre of the class: finds the patch in the current input view,
00079   // centered around ir, Searching around FAST corner locations only within a radius nRange only.
00080   // Inputs are given in level-zero coordinates! Returns true if the patch was found.
00081   bool FindPatchCoarse(CVD::ImageRef ir, KeyFrame &kf, unsigned int nRange);  
00082   int ZMSSDAtPoint(CVD::BasicImage<CVD::byte> &im, const CVD::ImageRef &ir); // This evaluates the score at one location
00083   // Results from step 3:
00084   // All positions are in the scale of level 0.
00085   inline CVD::ImageRef GetCoarsePos() { return CVD::ImageRef((int) mv2CoarsePos[0], (int) mv2CoarsePos[1]);} 
00086   inline Vector<2> GetCoarsePosAsVector() { return mv2CoarsePos; }
00087   
00088   // Step 4
00089   void MakeSubPixTemplate();  // Generate the inverse composition template and jacobians
00090   
00091   // Step 5 Functions
00092   bool IterateSubPixToConvergence(KeyFrame &kf, int nMaxIts);  // Run inverse composition till convergence
00093   double IterateSubPix(KeyFrame &kf);     // Single iteration of IC. Returns sum-squared pixel update dist, or negative if out of imag
00094   inline Vector<2> GetSubPixPos()  { return mv2SubPixPos;   }  // Get result
00095   void SetSubPixPos(Vector<2> v2)  { mv2SubPixPos = v2;     }  // Set starting point
00096   
00097   // Get the uncertainty estimate of a found patch;
00098   // This for just returns an appropriately-scaled identity!
00099   inline Matrix<2> GetCov()
00100   {
00101     return LevelScale(mnSearchLevel) * Identity;
00102   };
00103   
00104   int mnMaxSSD; // This is the max ZMSSD for a valid match. It's set in the constructor.
00105 
00106 protected:
00107  
00108   int mnPatchSize; // Size of one side of the matching template.
00109   
00110   // Some values stored for the coarse template:
00111   int mnTemplateSum;    // Cached pixel-sum of the coarse template
00112   int mnTemplateSumSq;  // Cached pixel-squared sum of the coarse template
00113   inline void MakeTemplateSums(); // Calculate above values
00114   
00115   CVD::Image<CVD::byte> mimTemplate;   // The matching template
00116   CVD::Image<std::pair<float,float> > mimJacs;  // Inverse composition jacobians; stored as floats to save a bit of space.
00117   
00118   Matrix<2> mm2WarpInverse;   // Warping matrix
00119   int mnSearchLevel;          // Search level in input pyramid
00120   Matrix<3> mm3HInv;          // Inverse composition JtJ^-1
00121   Vector<2> mv2SubPixPos;     // In the scale of level 0
00122   double mdMeanDiff;          // Updated during inverse composition
00123   
00124   CVD::ImageRef mirPredictedPos;  // Search center location of FindPatchCoarse in L0
00125   Vector<2> mv2CoarsePos;     // In the scale of level 0; hence the use of vector rather than ImageRef
00126   CVD::ImageRef mirCenter;    // Quantized location of the center pixel of the NxN pixel template
00127   bool mbFound;               // Was the patch found?
00128   bool mbTemplateBad;         // Error during template generation?
00129 
00130   // Some cached values to avoid duplicating work if the camera is stopped:
00131   MapPoint *mpLastTemplateMapPoint;  // Which was the last map point this PatchFinder used?
00132   Matrix<2> mm2LastWarpMatrix;       // What was the last warp matrix this PatchFinder used?
00133 };
00134 
00135 #endif
00136 
00137 
00138 
00139 
00140 
00141 
00142 
00143 


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