SURF(Speed up Robust Features) Code

int main() 
{
  if (PROCEDURE == 1) return mainImage("IMG_4207.png");
  if (PROCEDURE == 2) return mainVideo();
}

//-------------------------------------------------------

int mainImage(const char *filename)
{
  // Declare Ipoints and other stuff
  IpVec ipts;
  IplImage *img=cvLoadImage(filename);

  // Detect and describe interest points in the image
  surfDetDes(img, ipts, false, 3, 4, 1, 0.0004f);

  // Draw the detected points
  drawIpoints(img, ipts);

  // Display the result
  showImage(img);

  return 0;
}

//-------------------------------------------------------

//! Library function builds vector of described interest points
inline void surfDetDes(IplImage *img,  /* image to find Ipoints in */
                       std::vector<Ipoint> &ipts, /* reference to vector of Ipoints */
                       bool upright = false, /* run in rotation invariant mode? */
                       int octaves = OCTAVES, /* number of octaves to calculate */
                       int intervals = INTERVALS, /* number of intervals per octave */
                       int init_sample = INIT_SAMPLE, /* initial sampling step */
                       float thres = THRES /* blob response threshold */)
{
  // Create integral-image representation of the image
  IplImage *int_img = Integral(img);
  
  // Create Fast Hessian Object
  FastHessian fh(int_img, ipts, octaves, intervals, init_sample, thres);
 
  // Extract interest points and store in vector ipts
  fh.getIpoints();
  
  // Create Surf Descriptor Object
  Surf des(int_img, ipts);

  // Extract the descriptors for the ipts
  des.getDescriptors(upright);

  // Deallocate the integral image
  cvReleaseImage(&int_img);
}

//-------------------------------------------------------

//! Constructor with image
FastHessian::FastHessian(IplImage *img, std::vector<Ipoint> &ipts, 
                         const int octaves, const int intervals, const int init_sample, 
                         const float thres) 
                         : ipts(ipts), m_det(NULL), i_width(0), i_height(0)
{
  // Initialise variables and construct Det array
  this->octaves = octaves;
  this->intervals = intervals;
  this->init_sample = init_sample;
  this->thres = thres;

  // Set the current image
  setIntImage(img);
}

//! Find the image features and write into vector of features
void FastHessian::getIpoints()
{
  // Clear the vector of exisiting ipts
  ipts.clear();

  // Calculate approximated determinant of hessian values
  buildDet();

  for(int o=0; o < octaves; o++) 
  {
    // for each octave double the sampling step of the previous
    int step = init_sample * cvRound(pow(2.0f,o));

    // determine border width for the largest filter for each ocave
    int border = (3 * cvRound(pow(2.0,o+1)*(intervals)+1) + 1)/2;

    // check for maxima across the scale space
    for(int i = 1; i < intervals-1; ++i) 
      for(int r = (border); r < i_height - (border); r += step)
        for(int c = (border); c < i_width - (border); c += step) 
          if(isExtremum(o, i, c, r)) 
            interp_extremum(o,i,r,c);
  } 
}

//-------------------------------------------------------

//! Calculate determinant of hessian responses
void FastHessian::buildDet()
{
  int lobe, border, step;
  float Dxx, Dyy, Dxy, scale;

  for(int o=0; o<octaves; o++) 
  {
    // calculate filter border for this octave
    border = (3 * fRound(pow(2.0f,o+1)*(intervals)+1) + 1)/2;
    step = init_sample * fRound(pow(2.0f,o));

    for(int i=0; i<intervals; i++) {

      // calculate lobe length (filter side length/3)
      lobe = fRound(pow(2.0f,o+1)*(i+1)+1);
      scale = 1.0f/pow((float)(3*lobe),2);

      for(int r=border; r<i_height-border; r+=step) 
      {
        for(int c=border; c<i_width-border; c+=step) 
        {
          Dyy = Area(img, c-(lobe-1), r-((3*lobe-1)/2), (2*lobe-1), lobe)
             -2*Area(img, c-(lobe-1), r-((lobe-1)/2), (2*lobe-1), lobe)
               +Area(img, c-(lobe-1), r+((lobe+1)/2), (2*lobe-1), lobe);

          Dxx = Area(img, c-((3*lobe-1)/2), r-(lobe-1), lobe, (2*lobe-1))
             -2*Area(img, c-((lobe-1)/2), r-(lobe-1), lobe, (2*lobe-1))
               +Area(img, c+((lobe+1)/2), r-(lobe-1), lobe, (2*lobe-1));

          Dxy =  Area(img, c-lobe, r-lobe, lobe, lobe)
                +Area(img, c+1, r+1, lobe, lobe)
                -Area(img, c-lobe, r+1, lobe, lobe)
                -Area(img, c+1, r-lobe, lobe, lobe);

          // Normalise the filter responses with respect to their size
          Dxx *= scale;
          Dyy *= scale;
          Dxy *= scale;

          // Get the sign of the laplacian
          int lap_sign = (Dxx+Dyy >= 0 ? 1 : -1);

          // Get the determinant of hessian response
          float res = (Dxx*Dyy - pow(0.9f*Dxy,2));
          res = (res < thres ? 0 : lap_sign * res);

          // calculate approximated determinant of hessian value
          m_det[(o*intervals+i)*(i_width*i_height) + (r*i_width+c)] = res;
        }
      }
    }
  }
}   

