MiniPatch.cc

// Copyright 2008 Isis Innovation Limited
#include "MiniPatch.h"
#include "ATANCamera.h"
using namespace CVD;
using namespace std;

// Scoring function
inline int MiniPatch::SSDAtPoint(CVD::BasicImage<CVD::byte> &im, const CVD::ImageRef &ir)
{
  if(!im.in_image_with_border(ir, mnHalfPatchSize))
    return mnMaxSSD + 1;
  ImageRef irImgBase = ir - ImageRef(mnHalfPatchSize, mnHalfPatchSize);
  int nRows = mimOrigPatch.size().y;
  int nCols = mimOrigPatch.size().x;
  byte *imagepointer;
  byte *templatepointer;
  int nDiff;
  int nSumSqDiff = 0;
  for(int nRow = 0; nRow < nRows; nRow++)
    {
      imagepointer = &im[irImgBase + ImageRef(0,nRow)];
      templatepointer = &mimOrigPatch[ImageRef(0,nRow)];
      for(int nCol = 0; nCol < nCols; nCol++)
	{
	  nDiff = imagepointer[nCol] - templatepointer[nCol];
	  nSumSqDiff += nDiff * nDiff;
	};
    };
  //cout << "nSumSqDiff is " << nSumSqDiff << endl;
  return nSumSqDiff;
}


// Find a patch by searching at FAST corners in an input image
// If available, a row-corner LUT is used to speed up search through the
// FAST corners
bool MiniPatch::FindPatch(CVD::ImageRef &irPos, 
			  CVD::BasicImage<CVD::byte> &im, 
			  int nxRangeTL, int nyRangeTL,
			  int nxRangeBR, int nyRangeBR,
			  vector<ImageRef> &vCorners,
			  std::vector<int> *pvRowLUT)
{
  ImageRef irCenter = irPos;
  ImageRef irBest;
  int nBestSSD = mnMaxSSD + 1;
  ImageRef irBBoxTL = irPos - ImageRef(nxRangeTL, nyRangeTL);
  ImageRef irBBoxBR = irPos + ImageRef(nxRangeBR, nyRangeBR);
  vector<ImageRef>::iterator i;
  if(!pvRowLUT)
    {
      for(i = vCorners.begin(); i!=vCorners.end(); i++)
	if(i->y >= irBBoxTL.y) break;
    }
  else
    {
      int nTopRow = irBBoxTL.y;
      if(nTopRow < 0)
	nTopRow = 0;
      if(nTopRow >= (int) pvRowLUT->size())
	nTopRow = (int) pvRowLUT->size() - 1;
      i = vCorners.begin() + (*pvRowLUT)[nTopRow];
    }
  
    for(; i!=vCorners.end(); i++)
    {
      if(i->x < irBBoxTL.x  || i->x > irBBoxBR.x)
	continue;
      if(i->y > irBBoxBR.y)
	break;
      int nSSD = SSDAtPoint(im, *i);
      if(nSSD < nBestSSD)
	{
	  
	  irBest = *i;
	  nBestSSD = nSSD;
	}
    }
  if(nBestSSD < mnMaxSSD)
    {
      irPos = irBest;
      return true;
    }
  else
    return false;
}

bool MiniPatch::FindPatchEpipolar(CVD::ImageRef &irPos, 
				  CVD::BasicImage<CVD::byte> &im, 
				  Vector<3> EpipolarLine,
				  vector<ImageRef> &vCorners)
{
  ImageRef irCenter = irPos;
  ImageRef irBest;
  int nBestSSD = mnMaxSSD + 1;
  
  vector<ImageRef>::iterator i;
  for (i = vCorners.begin(); i!=vCorners.end(); i++)
    {
      if((unproject(vec(*i))*EpipolarLine)*(unproject(vec(*i))*EpipolarLine) > 0.05 || sqrt((vec(irPos) - vec(*i))*(vec(irPos) - vec(*i))) > 100)
	continue;
      int nSSD = SSDAtPoint(im, *i);
      if(nSSD < nBestSSD)
	{
	  
	  irBest = *i;
	  nBestSSD = nSSD;
	}
    }
  if(nBestSSD < mnMaxSSD)
    {
      irPos = irBest;
      return true;
    }
  else
    return false;
}





// Define the patch from an input image
void MiniPatch::SampleFromImage(ImageRef irPos, BasicImage<byte> &im)
{
  if(!im.in_image_with_border(irPos, mnHalfPatchSize)){
    cout << irPos << "was not in image of size " << im.size() << endl;
      assert(im.in_image_with_border(irPos, mnHalfPatchSize));
  }
  CVD::ImageRef irPatchSize( 2 * mnHalfPatchSize + 1 , 2 * mnHalfPatchSize + 1);
  mimOrigPatch.resize(irPatchSize);
  copy(im, mimOrigPatch, mimOrigPatch.size(), irPos - mimOrigPatch.size() / 2);
}

// Static members
int MiniPatch::mnHalfPatchSize = 4;
int MiniPatch::mnRange = 10;
int MiniPatch::mnMaxSSD = 9999;