// file:        sift.cpp
// author:      Andrea Vedaldi
// description: Sift definition

// AUTORIGHTS
// Copyright (c) 2006 The Regents of the University of California
// All Rights Reserved.
// 
// Created by Andrea Vedaldi (UCLA VisionLab)
// 
// Permission to use, copy, modify, and distribute this software and its
// documentation for educational, research and non-profit purposes,
// without fee, and without a written agreement is hereby granted,
// provided that the above copyright notice, this paragraph and the
// following three paragraphs appear in all copies.
// 
// This software program and documentation are copyrighted by The Regents
// of the University of California. The software program and
// documentation are supplied "as is", without any accompanying services
// from The Regents. The Regents does not warrant that the operation of
// the program will be uninterrupted or error-free. The end-user
// understands that the program was developed for research purposes and
// is advised not to rely exclusively on the program for any reason.
// 
// This software embodies a method for which the following patent has
// been issued: "Method and apparatus for identifying scale invariant
// features in an image and use of same for locating an object in an
// image," David G. Lowe, US Patent 6,711,293 (March 23,
// 2004). Provisional application filed March 8, 1999. Asignee: The
// University of British Columbia.
// 
// IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY
// FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES,
// INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS SOFTWARE AND
// ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF CALIFORNIA HAS BEEN
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. THE UNIVERSITY OF
// CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS"
// BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATIONS TO PROVIDE
// MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

/** @mainpage Scale Invariant Feature Transform
 **
 ** The algorithm is implemented by the class VL::Sift.
 **/

#include<sift.hpp>
#include<sift-conv.tpp>

#include<algorithm>
#include<iostream>
#include<sstream>

using namespace VL ;

// on startup, pre-compute expn(x) = exp(-x)
namespace VL { 
namespace Detail {

int const         expnTableSize = 256 ;
VL::float_t const expnTableMax  = VL::float_t(25.0) ;
VL::float_t       expnTable [ expnTableSize + 1 ] ;

struct buildExpnTable
{
  buildExpnTable() {
    for(int k = 0 ; k < expnTableSize + 1 ; ++k) {
      expnTable[k] = exp( - VL::float_t(k) / expnTableSize * expnTableMax ) ;
    }
  }
} _buildExpnTable ;

} }


