PatchFinder.h

// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited
// 
// This header declares the PatchFinder class.
// This is quite a low-level class.
// 
// The purpose of the PatchFinder is to find a map point in a new view. 
// This proceeds in multiple stages:
// 1. A warping matrix appropriate for the current view is calculated,
// 2. A `coarse' matching template of the map point is generated by warping;
// 3. The new view is searched for this template at corner locations;
// 4. An inverse-composition (`fine') matching template is generated;
// 5. A sub-pixel accurate position is calculated using inverse composition.
//
// To clarify points 1 and 2 above: Each map point does _not_ store a `patch' 
// in the style of many other SLAM implementations! Each map point came from 
// a key-frame, and that key-frame is stored in the map. Each map point only
// stores the information on where it came from in what key-frame. Patches 
// are then generated from the pixels of the source key-frame. According to the
// relative camera poses now and of the source key-frame, a warp has to be 
// applied to generate an NxN pixel square template appropriate for the current 
// view. Thus, the matching template is always NxN, but the region of source 
// pixels used can change size and shape! This is all described in the 
// Klein & Murray ISMAR 2007.
//
// Most of the above stages are optional, and there are different versions
// of stages 1/2, including some for operating when there is not yet a 3D map 
// point. The class provides no safety checks to ensure that the operations
// are carried out in the right order - it's the caller's responsibility.
//
// The patch finder uses zero-mean SSD as its difference metric.
//
// Although PatchFinder can use arbitrary-sized search templates (it's determined
// at construction), the use of 8x8 pixel templates (the default) is highly 
// recommended, as the coarse search for this size is SSE-optimised.

#ifndef __PATCHFINDER_H
#define __PATCHFINDER_H

#include <TooN/TooN.h>
using namespace TooN;
#include <TooN/se3.h>
#include <cvd/image.h>
#include <cvd/byte.h>
#include "MapPoint.h"
#include "LevelHelpers.h"
#include "KeyFrame.h"

class PatchFinder
{
public:
  PatchFinder(int nPatchSize = 8);
  
  int CalcSearchLevelAndWarpMatrix(MapPoint &p, SE3<> se3CFromW, Matrix<2> &m2CamDerivs);
  inline int GetLevel() { return mnSearchLevel; }
  inline int GetLevelScale(){return LevelScale(mnSearchLevel);}
  void CalcWarpMatrix(MapPoint &p,  SE3<> se3CamFromWorld, Matrix<2> &m2CamDerivs, int searchLevel);
  
  void MakeTemplateCoarseCont(MapPoint &p, bool left = true); 
  void MakeTemplateCoarse(MapPoint &p, SE3<> se3CFromW, Matrix<2> &m2CamDerivs); 
  void MakeTemplateCoarseNoWarp(MapPoint &p, bool left = true);
  void MakeTemplateCoarseNoWarp(Level Lev[], int nLevel, CVD::ImageRef irLevelPos); 
  bool IsTemplateSimilar(const PatchFinder &pf);
  void CopyTemplateFrom(const PatchFinder &Finder);
  
  inline bool TemplateBad()      { return mbTemplateBad;} 
  
  bool FindPatchCoarse(CVD::ImageRef ir, Level lV[], unsigned int nRange);  
  int ZMSSDAtPoint(CVD::BasicImage<CVD::byte> &im, const CVD::ImageRef &ir); 

  inline CVD::ImageRef GetCoarsePos() { return CVD::ImageRef((int) mv2CoarsePos[0], (int) mv2CoarsePos[1]);} 
  inline Vector<2> GetCoarsePosAsVector() { return mv2CoarsePos; }
  
  void MakeSubPixTemplate();    

  bool IterateSubPixToConvergence(Level *levels, int nMaxIts);  
  double IterateSubPix(Level *levels);
  inline Vector<2> GetSubPixPos()  { return mv2SubPixPos;   }  
  void SetSubPixPos(Vector<2> v2)  { mv2SubPixPos = v2;     }  
  
  inline Matrix<2> GetCov()
  {
    return LevelScale(mnSearchLevel) * Identity;
  };
  
  int mnMaxSSD; 
protected:
 
  int mnPatchSize;   
  int mnTemplateSum;    
  int mnTemplateSumSq;  
  inline void MakeTemplateSums(); 
  
  CVD::Image<CVD::byte> mimTemplate;   
  CVD::Image<std::pair<float,float> > mimJacs;  
  
  Matrix<2> mm2WarpInverse;   
  int mnSearchLevel;          
  Matrix<3> mm3HInv;          
  Vector<2> mv2SubPixPos;     
  double mdMeanDiff;          
  
  CVD::ImageRef mirPredictedPos; 
  Vector<2> mv2CoarsePos;     
  CVD::ImageRef mirCenter;    
  bool mbFound;               
  bool mbTemplateBad;         

  MapPoint *mpLastTemplateMapPoint;  
  Matrix<2> mm2LastWarpMatrix;      
};

#endif