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