calibimage.cc

this is software for visual SLAM

// Copyright 2008 Isis Innovation Limited
#include "OpenGL.h"
#include "CalibImage.h"
#include <gvars3/instances.h>
#include <cvd/utility.h>
#include <cvd/convolution.h>
#include <cvd/fast_corner.h>
#include <cvd/vector_image_ref.h>
#include <cvd/image_interpolate.h>

#include <TooN/se3.h>
#include <TooN/SVD.h>
#include <TooN/wls.h>

using namespace std;
using namespace CVD;
using namespace GVars3;

inline bool IsCorner(Image<byte> &im, ImageRef ir, int nGate)
{ // Does a quick check to see if a point in an image could be a grid corner.
  // Does this by going around a 16-pixel ring, and checking that there's four
  // transitions (black - white- black - white - )
  // Also checks that the central pixel is blurred.

  // Find the mean intensity of the pixel ring...
  int nSum = 0;
  static byte abPixels[16];
  for(int i=0; i<16; i++)
    {
      abPixels[i] = im[ir + fast_pixel_ring[i]];
      nSum += abPixels[i];
    };
  int nMean = nSum / 16;
  int nHiThresh = nMean + nGate;
  int nLoThresh = nMean - nGate;

  // If the center pixel is roughly the same as the mean, this isn't a corner.
  int nCenter = im[ir];
  if(nCenter <= nLoThresh || nCenter >= nHiThresh)
    return false;
  
  // Count transitions around the ring... there should be four!
  bool bState = (abPixels[15] > nMean);
  int nSwaps = 0;
  for(int i=0; i<16; i++)
    {
      byte bValNow = abPixels[i];
      if(bState)
	{
	  if(bValNow < nLoThresh)
	    {
	      bState = false;
	      nSwaps++;
	    }
	}
      else
	if(bValNow > nHiThresh)
	  {
	    bState = true;
	    nSwaps++;
	  };
    }
  return (nSwaps == 4);
};

Vector<2> GuessInitialAngles(Image<byte> &im, ImageRef irCenter)
{
  // The iterative patch-finder works better if the initial guess
  // is roughly aligned! Find one of the line-axes by searching round 
  // the circle for the strongest gradient, and use that and +90deg as the
  // initial guesses for patch angle.
  //
  // Yes, this is a very poor estimate, but it's generally (hopefully?) 
  // enough for the iterative finder to converge.
  
  image_interpolate<Interpolate::Bilinear, byte> imInterp(im);
  double dBestAngle = 0;
  double dBestGradMag = 0;
  double dGradAtBest = 0;
  for(double dAngle = 0.0; dAngle < M_PI; dAngle += 0.1)
    {
      Vector<2> v2Dirn;
      v2Dirn[0] = cos(dAngle);      v2Dirn[1] = sin(dAngle);
      Vector<2> v2Perp;
      v2Perp[1] = -v2Dirn[0];      v2Perp[0] = v2Dirn[1];
      
      double dG =     imInterp[vec(irCenter) + v2Dirn * 3.0 + v2Perp * 0.1] - 
	              imInterp[vec(irCenter) + v2Dirn * 3.0 - v2Perp * 0.1]
	       +      imInterp[vec(irCenter) - v2Dirn * 3.0 - v2Perp * 0.1] - 
		      imInterp[vec(irCenter) - v2Dirn * 3.0 + v2Perp * 0.1];
      if(fabs(dG) > dBestGradMag)
	{
	  dBestGradMag = fabs(dG);
	  dGradAtBest = dG;
	  dBestAngle = dAngle;
	};
    }
  
  Vector<2> v2Ret;
  if(dGradAtBest < 0)
    {   v2Ret[0] = dBestAngle; v2Ret[1] = dBestAngle + M_PI / 2.0;    }
  else
    {   v2Ret[1] = dBestAngle; v2Ret[0] = dBestAngle - M_PI / 2.0;    }
  return v2Ret;
}