//-------------------------------------------------------

//! Non Maximal Suppression function
int FastHessian::isExtremum(int octave, int interval, int c, int r)
{
  float val = getVal(octave, interval, c, r);
  int step = init_sample * cvRound(pow(2.0f,octave));

  // reject points with low response to the det of hessian function
  if(val < thres) return 0;

  // check for maximum 
  for(int i = -1; i <= 1; i++ )
    for(int j = -step; j <= step; j+=step )
      for(int k = -step; k <= step; k+=step ) 
        if (i != 0 || j != 0 || k != 0)
          if(getVal(octave, interval+i, c+j, r+k) > val)
            return 0;

  return 1;
}

//Interpolates a scale-space extremum's location and scale to subpixel
//accuracy to form an image feature.   
void FastHessian::interp_extremum(int octv, int intvl, int r, int c)
{
	double xi = 0, xr = 0, xc = 0;
	int i = 0;
  int step = init_sample * cvRound(pow(2.0f,octv));

  // Get the offsets to the actual location of the extremum
  interp_step( octv, intvl, r, c, &xi, &xr, &xc );
  
  // If point is sufficiently close to the actual extremum
  if( fabs( xi ) <= 0.5  &&  fabs( xr ) <= 0.5  &&  fabs( xc ) <= 0.5 )
  {
    // Create Ipoint and push onto Ipoints vector
    Ipoint ipt;
    ipt.x = (float) (c + step*xc);
    ipt.y = (float) (r + step*xr);
    ipt.scale = (float) ((1.2f/9.0f) * (3*(pow(2.0f, octv+1) * (intvl+xi+1)+1)));
    ipt.laplacian = getLaplacian(octv, intvl, c, r);
    ipts.push_back(ipt);
  }
}

//Performs one step of extremum interpolation. 
void 
FastHessian::interp_step( octv, int intvl, int r, int c, double* xi, double* xr, double* xc )
{
	CvMat* dD, * H, * H_inv, X;
	double x[3] = { 0 };

	dD = deriv_3D( octv, intvl, r, c );
	H = hessian_3D( octv, intvl, r, c );
	H_inv = cvCreateMat( 3, 3, CV_64FC1 );
	cvInvert( H, H_inv, CV_SVD );
	cvInitMatHeader( &X, 3, 1, CV_64FC1, x, CV_AUTOSTEP );
	cvGEMM( H_inv, dD, -1, NULL, 0, &X, 0 );

	cvReleaseMat( &dD );
	cvReleaseMat( &H );
	cvReleaseMat( &H_inv );

	*xi = x[2];
	*xr = x[1];
	*xc = x[0];
}

//-------------------------------------------------------

//Computes the partial derivatives in x, y, and scale of a pixel in the DoG
//scale space pyramid.
CvMat* FastHessian::deriv_3D( int octv, int intvl, int r, int c )
{
	CvMat* dI;
	double dx, dy, ds;
          int step = init_sample * cvRound(pow(2.0f,octv));

          dx = ( getValLowe(octv,intvl, r, c+step ) -
		getValLowe( octv,intvl, r, c-step ) ) / 2.0;
	dy = ( getValLowe( octv,intvl, r+step, c ) -
		getValLowe( octv,intvl, r-step, c ) ) / 2.0;
	ds = ( getValLowe( octv,intvl+1, r, c ) -
		getValLowe( octv,intvl-1, r, c ) ) / 2.0;

	dI = cvCreateMat( 3, 1, CV_64FC1 );
	cvmSet( dI, 0, 0, dx );
	cvmSet( dI, 1, 0, dy );
	cvmSet( dI, 2, 0, ds );

	return dI;
}



//-------------------------------------------------------