namespace VL {
namespace Detail {

/** Comment eater istream manipulator */
class _cmnt {} cmnt ;

/** @brief Extract a comment from a stream
 **
 ** The function extracts a block of consecutive comments from an
 ** input stream. A comment is a sequence of whitespaces, followed by
 ** a `#' character and terminated at the next line ending. A block of
 ** comments is just a sequence of comments.
 **/
std::istream& 
operator>>(std::istream& is, _cmnt& manip)
{
  char c ;
  char b [1024] ; 
  is>>c ;
  if( c != '#' ) 
    return is.putback(c) ;
  is.getline(b,1024) ;
  return is ;
}

}

/** @brief Insert PGM file into stream
 **
 ** The function iserts into the stream @a os the grayscale image @a
 ** im encoded as a PGM file. The immage is assumed to be normalized
 ** in the range 0.0 - 1.0.
 **
 ** @param os output stream.
 ** @param im pointer to image data.
 ** @param width image width.
 ** @param height image height.
 ** @return the stream @a os.
 **/
std::ostream& 
insertPgm(std::ostream& os, pixel_t const* im, int width, int height)
{
  os<< "P5"   << "\n"
    << width  << " "
    << height << "\n"
    << "255"  << "\n" ;
  for(int y = 0 ; y < height ; ++y) {
    for(int x = 0 ; x < width ; ++x) {
      unsigned char v = 
        (unsigned char)
        (std::max(std::min(*im++, 1.0f),0.f) * 255.0f) ;
      os << v ;
    }
  }
  return os ;
}

/** @brief Extract PGM file from stream.
 **
 ** The function extracts from the stream @a in a grayscale image
 ** encoded as a PGM file. The function fills the structure @a buffer,
 ** containing the image dimensions and a pointer to the image data.
 **
 ** The image data is an array of floats and is owned by the caller,
 ** which should erase it as in
 ** 
 ** @code
 **   delete [] buffer.data.
 ** @endcode
 **
 ** When the function encouters an error it throws a generic instance
 ** of VL::Exception.
 **
 ** @param in input stream.
 ** @param buffer buffer descriptor to be filled.
 ** @return the stream @a in.
 **/
std::istream& 
extractPgm(std::istream& in, PgmBuffer& buffer)
{
  pixel_t* im_pt ;
  int      width ;
  int      height ;
  int      maxval ;

  char c ;
  in>>c ;
  if( c != 'P') VL_THROW("File is not in PGM format") ;
  
  bool is_ascii ;
  in>>c ;
  switch( c ) {
  case '2' : is_ascii = true ; break ;
  case '5' : is_ascii = false ; break ;
  default  : VL_THROW("File is not in PGM format") ;
  }
  
  in >> Detail::cmnt
     >> width
     >> Detail::cmnt 
     >> height
     >> Detail::cmnt
     >> maxval ;

  // after maxval no more comments, just a whitespace or newline
  {char trash ; in.get(trash) ;}

  if(maxval > 255)
    VL_THROW("Only <= 8-bit per channel PGM files are supported") ;

  if(! in.good()) 
    VL_THROW("PGM header parsing error") ;
  
  im_pt = new pixel_t [ width*height ];
  
  try {
    if( is_ascii ) {
      pixel_t* start = im_pt ;
      pixel_t* end   = start + width*height ; 
      pixel_t  norm  = pixel_t( maxval ) ;
      
      while( start != end ) {        
        int i ;
        in >> i ;	
        if( ! in.good() ) VL_THROW
                            ("PGM parsing error file (width="<<width
                             <<" height="<<height
                             <<" maxval="<<maxval
                             <<" at pixel="<<start-im_pt<<")") ;    
        *start++ = pixel_t( i ) / norm ;        
      }
    } else {
      std::streampos beg = in.tellg() ;
      char* buffer = new char [width*height] ;
      in.read(buffer, width*height) ;
      if( ! in.good() ) VL_THROW
			  ("PGM parsing error file (width="<<width
			   <<" height="<<height
			   <<" maxval="<<maxval
			   <<" at pixel="<<in.tellg()-beg<<")") ;
      
      pixel_t* start = im_pt ;
      pixel_t* end   = start + width*height ; 
      uint8_t* src = reinterpret_cast<uint8_t*>(buffer) ;      
      while( start != end ) *start++ = *src++ / 255.0f ;
    }       
  } catch(...) {
    delete [] im_pt ; 
    throw ;
  }
  
  buffer.width  = width ;
  buffer.height = height ;
  buffer.data   = im_pt ;

  return in ;
}

// ===================================================================
//                                                 Low level image ops
// -------------------------------------------------------------------

namespace Detail {

/** @brief Copy an image
 ** @param dst output imgage buffer.
 ** @param src input image buffer.
 ** @param width input image width.
 ** @param height input image height.
 **/
void
copy(pixel_t* dst, pixel_t const* src, int width, int height)
{
  memcpy(dst, src, sizeof(pixel_t)*width*height)  ;
}

/** @brief Copy an image upsanmpling two times
 **
 ** The destination buffer must be at least as big as two times
 ** the input buffer. Bilinear interpolation is used.
 **
 ** @param dst output imgage buffer.
 ** @param src input image buffer.
 ** @param width input image width.
 ** @param height input image height.
 **/
void 
copyAndUpsampleRows
(pixel_t* dst, pixel_t const* src, int width, int height)
{
  for(int y = 0 ; y < height ; ++y) {
    pixel_t b, a ;
    b = a = *src++ ;
    for(int x = 0 ; x < width-1 ; ++x) {
      b = *src++ ;
      *dst = a ;         dst += height ;
      *dst = 0.5*(a+b) ; dst += height ;
      a = b ;
    }
    *dst = b ; dst += height ;
    *dst = b ; dst += height ;
    dst += 1 - width * 2 * height ;
  }  
}

/** @brief Copy an image and downsample
 **
 ** The image is downsampled @a d times, i.e. reduced to @c 1/2^d of
 ** its original size. The parameters @a width and @a height are the
 ** size of the input image. The destination image is assumed to be @c
 ** floor(width/2^d) pixels wide and @c floor(height/2^d) pixels high.
 **
 ** @param dst output imgage buffer.
 ** @param src input image buffer.
 ** @param width input image width.
 ** @param height input image height.
 ** @param d downsampling factor.
 **/
void 
copyAndDownsample(pixel_t* dst, pixel_t const* src, 
                  int width, int height, int d)
{
  for(int y = 0 ; y < height ; y+=d) {
    pixel_t const * srcrowp = src + y * width ;    
    for(int x = 0 ; x < width - (d-1) ; x+=d) {     
      *dst++ = *srcrowp ;
      srcrowp += d ;
    }
  }
}

}

/** @brief Smooth an image 
 **
 ** The function convolves the image @a src by a Gaussian kernel of
 ** variance @a s and writes the result to @a dst. The function also
 ** needs a scratch buffer @a dst of the same size of @a src and @a
 ** dst.
 **
 ** @param dst output image buffer.
 ** @param temp scratch image buffer.
 ** @param src input image buffer.
 ** @param width width of the buffers.
 ** @param height height of the buffers.
 ** @param s standard deviation of the Gaussian kernel.
 **/
void
Sift::smooth
(pixel_t* dst, pixel_t* temp, 
 pixel_t const* src, int width, int height, 
 VL::float_t s)
{
  // make sure a buffer larege enough has been allocated
  // to hold the filter
  int W = int( ceil( VL::float_t(4.0) * s ) ) ;
  if( ! filter ) {
    filterReserved = 0 ;
  }
  
  if( filterReserved < W ) {
    filterReserved = W ;
    if( filter ) delete [] filter ;
    filter = new pixel_t [ 2* filterReserved + 1 ] ;
  }
  
  // pre-compute filter
  for(int j = 0 ; j < 2*W+1 ; ++j) 
    filter[j] = VL::pixel_t
      (std::exp
       (VL::float_t
        (-0.5 * (j-W) * (j-W) / (s*s) ))) ;
  
  // normalize to one
  normalize(filter, W) ;
  
  // convolve
  econvolve(temp, src, width, height, filter, W) ;
  econvolve(dst, temp, height, width, filter, W) ;
}

// ===================================================================
//                                                     Sift(), ~Sift()
// -------------------------------------------------------------------

/** @brief Initialize Gaussian scale space parameters
 **
 ** @param _im_pt  Source image data
 ** @param _width  Soruce image width
 ** @param _height Soruce image height
 ** @param _sigman Nominal smoothing value of the input image.
 ** @param _sigma0 Base smoothing level.
 ** @param _O      Number of octaves.
 ** @param _S      Number of levels per octave.
 ** @param _omin   First octave.
 ** @param _smin   First level in each octave.
 ** @param _smax   Last level in each octave.
 **/
Sift::Sift(const pixel_t* _im_pt, int _width, int _height,
     VL::float_t _sigman,
     VL::float_t _sigma0,
     int _O, int _S,
     int _omin, int _smin, int _smax)
  : sigman( _sigman ), 
    sigma0( _sigma0 ),
    O( _O ),
    S( _S ),
    omin( _omin ),
    smin( _smin ),
    smax( _smax ),
    
