[libcvd-members] libcvd/cvd harris_corner.h

[Edward Rosten]

CVSROOT:        /cvsroot/libcvd
Module name:    libcvd
Changes by:     Edward Rosten <edrosten>        09/02/09 17:19:39

Added files:
        cvd            : harris_corner.h 

Log message:
        Implementation of Harris and Shi-Tomasi corners.
        
        Initial version, so the interface may not be stable. Feel free to hack 
this
        around.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd/harris_corner.h?cvsroot=libcvd&rev=1.1

Patches:
Index: harris_corner.h
===================================================================
RCS file: harris_corner.h
diff -N harris_corner.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ harris_corner.h     9 Feb 2009 17:19:39 -0000       1.1
@@ -0,0 +1,190 @@
+#ifndef CVD_HARRIS_CORNER_H
+#define CVD_HARRIS_CORNER_H
+
+#include <vector>
+#include <utility>
+#include <algorithm>
+#include <cmath>
+#include <cstdlib>
+
+#include <cvd/image.h>
+#include <cvd/convolution.h>
+
+namespace CVD
+{
+       
+       namespace Harris
+       {
+               ///A Utility fuction.
+               template<class C> inline C sq(const C& c)
+               {
+                       return c*c;
+               }
+
+               ///Compute the corner score according to Harris
+               ///@ingroup gVision
+               struct HarrisScore
+               {
+                       static float Compute(float xx, float xy, float yy)
+                       {
+                               return (xx * yy - xy * xy) - 0.04 * sq(xx + yy);
+                       }
+               };
+               
+               ///Compute the score according to Shi-Tomasi
+               ///@ingroup gVision
+               struct ShiTomasiScore
+               {
+                       static float Compute(float xx, float xy, float yy)
+                       {
+                               float l1 = xx + yy + std::sqrt(sq(xx - 
yy)+4.0*xy*xy);   
+                               float l2 = xx + yy - std::sqrt(sq(xx - 
yy)+4.0*xy*xy);
+                               return std::min(abs(l1), std::abs(l2));
+                       }
+               };
+               
+               ///Used to save corner positions from harrislike_corner_detect
+               ///@ingroup gVision
+               struct PosInserter
+               {
+                       static void insert(std::vector<ImageRef>& i, const 
std::pair<float, ImageRef>& p)
+                       {
+                               i.push_back(p.second);
+                       }
+               };
+
+               ///Used to save corner positions and scores from 
harrislike_corner_detect
+               ///@ingroup gVision
+               struct PairInserter
+               {
+                       static void insert(std::vector<std::pair<float, 
ImageRef> >& i, const std::pair<float, ImageRef>& p)
+                       {
+                               i.push_back(p);
+                       }
+               };
+       }
+
+
+       /// Generic Harris corner detection function. This can use any scoring 
metric and
+       /// can store corners in any container. The images used to hold the 
intermediate
+       /// results must be passed to this function.
+       ///
+       ///@param i Input image.
+       ///@param c Container holding detected corners
+       ///@param N Number of corners to detect
+       ///@param blur Blur radius to use
+       ///@param sigmas Number of sigmas to use in blur.
+       ///@param xx Holds the result of blurred, squared X gradient.
+       ///@param xy Holds the result of blurred, X times Y gradient.
+       ///@param yy Holds the result of blurred, squared Y gradient.
+       ///@ingroup gVision
+       template<class Score, class Inserter, class C, class B> void 
harrislike_corner_detect(const BasicImage<B>& i, C& c, unsigned int N, float 
blur, float sigmas, BasicImage<float>& xx, BasicImage<float>& xy, 
BasicImage<float>& yy)
+       {
+               using std::pair;
+               using std::greater;
+               using std::vector;
+               using std::make_pair;
+
+               if(! (i.size() == xx.size() && i.size() ==xy.size() && i.size() 
== yy.size()))
+                       throw 
Exceptions::Convolution::IncompatibleImageSizes("harrislike_corner_detect");
+
+               zeroBorders(xx);
+               zeroBorders(xy);
+               zeroBorders(yy);
+
+               typedef typename Pixel::traits<B>::wider_type gType;
+               
+               //Compute gradients
+               for(int y=1; y < i.size().y - 1; y++)
+                       for(int x=1; x < i.size().x - 1; x++)
+                       {
+                               
+                               //FIXME use fast-casting using an arrsy for 
byte to float conversion.
+                               gType gx = (gType)i[y][x-1] - i[y][x+1];
+                               gType gy = (gType)i[y-1][x] - i[y+1][x];
+                               
+                               //Compute the gradient moments
+                               xx[y][x] = gx * gx;
+                               xy[y][x] = gx * gy;
+                               yy[y][x] = gy * gy;
+                       }
+
+               convolveGaussian(xx, xx, blur, sigmas);
+               convolveGaussian(xy, xy, blur, sigmas);
+               convolveGaussian(yy, yy, blur, sigmas);
+               
+               //Avoid computing the score along the image borders where the
+               //result of the convolution is not valid.
+               int     kspread = (int)ceil(sigmas * blur);
+
+               //Compute harris score
+               for(int y=kspread; y < i.size().y-kspread; y++)
+                       for(int x=kspread; x <i.size().x-kspread; x++)
+                               xx[y][x] = Score::Compute(xx[y][x], xy[y][x], 
yy[y][x]);
+
+               vector<pair<float, ImageRef> > corner_heap;
+               corner_heap.reserve(N+1);
+
+               typedef greater<pair<float, ImageRef> > minheap_compare;
+
+               //Keep the N best corner scores, using a min-heap. This allows 
us to always
+               //remove the smallest element, keeping the largest ones.
+
+               //C++ heap functions use std::less to create a max-heap, 
growing from the
+               //beginning of the array. This allows for convenient sorting. 
pop_heap
+               //gets the largest element, and the heap is shrunk by 1. This 
element
+               //is then put on to the end of the array, ie the spot freed up 
by shrinking
+               //the heap by 1. Repeating this procedure will sort the heap, 
pulling out
+               //the largest elements and placing them at the end. The 
resulting array will
+               //then be sorted by std::less.
+
+               //Therefore we need to use std::greater to create a min-heap
+
+               //The first element in the array will be the smallest value
+
+               //Find local maxima
+               for(int y=kspread; y < i.size().y-kspread; y++)
+                       for(int x=kspread; x <i.size().x-kspread; x++)
+                       {
+                               float c = xx[y][x];
+
+                               if( c > xx[y-1][x-1]  &&
+                                       c > xx[y-1][x+0]  &&
+                                       c > xx[y-1][x+1]  &&
+                                       c > xx[y-0][x-1]  &&
+                                       c > xx[y-0][x+1]  &&
+                                       c > xx[y+1][x-1]  &&
+                                       c > xx[y+1][x+0]  &&
+                                       c > xx[y+1][x+1])
+                               {
+                                       if(corner_heap.size() <= N  || c > 
corner_heap[0].first)
+                                       {
+                                               
corner_heap.push_back(make_pair(c, ImageRef(x,y)));
+                                               push_heap(corner_heap.begin(), 
corner_heap.end(), minheap_compare());
+                                       }
+
+                                       if(corner_heap.size() > N)
+                                       {
+                                               pop_heap(corner_heap.begin(), 
corner_heap.end(), minheap_compare());
+                                               corner_heap.pop_back();
+                                       }
+                               }
+                       }
+
+               for(unsigned int i=0; i < corner_heap.size(); i++)
+                       Inserter::insert(c, corner_heap[i]);
+       }
+
+       template<class C> void harris_corner_detect(const Image<C>& i, 
std::vector<ImageRef>& c, unsigned int N, float blur=1.0, float sigmas = 3.0)
+       {
+               Image<float> xx(i.size()), xy(i.size()), yy(i.size());
+               harrislike_corner_detect<Harris::HarrisScore, 
Harris::PosInserter>(i, c, N, blur, sigmas, xx, xy, yy);
+       }
+
+       template<class C> void shitomasi_corner_detect(const Image<C>& i, 
std::vector<ImageRef>& c, unsigned int N, float blur=1.0, float sigmas = 3.0)
+       {
+               Image<float> xx(i.size()), xy(i.size()), yy(i.size());
+               harrislike_corner_detect<Harris::ShiTomasiScore, 
Harris::PosInserter>(i, c, N, blur, sigmas, xx, xy, yy);
+       }
+}
+#endif