//Computes the 3D Hessian matrix for a pixel in the DoG scale space pyramid.
CvMat* FastHessian::hessian_3D(int octv, int intvl, int r, int c )
{
	CvMat* H;
	double v, dxx, dyy, dss, dxy, dxs, dys;
  int step = init_sample * cvRound(pow(2.0f,octv));

	v = getValLowe( octv,intvl, r, c );
	dxx = ( getValLowe( octv,intvl, r, c+step ) + 
			getValLowe( octv,intvl, r, c-step ) - 2 * v );
	dyy = ( getValLowe( octv,intvl, r+step, c ) +
			getValLowe( octv,intvl, r-step, c ) - 2 * v );
	dss = ( getValLowe( octv,intvl+1, r, c ) +
			getValLowe( octv,intvl-1, r, c ) - 2 * v );
	dxy = ( getValLowe( octv,intvl, r+step, c+step ) -
			getValLowe( octv,intvl, r+step, c-step ) -
			getValLowe( octv,intvl, r-step, c+step ) +
			getValLowe( octv,intvl, r-step, c-step ) ) / 4.0;
	dxs = ( getValLowe( octv,intvl+1, r, c+step ) -
			getValLowe( octv,intvl+1, r, c-step ) -
			getValLowe( octv,intvl-1, r, c+step ) +
			getValLowe( octv,intvl-1, r, c-step ) ) / 4.0;
	dys = ( getValLowe( octv,intvl+1, r+step, c ) -
			getValLowe( octv,intvl+1, r-step, c ) -
			getValLowe( octv,intvl-1, r+step, c ) +
			getValLowe( octv,intvl-1, r-step, c ) ) / 4.0;

	H = cvCreateMat( 3, 3, CV_64FC1 );
	cvmSet( H, 0, 0, dxx );
	cvmSet( H, 0, 1, dxy );
	cvmSet( H, 0, 2, dxs );
	cvmSet( H, 1, 0, dxy );
	cvmSet( H, 1, 1, dyy );
	cvmSet( H, 1, 2, dys );
	cvmSet( H, 2, 0, dxs );
	cvmSet( H, 2, 1, dys );
	cvmSet( H, 2, 2, dss );

	return H;
}



//-------------------------------------------------------

//! Return the value of the approximated determinant of hessian
inline float FastHessian::getValLowe(int o, int i, int r, int c)
{
  return fabs(m_det[(o*intervals+i)*(i_width*i_height) + (r*i_width+c)]);
}



//-------------------------------------------------------

//! Describe all features in the supplied vector
void Surf::getDescriptors(bool upright)
{
  // Check there are Ipoints to be described
  if (!ipts.size()) return;

  // Get the size of the vector for fixed loop bounds
  int ipts_size = ipts.size();

  if (upright)
  {
    // U-SURF loop just gets descriptors
    for (unsigned int i = 0; i < ipts_size; ++i)
    {
      // Set the Ipoint to be described
      index = i;

      // Extract upright (i.e. not rotation invariant) descriptors
      getUprightDescriptor();
    }
  }
  else
  {
    // Main SURF-64 loop assigns orientations and gets descriptors
    for (unsigned int i = 0; i < ipts_size; ++i)
    {
      // Set the Ipoint to be described
      index = i;

      // Assign Orientations and extract rotation invariant descriptors
      getOrientation();
      getDescriptor();
    }
  }
}

//-------------------------------------------------------

//! Assign the supplied Ipoint an orientation
void Surf::getOrientation()
{
  Ipoint *ipt = &ipts.at(index);
  float gauss, scale = ipt->scale;
  int s = cvRound(scale), r = cvRound(ipt->y), c = cvRound(ipt->x);
  std::vector<float> resX, resY, Ang;

  // calculate haar responses for points within radius of 6*scale
  for(int i = -6*s; i <= 6*s; i+=s) 
  {
    for(int j = -6*s; j <= 6*s; j+=s) 
    {
      if( i*i + j*j  < 36*s*s ) 
      {
        gauss = gaussian(i, j, 2.5f*s);
        resX.push_back( gauss * haarX(r+j, c+i, 4*s) );
        resY.push_back( gauss * haarY(r+j, c+i, 4*s) );
        Ang.push_back(getAngle(resX.back(), resY.back()));
      }
    }
  }

  // calculate the dominant direction 
  float sumX, sumY;
  float max=0, old_max = 0, orientation = 0, old_orientation = 0;
  float ang1, ang2, ang;

  // loop slides pi/3 window around feature point
  for(ang1 = 0; ang1 < 2*pi;  ang1+=0.2f) {
    ang2 = ( ang1+pi/3.0f > 2*pi ? ang1-5.0f*pi/3.0f : ang1+pi/3.0f);
    sumX = sumY = 0; 
    for(unsigned int k = 0; k < Ang.size(); k++) 
    {
      // get angle from the x-axis of the sample point
      ang = Ang.at(k);

      // determine whether the point is within the window
      if (ang1 < ang2 && ang1 < ang && ang < ang2) 
      {
        sumX+=resX.at(k);  
        sumY+=resY.at(k);
      } 
      else if (ang2 < ang1 && 
        ((ang > 0 && ang < ang2) || (ang > ang1 && ang < 2*pi) )) 
      {
        sumX+=resX.at(k);  
        sumY+=resY.at(k);
      }
    }

    // if the vector produced from this window is longer than all 
    // previous vectors then this forms the new dominant direction
    if (sumX*sumX + sumY*sumY > max) 
    {
      // store second largest orientation
      old_max = max;
      old_orientation = orientation;

      // store largest orientation
      max = sumX*sumX + sumY*sumY;
      orientation = getAngle(sumX, sumY);
    }
  }

  // check whether there are two dominant orientations based on 0.8 threshold
  if (old_max >= 0.8*max)
  {
    // assign second largest orientation and push copy onto vector
    ipt->orientation = old_orientation;
    ipts.push_back(*ipt);
    
    // Reset ipt to point to correct Ipoint in the vector
    ipt = &ipts.at(index);
  }

  // assign orientation of the dominant response vector
  ipt->orientation = orientation;
}