    temp( NULL ),
    octaves( NULL ),
    filter( NULL )    
{
  process(_im_pt, _width, _height) ;
}

/** @b
rief Destroy SIFT filter.
 **/
Sift::~Sift()
{
  freeBuffers() ;
}

/** Allocate buffers. Buffer sizes depend on the image size and the
 ** value of omin.
 **/
void
Sift::
prepareBuffers()
{
  // compute buffer size
  int w = (omin >= 0) ? (width  >> omin) : (width  << -omin) ;
  int h = (omin >= 0) ? (height >> omin) : (height << -omin) ;
  int size = w*h* std::max
    ((smax - smin), 2*((smax+1) - (smin-2) +1)) ;

  if( temp && tempReserved == size ) return ;
  
  freeBuffers() ;
  
  // allocate
  temp           = new pixel_t [ size ] ; 
  tempReserved   = size ;
  tempIsGrad     = false ;
  tempOctave     = 0 ;

  octaves = new pixel_t* [ O ] ;
  for(int o = 0 ; o < O ; ++o) {
    octaves[o] = new pixel_t [ (smax - smin + 1) * w * h ] ;
    w >>= 1 ;
    h >>= 1 ;
  }
}
  
/** @brief Free buffers.
 **
 ** This function releases any buffer allocated by prepareBuffers().
 **
 ** @sa prepareBuffers().
 **/
void
Sift::
freeBuffers()
{
  if( filter ) {
    delete [] filter ;
  }
  filter = 0 ;

  if( octaves ) {
    for(int o = 0 ; o < O ; ++o) {
      delete [] octaves[ o ] ;
    }
    delete [] octaves ;
  }
  octaves = 0 ;
  
  if( temp ) {
    delete [] temp ;   
  }
  temp = 0  ; 
}

// ===================================================================
//                                                         getKeypoint
// -------------------------------------------------------------------

/** @brief Get keypoint from position and scale
 **
 ** The function returns a keypoint with a given position and
 ** scale. Note that the keypoint structure contains fields that make
 ** sense only in conjunction with a specific scale space. Therefore
 ** the keypoint structure should be re-calculated whenever the filter
 ** is applied to a new image, even if the parameters @a x, @a y and
 ** @a sigma do not change.
 **
 ** @param x x coordinate of the center.
 ** @peram y y coordinate of the center.
 ** @param sigma scale.
 ** @return Corresponing keypoint.
 **/
Sift::Keypoint
Sift::getKeypoint(VL::float_t x, VL::float_t y, VL::float_t sigma) const
{
  // keypoints are restricted to have
  //  * si in the range smin+1 smax-2 
  //  * o  in the range omin,omin+O-1
  //
  // Depending on the values of smin and smax, often the same
  // keypoint can have multiple equivalent indexes si,o. In this
  // case we choose the one with biggest index o (save computation).
  //
  // In addition, we relax a bit the requirements on si. We start by
  // letting si vary in the range smin,smax-1 and we compute both o
  // and si. Then we clamp si to the range smin+1,smax-2. This allows
  // us to be more consistent with the way SIFT descriptors are
  // usually generated (indeed for the SIFT detector si is always in
  // the range smin+1,smax-2, but in some cases round(s) != s because
  // of the quadratic interpolation in scale).

  /* We do this in steps. Let phi=log2(sigma/sigma0)
     
    1) find max o : o+smin/S < phi, i.e. o=floor(phi+smin/S).
    2) clamp this vlaue to the range [omin,omin+O-1]
    3) compute the fractional scale index s = S(phi-o)
    4) round the fractional scale index to get si = round(s)
    5) clamp si to the range [smin+1,smax-2]
  */

  int o,ix,iy,is ;
  VL::float_t s,phi ;

  phi = log2(sigma/sigma0) ;
  o   = fast_floor( phi + VL::float_t(smin)/S ) ;
  o   = std::min(o,omin+O-1) ;
  o   = std::max(o,omin) ;
  s   = S*(phi-o) ;

  is  = int(s + 0.5) ;
  is  = std::max(is,smax-2) ;
  is  = std::min(is,smin+1) ;
  
  VL::float_t per = getOctaveSamplingPeriod(o) ;
  ix = int(x * per + 0.5) ;
  iy = int(y * per + 0.5) ;
  
  Keypoint key ;
  key.o  = o ;

  key.ix = ix ;
  key.iy = iy ;
  key.is = is ;

  key.x = x ;
  key.y = y ;
  key.s = s ;
  
  key.sigma = sigma ;
  
  return key ;
}

// ===================================================================
//                                                           process()
// -------------------------------------------------------------------

/** @brief Compute Gaussian Scale Space
 **
 ** The method computes the Gaussian scale space of the specified
 ** image. The scale space data is managed internally and can be
 ** accessed by means of getOctave() and getLevel().
 **
 ** @remark Calling this method will delete the list of keypoints
 ** constructed by detectKeypoints().
 **
 ** @param _im_pt pointer to image data.
 ** @param _width image width.
 ** @param _height image height .
 **/
void
Sift::
process(const pixel_t* _im_pt, int _width, int _height)
{
  using namespace Detail ;

  width  = _width ;
  height = _height ;
  prepareBuffers() ;
  
  VL::float_t sigmak = powf(2.0f, 1.0 / S) ;
  VL::float_t dsigma0 = sigma0 * sqrt (1.0f - 1.0f / (sigmak*sigmak) ) ;
  
  // -----------------------------------------------------------------
  //                                                 Make pyramid base
  // -----------------------------------------------------------------
  if( omin < 0 ) {
    copyAndUpsampleRows(temp,       _im_pt, width,  height  ) ;
    copyAndUpsampleRows(octaves[0], temp,   height, 2*width ) ;      

    for(int o = -1 ; o > omin ; --o) {
      copyAndUpsampleRows(temp,       octaves[0], width  << -o,    height << -o) ;
      copyAndUpsampleRows(octaves[0], temp,       height << -o, 2*(width  << -o)) ;             }

  } else if( omin > 0 ) {
    copyAndDownsample(octaves[0], _im_pt, width, height, 1 << omin) ;
  } else {
    copy(octaves[0], _im_pt, width, height) ;
  }

  {
    VL::float_t sa = sigma0 * powf(sigmak, smin) ; 
    VL::float_t sb = sigman / powf(2.0f,   omin) ; // review this
    if( sa > sb ) {
      VL::float_t sd = sqrt ( sa*sa - sb*sb ) ;
      smooth( octaves[0], temp, octaves[0], 
              getOctaveWidth(omin),
              getOctaveHeight(omin), 
              sd ) ;
    }
  }

  // -----------------------------------------------------------------
  //                                                      Make octaves
  // -----------------------------------------------------------------
  for(int o = omin ; o < omin+O ; ++o) {
    // Prepare octave base
    if( o > omin ) {
      int sbest = std::min(smin + S, smax) ;
      copyAndDownsample(getLevel(o,   smin ), 
				getLevel(o-1, sbest),
				getOctaveWidth(o-1),
				getOctaveHeight(o-1), 2 ) ;
      VL::float_t sa = sigma0 * powf(sigmak, smin      ) ;
      VL::float_t sb = sigma0 * powf(sigmak, sbest - S ) ;
      if(sa > sb ) {
        VL::float_t sd = sqrt ( sa*sa - sb*sb ) ;
        smooth( getLevel(o,0), temp, getLevel(o,0), 
                getOctaveWidth(o), getOctaveHeight(o),
                sd ) ;
      }
    }

    // Make other levels
    for(int s = smin+1 ; s <= smax ; ++s) {
      VL::float_t sd = dsigma0 * powf(sigmak, s) ;
      smooth( getLevel(o,s), temp, getLevel(o,s-1),
              getOctaveWidth(o), getOctaveHeight(o),
              sd ) ;
    }
  }
}

/** @brief Sift detector
 **
 ** The function runs the SIFT detector on the stored Gaussian scale
 ** space (see process()). The detector consists in three steps
 **
 ** - local maxima detection;
 ** - subpixel interpolation;
 ** - rejection of weak keypoints (@a threhsold);
 ** - rejection of keypoints on edge-like structures (@a edgeThreshold).
 **
 ** As they are found, keypoints are added to an internal list.  This
 ** list can be accessed by means of the member functions
 ** getKeypointsBegin() and getKeypointsEnd(). The list is ordered by
 ** octave, which is usefult to speed-up computeKeypointOrientations()
 ** and computeKeypointDescriptor().
 **/
void
Sift::detectKeypoints(VL::float_t threshold, VL::float_t edgeThreshold)
{
  keypoints.clear() ;

  int nValidatedKeypoints = 0 ;

  // Process one octave per time
  for(int o = omin ; o < omin + O ; ++o) {
        
    int const xo = 1 ;
    int const yo = getOctaveWidth(o) ;
    int const so = getOctaveWidth(o) * getOctaveHeight(o) ;
    int const ow = getOctaveWidth(o) ;
    int const oh = getOctaveHeight(o) ;

    VL::float_t xperiod = getOctaveSamplingPeriod(o) ;

    // -----------------------------------------------------------------
    //                                           Difference of Gaussians
    // -----------------------------------------------------------------
    pixel_t* dog = temp ;
    tempIsGrad = false ;
    {
      pixel_t* pt = dog ;
      for(int s = smin ; s <= smax-1 ; ++s) {
        pixel_t* srca = getLevel(o, s  ) ;
        pixel_t* srcb = getLevel(o, s+1) ;
        pixel_t* enda = srcb ;
        while( srca != enda ) {
          *pt++ = *srcb++ - *srca++ ;
        }
      }
    }
    
    // -----------------------------------------------------------------
    //                                           Find points of extremum
    // -----------------------------------------------------------------
    {
      pixel_t* pt  = dog + xo + yo + so ;
      for(int s = smin+1 ; s <= smax-2 ; ++s) {
        for(int y = 1 ; y < oh - 1 ; ++y) {
          for(int x = 1 ; x < ow - 1 ; ++x) {          
            pixel_t v = *pt ;
            
            // assert( (pt - x*xo - y*yo - (s-smin)*so) - dog == 0 ) ;
            
#define CHECK_NEIGHBORS(CMP,SGN)                    \
            ( v CMP ## = SGN 0.8 * threshold &&     \
              v CMP *(pt + xo) &&                   \
              v CMP *(pt - xo) &&                   \
              v CMP *(pt + so) &&                   \
              v CMP *(pt - so) &&                   \
              v CMP *(pt + yo) &&                   \
              v CMP *(pt - yo) &&                   \
                                                    \
              v CMP *(pt + yo + xo) &&              \
              v CMP *(pt + yo - xo) &&              \
              v CMP *(pt - yo + xo) &&              \
              v CMP *(pt - yo - xo) &&              \
                                                    \
              v CMP *(pt + xo      + so) &&         \
              v CMP *(pt - xo      + so) &&         \
              v CMP *(pt + yo      + so) &&         \
              v CMP *(pt - yo      + so) &&         \
              v CMP *(pt + yo + xo + so) &&         \
              v CMP *(pt + yo - xo + so) &&         \
              v CMP *(pt - yo + xo + so) &&         \
              v CMP *(pt - yo - xo + so) &&         \
                                                    \
              v CMP *(pt + xo      - so) &&         \
              v CMP *(pt - xo      - so) &&         \
              v CMP *(pt + yo      - so) &&         \
              v CMP *(pt - yo      - so) &&         \
              v CMP *(pt + yo + xo - so) &&         \
              v CMP *(pt + yo - xo - so) &&         \
              v CMP *(pt - yo + xo - so) &&         \
              v CMP *(pt - yo - xo - so) )
            
            if( CHECK_NEIGHBORS(>,+) || CHECK_NEIGHBORS(<,-) ) {
              
              Keypoint k ;
              k.ix = x ;
              k.iy = y ;
              k.is = s ;
              keypoints.push_back(k) ;
            }
            pt += 1 ;
          }
          pt += 2 ;
        }
        pt += 2*yo ;
      }
    }

    // -----------------------------------------------------------------
    //                                               Refine local maxima
    // -----------------------------------------------------------------
    { // refine
      KeypointsIter siter ;
      KeypointsIter diter ;
      
      for(diter = siter = keypointsBegin() + nValidatedKeypoints ; 
          siter != keypointsEnd() ; 
          ++siter) {
       
        int x = int( siter->ix ) ;
        int y = int( siter->iy ) ;
        int s = int( siter->is ) ;
                
        VL::float_t Dx=0,Dy=0,Ds=0,Dxx=0,Dyy=0,Dss=0,Dxy=0,Dxs=0,Dys=0 ;
        VL::float_t  b [3] ;
        pixel_t* pt ;
        int dx = 0 ;
        int dy = 0 ;

        // must be exec. at least once
        for(int iter = 0 ; iter < 5 ; ++iter) {

          VL::float_t A[3*3] ;          

          x += dx ;
          y += dy ;

          pt = dog 
            + xo * x
            + yo * y
            + so * (s - smin) ;

#define at(dx,dy,ds) (*( pt + (dx)*xo + (dy)*yo + (ds)*so))
#define Aat(i,j)     (A[(i)+(j)*3])    
          
          /* Compute the gradient. */
          Dx = 0.5 * (at(+1,0,0) - at(-1,0,0)) ;
          Dy = 0.5 * (at(0,+1,0) - at(0,-1,0));
          Ds = 0.5 * (at(0,0,+1) - at(0,0,-1)) ;
          
          /* Compute the Hessian. */
          Dxx = (at(+1,0,0) + at(-1,0,0) - 2.0 * at(0,0,0)) ;
          Dyy = (at(0,+1,0) + at(0,-1,0) - 2.0 * at(0,0,0)) ;
          Dss = (at(0,0,+1) + at(0,0,-1) - 2.0 * at(0,0,0)) ;
          
          Dxy = 0.25 * ( at(+1,+1,0) + at(-1,-1,0) - at(-1,+1,0) - at(+1,-1,0) ) ;
          Dxs = 0.25 * ( at(+1,0,+1) + at(-1,0,-1) - at(-1,0,+1) - at(+1,0,-1) ) ;
          Dys = 0.25 * ( at(0,+1,+1) + at(0,-1,-1) - at(0,-1,+1) - at(0,+1,-1) ) ;
          
          /* Solve linear system. */
          Aat(0,0) = Dxx ;
          Aat(1,1) = Dyy ;
          Aat(2,2) = Dss ;
          Aat(0,1) = Aat(1,0) = Dxy ;
          Aat(0,2) = Aat(2,0) = Dxs ;
          Aat(1,2) = Aat(2,1) = Dys ;
          
          b[0] = - Dx ;
          b[1] = - Dy ;
          b[2] = - Ds ;
          
          // Gauss elimination
          for(int j = 0 ; j < 3 ; ++j) {

            // look for leading pivot
            VL::float_t maxa = 0 ;
            VL::float_t maxabsa = 0 ;
            int   maxi = -1 ;
            int i ;
            for(i = j ; i < 3 ; ++i) {
              VL::float_t a    = Aat(i,j) ;
              VL::float_t absa = fabsf( a ) ;
              if ( absa > maxabsa ) {
                maxa    = a ;
                maxabsa = absa ;
                maxi    = i ;
              }
            }

            // singular?
            if( maxabsa < 1e-10f ) {
              b[0] = 0 ;
              b[1] = 0 ;
              b[2] = 0 ;
              break ;
            }

            i = maxi ;

            // swap j-th row with i-th row and
            // normalize j-th row
            for(int jj = j ; jj < 3 ; ++jj) {
              std::swap( Aat(j,jj) , Aat(i,jj) ) ;
              Aat(j,jj) /= maxa ;
            }
            std::swap( b[j], b[i] ) ;
            b[j] /= maxa ;

            // elimination
            for(int ii = j+1 ; ii < 3 ; ++ii) {
              VL::float_t x = Aat(ii,j) ;
              for(int jj = j ; jj < 3 ; ++jj) {
                Aat(ii,jj) -= x * Aat(j,jj) ;                
              }
              b[ii] -= x * b[j] ;
            }
          }

          // backward substitution
          for(int i = 2 ; i > 0 ; --i) {
            VL::float_t x = b[i] ;
            for(int ii = i-1 ; ii >= 0 ; --ii) {
              b[ii] -= x * Aat(ii,i) ;
            }
          }

          /* If the translation of the keypoint is big, move the keypoint
           * and re-iterate the computation. Otherwise we are all set.
           */
          dx= ((b[0] >  0.6 && x < ow-2) ?  1 : 0 )
            + ((b[0] < -0.6 && x > 1   ) ? -1 : 0 ) ;
          
          dy= ((b[1] >  0.6 && y < oh-2) ?  1 : 0 )
            + ((b[1] < -0.6 && y > 1   ) ? -1 : 0 ) ;

          /*          
          std::cout<<x<<","<<y<<"="<<at(0,0,0)
                   <<"("
                   <<at(0,0,0)+0.5 * (Dx * b[0] + Dy * b[1] + Ds * b[2])<<")"
                   <<" "<<std::flush ; 
          */

          if( dx == 0 && dy == 0 ) break ;
        }
        
        /* std::cout<<std::endl ; */
        
        // Accept-reject keypoint
        {
          VL::float_t val = at(0,0,0) + 0.5 * (Dx * b[0] + Dy * b[1] + Ds * b[2]) ; 
          VL::float_t score = (Dxx+Dyy)*(Dxx+Dyy) / (Dxx*Dyy - Dxy*Dxy) ; 
          VL::float_t xn = x + b[0] ;
          VL::float_t yn = y + b[1] ;
          VL::float_t sn = s + b[2] ;
          
          if(fast_abs(val) > threshold &&
             score < (edgeThreshold+1)*(edgeThreshold+1)/edgeThreshold && 
             score >= 0 &&
             fast_abs(b[0]) < 1.5 &&
             fast_abs(b[1]) < 1.5 &&
             fast_abs(b[2]) < 1.5 &&
             xn >= 0    &&
             xn <= ow-1 &&
             yn >= 0    &&
             yn <= oh-1 &&
             sn >= smin &&
             sn <= smax ) {
            
            diter->o  = o ;
       
            diter->ix = x ;
            diter->iy = y ;
            diter->is = s ;

            diter->x = xn * xperiod ; 
            diter->y = yn * xperiod ; 
            diter->s = sn ;

            diter->sigma = getScaleFromIndex(o,sn) ;

            ++diter ;
          }
        }
      } // next candidate keypoint

      // prepare for next octave
      keypoints.resize( diter - keypoints.begin() ) ;
      nValidatedKeypoints = keypoints.size() ;
    } // refine block

  } // next octave
}

// ===================================================================
//                                       computeKeypointOrientations()
// -------------------------------------------------------------------

/** @brief Compute modulus and phase of the gradient
 **
 ** The function computes the modulus and the angle of the gradient of
 ** the specified octave @a o. The result is stored in a temporary
 ** internal buffer accessed by computeKeypointDescriptor() and
 ** computeKeypointOrientations().
 **
 ** The SIFT detector provides keypoint with scale index s in the
 ** range @c smin+1 and @c smax-2. As such, the buffer contains only
 ** these levels.
 **
 ** If called mutliple time on the same data, the function exits
 ** immediately.
 **
 ** @param o octave of interest.
 **/
void
Sift::prepareGrad(int o)
{ 
  int const ow = getOctaveWidth(o) ;
  int const oh = getOctaveHeight(o) ;
  int const xo = 1 ;
  int const yo = ow ;
  int const so = oh*ow ;

  if( ! tempIsGrad || tempOctave != o ) {

    // compute dx/dy
    for(int s = smin+1 ; s <= smax-2 ; ++s) {
      for(int y = 1 ; y < oh-1 ; ++y ) {
        pixel_t* src  = getLevel(o, s) + xo + yo*y ;        
        pixel_t* end  = src + ow - 1 ;
        pixel_t* grad = 2 * (xo + yo*y + (s - smin -1)*so) + temp ;
        while(src != end) {
          VL::float_t Gx = 0.5 * ( *(src+xo) - *(src-xo) ) ;
          VL::float_t Gy = 0.5 * ( *(src+yo) - *(src-yo) ) ;
          VL::float_t m = fast_sqrt( Gx*Gx + Gy*Gy ) ;
          VL::float_t t = fast_mod_2pi( fast_atan2(Gy, Gx) + VL::float_t(2*M_PI) );
          *grad++ = pixel_t( m ) ;
          *grad++ = pixel_t( t ) ;
          ++src ;
        }
      }
    }
  }
  
  tempIsGrad = true ;
  tempOctave = o ;
}

/** @brief Compute the orientation(s) of a keypoint
 **
 ** The function computes the orientation of the specified keypoint.
 ** The function returns up to four different orientations, obtained
 ** as strong peaks of the histogram of gradient orientations (a
 ** keypoint can theoretically generate more than four orientations,
 ** but this is very unlikely).
 **
 ** @remark The function needs to compute the gradient modululs and
 ** orientation of the Gaussian scale space octave to which the
 ** keypoint belongs. The result is cached, but discarded if different
 ** octaves are visited. Thereofre it is much quicker to evaluate the
 ** keypoints in their natural octave order.
 **
 ** The keypoint must lie within the scale space. In particular, the
 ** scale index is supposed to be in the range @c smin+1 and @c smax-1
 ** (this is from the SIFT detector). If this is not the case, the
 ** computation is silently aborted and no orientations are returned.
 **
 ** @param angles buffers to store the resulting angles.
 ** @param keypoint keypoint to process.
 ** @return number of orientations found.
 **/
int
Sift::computeKeypointOrientations(VL::float_t angles [4], Keypoint keypoint)
{
  int const   nbins = 36 ;
  VL::float_t const winFactor = 1.5 ;
  VL::float_t hist [nbins] ;

  // octave
  int o = keypoint.o ;
  VL::float_t xperiod = getOctaveSamplingPeriod(o) ;

  // offsets to move in the Gaussian scale space octave
  const int ow = getOctaveWidth(o) ;
  const int oh = getOctaveHeight(o) ;
  const int xo = 2 ;
  const int yo = xo * ow ;
  const int so = yo * oh ;

  // keypoint fractional geometry
  VL::float_t x     = keypoint.x / xperiod ;
  VL::float_t y     = keypoint.y / xperiod ;
  VL::float_t sigma = keypoint.sigma / xperiod ;
  
  // shall we use keypoints.ix,iy,is here?
  int xi = ((int) (x+0.5)) ; 
  int yi = ((int) (y+0.5)) ;
  int si = keypoint.is ;
  
  VL::float_t const sigmaw = winFactor * sigma ;
  int W = (int) floor(3.0 * sigmaw) ;
  
  // skip the keypoint if it is out of bounds
  if(o  < omin   ||
     o  >=omin+O ||
     xi < 0      || 
     xi > ow-1   || 
     yi < 0      || 
     yi > oh-1   || 
     si < smin+1 || 
     si > smax-2 ) {
    std::cerr<<"!"<<std::endl ;
    return 0 ;
  }
  
  // make sure that the gradient buffer is filled with octave o
  prepareGrad(o) ;

  // clear the SIFT histogram
  std::fill(hist, hist + nbins, 0) ;

  // fill the SIFT histogram
  pixel_t* pt = temp + xi * xo + yi * yo + (si - smin -1) * so ;

#undef at
#define at(dx,dy) (*(pt + (dx)*xo + (dy)*yo))

  for(int ys = std::max(-W, 1-yi) ; ys <= std::min(+W, oh -2 -yi) ; ++ys) {
    for(int xs = std::max(-W, 1-xi) ; xs <= std::min(+W, ow -2 -xi) ; ++xs) {
      
      VL::float_t dx = xi + xs - x;
      VL::float_t dy = yi + ys - y;
      VL::float_t r2 = dx*dx + dy*dy ;

      // limit to a circular window
      if(r2 >= W*W+0.5) continue ;

      VL::float_t wgt = VL::fast_expn( r2 / (2*sigmaw*sigmaw) ) ;
      VL::float_t mod = *(pt + xs*xo + ys*yo) ;
      VL::float_t ang = *(pt + xs*xo + ys*yo + 1) ;

      //      int bin = (int) floor( nbins * ang / (2*M_PI) ) ;
      int bin = (int) floor( nbins * ang / (2*M_PI) ) ;
      hist[bin] += mod * wgt ;        
    }
  }
  
  // smooth the histogram
#if defined VL_LOWE_STRICT
  // Lowe's version apparently has a little issue with orientations
  // around + or - pi, which we reproduce here for compatibility
  for (int iter = 0; iter < 6; iter++) {
    VL::float_t prev  = hist[nbins/2] ;
    for (int i = nbins/2-1; i >= -nbins/2 ; --i) {
      int const j  = (i     + nbins) % nbins ;
      int const jp = (i - 1 + nbins) % nbins ;
      VL::float_t newh = (prev + hist[j] + hist[jp]) / 3.0;
      prev = hist[j] ;
      hist[j] = newh ;
    }
  }
#else
  // this is slightly more correct
  for (int iter = 0; iter < 6; iter++) {
    VL::float_t prev  = hist[nbins-1] ;
    VL::float_t first = hist[0] ;
    int i ;
    for (i = 0; i < nbins - 1; i++) {
      VL::float_t newh = (prev + hist[i] + hist[(i+1) % nbins]) / 3.0;
      prev = hist[i] ;
      hist[i] = newh ;
    }
    hist[i] = (prev + hist[i] + first)/3.0 ;
  }
#endif
  
  // find the histogram maximum
  VL::float_t maxh = * std::max_element(hist, hist + nbins) ;

  // find peaks within 80% from max
  int nangles = 0 ;
  for(int i = 0 ; i < nbins ; ++i) {
    VL::float_t h0 = hist [i] ;
    VL::float_t hm = hist [(i-1+nbins) % nbins] ;
    VL::float_t hp = hist [(i+1+nbins) % nbins] ;
    
    // is peak?
    if( h0 > 0.8*maxh && h0 > hm && h0 > hp ) {
      
      // quadratic interpolation
      //      VL::float_t di = -0.5 * (hp-hm) / (hp+hm-2*h0) ; 
      VL::float_t di = -0.5 * (hp-hm) / (hp+hm-2*h0) ; 
      VL::float_t th = 2*M_PI*(i+di+0.5)/nbins ;      
      angles [ nangles++ ] = th ;
    }
  }
  return nangles ;
}

// ===================================================================
//                                         computeKeypointDescriptor()
// -------------------------------------------------------------------

namespace Detail {

/** Normalizes in norm L_2 a descriptor. */
void
normalize_histogram(VL::float_t* L_begin, VL::float_t* L_end)
{
  VL::float_t* L_iter ;
  VL::float_t norm = 0.0 ;

  for(L_iter = L_begin; L_iter != L_end ; ++L_iter)
    norm += (*L_iter) * (*L_iter) ;

  norm = fast_sqrt(norm) ;

  for(L_iter = L_begin; L_iter != L_end ; ++L_iter)
    *L_iter /= (norm + std::numeric_limits<VL::float_t>::epsilon() ) ;
}

}

/** @brief SIFT descriptor
 **
 ** The function computes the descriptor of the keypoint @a keypoint.
 ** The function fills the buffer @a descr_pt which must be large
 ** enough. The funciton uses @a angle0 as rotation of the keypoint.
 ** By calling the function multiple times, different orientations can
 ** be evaluated.
 **
 ** @remark The function needs to compute the gradient modululs and
 ** orientation of the Gaussian scale space octave to which the
 ** keypoint belongs. The result is cached, but discarded if different
 ** octaves are visited. Thereofre it is much quicker to evaluate the
 ** keypoints in their natural octave order.
 **
 ** The function silently abort the computations of keypoints without
 ** the scale space boundaries. See also siftComputeOrientations().
 **/
void
Sift::computeKeypointDescriptor
(VL::float_t* descr_pt,
 Keypoint keypoint, 
 VL::float_t angle0)
{

  /* The SIFT descriptor is a  three dimensional histogram of the position
   * and orientation of the gradient.  There are NBP bins for each spatial
   * dimesions and NBO  bins for the orientation dimesion,  for a total of
   * NBP x NBP x NBO bins.
   *
   * The support  of each  spatial bin  has an extension  of SBP  = 3sigma
   * pixels, where sigma is the scale  of the keypoint.  Thus all the bins
   * together have a  support SBP x NBP pixels wide  . Since weighting and
   * interpolation of  pixel is used, another  half bin is  needed at both
   * ends of  the extension. Therefore, we  need a square window  of SBP x
   * (NBP + 1) pixels. Finally, since the patch can be arbitrarly rotated,
   * we need to consider  a window 2W += sqrt(2) x SBP  x (NBP + 1) pixels
   * wide.
   */      

  // octave
  int o = keypoint.o ;
  VL::float_t xperiod = getOctaveSamplingPeriod(o) ;

  // offsets to move in Gaussian scale space octave
  const int ow = getOctaveWidth(o) ;
  const int oh = getOctaveHeight(o) ;
  const int xo = 2 ;
  const int yo = xo * ow ;
  const int so = yo * oh ;

  // keypoint fractional geometry
  VL::float_t x     = keypoint.x / xperiod;
  VL::float_t y     = keypoint.y / xperiod ;
  VL::float_t sigma = keypoint.sigma / xperiod ;

  VL::float_t st0   = sinf( angle0 ) ;
  VL::float_t ct0   = cosf( angle0 ) ;
  
  // shall we use keypoints.ix,iy,is here?
  int xi = ((int) (x+0.5)) ; 
  int yi = ((int) (y+0.5)) ;
  int si = keypoint.is ;

  const VL::float_t magnif = 3.0f ;
  const int NBO = 8 ;
  const int NBP = 4 ;
  const VL::float_t SBP = magnif * sigma ;
  const int   W = (int) floor (sqrt(2.0) * SBP * (NBP + 1) / 2.0 + 0.5) ;
  
  /* Offsets to move in the descriptor. */
  /* Use Lowe's convention. */
  const int binto = 1 ;
  const int binyo = NBO * NBP ;
  const int binxo = NBO ;
  // const int bino  = NBO * NBP * NBP ;
  
  int bin ;
  
  // check bounds
  if(o  < omin   ||
     o  >=omin+O ||
     xi < 0      || 
     xi > ow-1   || 
     yi < 0      || 
     yi > oh-1   ||
     si < smin+1 ||
     si > smax-2 )
        return ;
  
  // make sure gradient buffer is up-to-date
  prepareGrad(o) ;

  std::fill( descr_pt, descr_pt + NBO*NBP*NBP, 0 ) ;

  /* Center the scale space and the descriptor on the current keypoint. 
   * Note that dpt is pointing to the bin of center (SBP/2,SBP/2,0).
   */
  pixel_t const * pt = temp + xi*xo + yi*yo + (si - smin - 1)*so ;
  VL::float_t *  dpt = descr_pt + (NBP/2) * binyo + (NBP/2) * binxo ;
     
#define atd(dbinx,dbiny,dbint) *(dpt + (dbint)*binto + (dbiny)*binyo + (dbinx)*binxo)
      
  /*
   * Process pixels in the intersection of the image rectangle
   * (1,1)-(M-1,N-1) and the keypoint bounding box.
   */
  for(int dyi = std::max(-W, 1-yi) ; dyi <= std::min(+W, oh-2-yi) ; ++dyi) {
    for(int dxi = std::max(-W, 1-xi) ; dxi <= std::min(+W, ow-2-xi) ; ++dxi) {
      
      // retrieve 
      VL::float_t mod   = *( pt + dxi*xo + dyi*yo + 0 ) ;
      VL::float_t angle = *( pt + dxi*xo + dyi*yo + 1 ) ;
      VL::float_t theta = fast_mod_2pi(-angle + angle0) ; // lowe compatible ?
      
      // fractional displacement
      VL::float_t dx = xi + dxi - x;
      VL::float_t dy = yi + dyi - y;
      
      // get the displacement normalized w.r.t. the keypoint
      // orientation and extension.
      VL::float_t nx = ( ct0 * dx + st0 * dy) / SBP ;
      VL::float_t ny = (-st0 * dx + ct0 * dy) / SBP ; 
      VL::float_t nt = NBO * theta / (2*M_PI) ;
      
      // Get the gaussian weight of the sample. The gaussian window
      // has a standard deviation equal to NBP/2. Note that dx and dy
      // are in the normalized frame, so that -NBP/2 <= dx <= NBP/2.
      VL::float_t const wsigma = NBP/2 ;
      VL::float_t win = VL::fast_expn((nx*nx + ny*ny)/(2.0 * wsigma * wsigma)) ;
      
      // The sample will be distributed in 8 adjacent bins.
      // We start from the ``lower-left'' bin.
      int binx = fast_floor( nx - 0.5 ) ;
      int biny = fast_floor( ny - 0.5 ) ;
      int bint = fast_floor( nt ) ;
      VL::float_t rbinx = nx - (binx+0.5) ;
      VL::float_t rbiny = ny - (biny+0.5) ;
      VL::float_t rbint = nt - bint ;
      int dbinx ;
      int dbiny ;
      int dbint ;

      // Distribute the current sample into the 8 adjacent bins
      for(dbinx = 0 ; dbinx < 2 ; ++dbinx) {
        for(dbiny = 0 ; dbiny < 2 ; ++dbiny) {
          for(dbint = 0 ; dbint < 2 ; ++dbint) {
            
            if( binx+dbinx >= -(NBP/2) &&
                binx+dbinx <   (NBP/2) &&
                biny+dbiny >= -(NBP/2) &&
                biny+dbiny <   (NBP/2) ) {
              VL::float_t weight = win 
                * mod 
                * fast_abs (1 - dbinx - rbinx)
                * fast_abs (1 - dbiny - rbiny)
                * fast_abs (1 - dbint - rbint) ;
              
              atd(binx+dbinx, biny+dbiny, (bint+dbint) % NBO) += weight ;
            }
          }            
        }
      }
    }  
  }
  
  /* Normalize the histogram to L2 unit length. */        
  Detail::normalize_histogram(descr_pt, descr_pt + NBO*NBP*NBP) ;
  
  /* Truncate at 0.2. */
  for(bin = 0; bin < NBO*NBP*NBP ; ++bin) {
    if (descr_pt[bin] > 0.2) descr_pt[bin] = 0.2;
  }
  
  /* Normalize again. */
  Detail::normalize_histogram(descr_pt, descr_pt + NBO*NBP*NBP) ;
}

// namespace VL
}