bool CalibImage::MakeFromImage(Image<byte> &im)
{
  static gvar3<int> gvnCornerPatchSize("CameraCalibrator.CornerPatchPixelSize", 20, SILENT);
  mvCorners.clear();
  mvGridCorners.clear();
  
  mim = im;
  mim.make_unique();
  
  // Find potential corners..
  // This works better on a blurred image, so make a blurred copy
  // and run the corner finding on that.
  {
    Image<byte> imBlurred = mim;
    imBlurred.make_unique();
    convolveGaussian(imBlurred, GV2.GetDouble("CameraCalibrator.BlurSigma", 1.0, SILENT));
    ImageRef irTopLeft(5,5);
    ImageRef irBotRight = mim.size() - irTopLeft;
    ImageRef ir = irTopLeft;
    glPointSize(1);
    glBegin(GL_POINTS);
    int nGate = GV2.GetInt("CameraCalibrator.MeanGate", 10, SILENT);
    do
      if(IsCorner(imBlurred, ir, nGate))
	{
	  mvCorners.push_back(ir);
	  glVertex(ir);
	}
    while(ir.next(irTopLeft, irBotRight));
    glEnd();
  }
  
  // If there's not enough corners, i.e. camera pointing somewhere random, abort.
  if((int) mvCorners.size() < GV2.GetInt("CameraCalibrator.MinCornersForGrabbedImage", 20, SILENT))
    return false;
  
  // Pick a central corner point...
  ImageRef irCenterOfImage = mim.size()  / 2;
  ImageRef irBestCenterPos;
  unsigned int nBestDistSquared = 99999999;
  for(unsigned int i=0; i<mvCorners.size(); i++)
    {
      unsigned int nDist = (mvCorners[i] - irCenterOfImage).mag_squared();
      if(nDist < nBestDistSquared)
	{
	  nBestDistSquared = nDist;
	  irBestCenterPos = mvCorners[i];
	}
    }
  
  // ... and try to fit a corner-patch to that.
  CalibCornerPatch Patch(*gvnCornerPatchSize);
  CalibCornerPatch::Params Params;
  Params.v2Pos = vec(irBestCenterPos);
  Params.v2Angles = GuessInitialAngles(mim, irBestCenterPos); 
  Params.dGain = 80.0;
  Params.dMean = 120.0;
  
  if(!Patch.IterateOnImage(Params, mim))
    return false;
  
  // The first found corner patch becomes the origin of the detected grid.
  CalibGridCorner cFirst;
  cFirst.Params = Params;
  mvGridCorners.push_back(cFirst);
  cFirst.Draw();
  
  // Next, go in two compass directions from the origin patch, and see if 
  // neighbors can be found.
  if(!ExpandByAngle(0,0))
    return false;
  if(!ExpandByAngle(0,1))
    return false;
  
  mvGridCorners[1].mInheritedSteps = mvGridCorners[2].mInheritedSteps = mvGridCorners[0].GetSteps(mvGridCorners);
  
  // The three initial grid elements are enough to find the rest of the grid.
  int nNext;
  int nSanityCounter = 0; // Stop it getting stuck in an infinite loop...
  const int nSanityCounterLimit = 500;
  while((nNext = NextToExpand()) >= 0 && nSanityCounter < nSanityCounterLimit )
    {
      ExpandByStep(nNext);
      nSanityCounter++;
    }
  if(nSanityCounter == nSanityCounterLimit)
    return false;
  
  DrawImageGrid();
  return true;
}

bool CalibImage::ExpandByAngle(int nSrc, int nDirn)
{
  static gvar3<int> gvnCornerPatchSize("CameraCalibrator.CornerPatchPixelSize", 20, SILENT);
  CalibGridCorner &gSrc = mvGridCorners[nSrc];
  
  ImageRef irBest;
  double dBestDist = 99999;
  Vector<2> v2TargetDirn = gSrc.Params.m2Warp().T()[nDirn];
  for(unsigned int i=0; i<mvCorners.size(); i++)
    {
      Vector<2> v2Diff = vec(mvCorners[i]) - gSrc.Params.v2Pos;
      if(v2Diff * v2Diff < 100)
	continue;
      if(v2Diff * v2Diff > dBestDist * dBestDist)
	continue;
      Vector<2> v2Dirn = v2Diff;
      normalize(v2Dirn);
      if(v2Dirn * v2TargetDirn < cos(M_PI / 18.0))
	continue;
      dBestDist = sqrt(v2Diff * v2Diff);
      irBest = mvCorners[i];
    }
  
  CalibGridCorner gTarget;
  gTarget.Params = gSrc.Params;
  gTarget.Params.v2Pos = vec(irBest);
  gTarget.Params.dGain *= -1;
  
  CalibCornerPatch Patch(*gvnCornerPatchSize);
  if(!Patch.IterateOnImage(gTarget.Params, mim))
    {
      gSrc.aNeighborStates[nDirn].val = N_FAILED;
      return false;
    }

  gTarget.irGridPos = gSrc.irGridPos;
  gTarget.irGridPos[nDirn]++;
  // Update connection states:
  mvGridCorners.push_back(gTarget); // n.b. This invalidates gSrc!
  mvGridCorners.back().aNeighborStates[(nDirn + 2) % 4].val = nSrc;
  mvGridCorners[nSrc].aNeighborStates[nDirn].val = mvGridCorners.size() - 1;
  
  mvGridCorners.back().Draw();
  return true;
}


void CalibGridCorner::Draw()
{
  glColor3f(0,1,0);
  glEnable(GL_LINE_SMOOTH);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glBegin(GL_LINES);
  glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef( 10,0)));
  glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef(-10,0)));
  glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef( 0, 10)));
  glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef( 0,-10)));
  glEnd();
}


double CalibGridCorner::ExpansionPotential()
{
  // Scoring function. How good would this grid corner be at finding a neighbor?
  // The best case is if it's already surrounded by three neighbors and only needs
  // to find the last one (because it'll have the most accurate guess for where
  // the last one should be) and so on.
  int nMissing = 0;
  for(int i=0; i<4; i++)
    if(aNeighborStates[i].val == N_NOT_TRIED)
      nMissing++;

  if(nMissing == 0)
    return 0.0;
  
  if(nMissing == 1)
    return 100.0;
  
  if(nMissing == 3)
    return 1.0;

  if(nMissing == 2)
    {
      int nFirst = 0;
      while(aNeighborStates[nFirst].val != N_NOT_TRIED)
	nFirst++;
      if(aNeighborStates[(nFirst + 2) + 2].val == N_NOT_TRIED)
	return 10.0;
      else
	return 20.0;
    }
  assert(0); // should never get here
  return 0.0;
};


Matrix<2> CalibGridCorner::GetSteps(vector<CalibGridCorner> &vgc)
{
  Matrix<2> m2Steps;
  for(int dirn=0; dirn<2; dirn++)
    {
      Vector<2> v2Dirn;
      int nFound = 0;
      Zero(v2Dirn);
      if(aNeighborStates[dirn].val >=0)
	{
	  v2Dirn += vgc[aNeighborStates[dirn].val].Params.v2Pos - Params.v2Pos;