//-------------------------------------------------------

//! Get the descriptor vector of the provided Ipoint
void Surf::getDescriptor()
{
  int y, x, count=0;
  float scale, *desc, dx, dy, mdx, mdy, co, si;
  float gauss, rx, ry, rrx, rry, sample_x, sample_y, len=0;
  
  Ipoint *ipt = &ipts.at(index);
  scale = ipt->scale;
  x = cvRound(ipt->x);
  y = cvRound(ipt->y);  
  co = cos(ipt->orientation);
  si = sin(ipt->orientation);
  desc = ipt->descriptor;

  // Calculate descriptor for this interest point
  for (int i = -10; i < 10; i+=5)
  {
    for (int j = -10; j < 10; j+=5) 
    {
      dx=dy=mdx=mdy=0;
      for (int k = i; k < i + 5; ++k) 
      {
        for (int l = j; l < j + 5; ++l) 
        {
          // Get coords of sample point on the rotated axis
          sample_x = cvRound(x + (-l*scale*si + k*scale*co));
          sample_y = cvRound(y + ( l*scale*co + k*scale*si));

          // Get the gaussian weighted x and y responses
          gauss = gaussian(k*scale, l*scale, 3.3f*scale);  
          rx = gauss * haarX(sample_y, sample_x, 2*scale);
          ry = gauss * haarY(sample_y, sample_x, 2*scale);

          // Get the gaussian weighted x and y responses on rotated axis
          rrx = -rx*si + ry*co;
          rry = rx*co + ry*si;

          dx += rrx;
          dy += rry;
          mdx += fabs(rrx);
          mdy += fabs(rry);
        }
      }

      // add the values to the descriptor vector
      desc[count++] = dx;
      desc[count++] = dy;
      desc[count++] = mdx;
      desc[count++] = mdy;

      // store the current length^2 of the vector
      len += dx*dx + dy*dy + mdx*mdx + mdy*mdy;
    }
  }

  // convert to unit vector
  len = sqrt(len);
  for( i = 0; i < 64; i++)
    desc[i] /= len;

}

//-------------------------------------------------------

//! Get the upright descriptor vector of the provided Ipoint
void Surf::getUprightDescriptor()
{
  int y, x, count=0;
  float scale, *desc, dx, dy, mdx, mdy;
  float gauss, rx, ry, len = 0.0f;

  Ipoint *ipt = &ipts.at(index);
  scale = ipt->scale;
  y = cvRound(ipt->y);  
  x = cvRound(ipt->x);
  desc = ipt->descriptor;

  // Calculate descriptor for this interest point
  for (int i = -10; i < 10; i+=5)
  {
    for (int j = -10; j < 10; j+=5) 
    {
      dx=dy=mdx=mdy=0;
      for (int k = i; k < i + 5; ++k) 
      {
        for (int l = j; l < j + 5; ++l) 
        {
          // get Gaussian weighted x and y responses
          gauss = gaussian(k*scale, l*scale, 3.3f*scale);  
          rx = gauss * haarX(k*scale+y, l*scale+x, 2*scale);
          ry = gauss * haarY(k*scale+y, l*scale+x, 2*scale);

          dx += rx;
          dy += ry;
          mdx += fabs(rx);
          mdy += fabs(ry);
        }
      }

      // add the values to the descriptor vector
      desc[count++] = dx;
      desc[count++] = dy;
      desc[count++] = mdx;
      desc[count++] = mdy;

      // store the current length^2 of the vector
      len += dx*dx + dy*dy + mdx*mdx + mdy*mdy;
    }
  }

  // convert to unit vector
  len = sqrt(len);
  for(i = 0; i < 64; i++)
    desc[i] /= len;

}