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 #include <boost/shared_ptr.hpp>
00048 
00049 class PatchFinder
00050 {
00051 public:
00052   // Constructor defines size of search patch.
00053   PatchFinder(int nPatchSize = 8);
00054 
00055   // Step 1 Function.
00056   // This calculates the warping matrix appropriate for observing point p
00057   // from the current view (described as an SE3.) This also needs the camera
00058   // projection derivates at level zero for the point's projection in the new view.
00059   // It also calculates which pyramid level we should search in, and this is
00060   // returned as an int. Negative level returned denotes an inappropriate 
00061   // transformation.
00062   int CalcSearchLevelAndWarpMatrix(boost::shared_ptr<MapPoint> p, SE3<> se3CFromW, Matrix<2> &m2CamDerivs);
00063   inline int GetLevel() { return mnSearchLevel; }
00064   inline int GetLevelScale() { return LevelScale(mnSearchLevel); }
00065 
00066   // Step 2 Functions
00067   // Generates the NxN search template either from the pre-calculated warping matrix,
00068   // or an identity transformation.
00069   void MakeTemplateCoarseCont(boost::shared_ptr<MapPoint> p); // If the warping matrix has already been pre-calced, use this.
00070   void MakeTemplateCoarse(boost::shared_ptr<MapPoint>, SE3<> se3CFromW, Matrix<2> &m2CamDerivs); // This also calculates the warp.
00071   void MakeTemplateCoarseNoWarp(boost::shared_ptr<MapPoint> p);  // Identity warp: just copies pixels from the source KF.
00072   void MakeTemplateCoarseNoWarp(boost::shared_ptr<KeyFrame> k, int nLevel, CVD::ImageRef irLevelPos); // Identity warp if no MapPoint struct exists yet.
00073 
00074   // If the template making failed (i.e. it needed pixels outside the source image),
00075   // this bool will return false.
00076   inline bool TemplateBad()      { return mbTemplateBad;} 
00077 
00078   // Step 3 Functions
00079   // This is the raison d'etre of the class: finds the patch in the current input view,
00080   // centered around ir, Searching around FAST corner locations only within a radius nRange only.
00081   // Inputs are given in level-zero coordinates! Returns true if the patch was found.
00082   bool FindPatchCoarse(CVD::ImageRef ir, boost::shared_ptr<KeyFrame> kf, unsigned int nRange);
00083   int ZMSSDAtPoint(CVD::BasicImage<CVD::byte> &im, const CVD::ImageRef &ir); // This evaluates the score at one location
00084   // Results from step 3:
00085   // All positions are in the scale of level 0.
00086   inline CVD::ImageRef GetCoarsePos() { return CVD::ImageRef((int) mv2CoarsePos[0], (int) mv2CoarsePos[1]);} 
00087   inline Vector<2> GetCoarsePosAsVector() { return mv2CoarsePos; }
00088 
00089   // Step 4
00090   void MakeSubPixTemplate();  // Generate the inverse composition template and jacobians
00091 
00092   // Step 5 Functions
00093   bool IterateSubPixToConvergence(KeyFrame& kf, int nMaxIts);  // Run inverse composition till convergence
00094   double IterateSubPix(KeyFrame& kf);     // Single iteration of IC. Returns sum-squared pixel update dist, or negative if out of imag
00095   inline Vector<2> GetSubPixPos()  { return mv2SubPixPos;   }  // Get result
00096   void SetSubPixPos(Vector<2> v2)  { mv2SubPixPos = v2;     }  // Set starting point
00097 
00098   // Get the uncertainty estimate of a found patch;
00099   // This for just returns an appropriately-scaled identity!
00100   inline Matrix<2> GetCov()
00101       {
00102     return LevelScale(mnSearchLevel) * Identity;
00103       };
00104 
00105   int mnMaxSSD; // This is the max ZMSSD for a valid match. It's set in the constructor.
00106 
00107 protected:
00108 
00109   int mnPatchSize; // Size of one side of the matching template.
00110 
00111   // Some values stored for the coarse template:
00112   int mnTemplateSum;    // Cached pixel-sum of the coarse template
00113   int mnTemplateSumSq;  // Cached pixel-squared sum of the coarse template
00114   inline void MakeTemplateSums(); // Calculate above values
00115 
00116   CVD::Image<CVD::byte> mimTemplate;   // The matching template
00117   CVD::Image<std::pair<float,float> > mimJacs;  // Inverse composition jacobians; stored as floats to save a bit of space.
00118 
00119   Matrix<2> mm2WarpInverse;   // Warping matrix
00120   int mnSearchLevel;          // Search level in input pyramid
00121   Matrix<3> mm3HInv;          // Inverse composition JtJ^-1
00122   Vector<2> mv2SubPixPos;     // In the scale of level 0
00123   double mdMeanDiff;          // Updated during inverse composition
00124 
00125   CVD::ImageRef mirPredictedPos;  // Search center location of FindPatchCoarse in L0
00126   Vector<2> mv2CoarsePos;     // In the scale of level 0; hence the use of vector rather than ImageRef
00127   CVD::ImageRef mirCenter;    // Quantized location of the center pixel of the NxN pixel template
00128   bool mbFound;               // Was the patch found?
00129   bool mbTemplateBad;         // Error during template generation?
00130 
00131   // Some cached values to avoid duplicating work if the camera is stopped:
00132   MapPoint::Ptr mpLastTemplateMapPoint;  // Which was the last map point this PatchFinder used?
00133   Matrix<2> mm2LastWarpMatrix;       // What was the last warp matrix this PatchFinder used?
00134 };
00135 
00136 #endif
00137 
00138 
00139 
00140 
00141 
00142 
00143 
00144 


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