CVD 0.8
cvd/harris_corner.h
00001 #ifndef CVD_HARRIS_CORNER_H
00002 #define CVD_HARRIS_CORNER_H
00003 
00004 #include <vector>
00005 #include <utility>
00006 #include <algorithm>
00007 #include <cmath>
00008 #include <cstdlib>
00009 
00010 #include <cvd/image.h>
00011 #include <cvd/convolution.h>
00012 
00013 namespace CVD
00014 {
00015     
00016     namespace Harris
00017     {
00019         template<class C> inline C sq(const C& c)
00020         {
00021             return c*c;
00022         }
00023 
00026         struct HarrisScore
00027         {
00028             static float Compute(float xx, float xy, float yy)
00029             {
00030                 return (xx * yy - xy * xy) - 0.04 * sq(xx + yy);
00031             }
00032         };
00033         
00036         struct ShiTomasiScore
00037         {
00038             static float Compute(float xx, float xy, float yy)
00039             {
00040                 float l1 = xx + yy + std::sqrt(sq(xx - yy)+4.0*xy*xy);   
00041                 float l2 = xx + yy - std::sqrt(sq(xx - yy)+4.0*xy*xy);
00042                 return std::min(abs(l1), std::abs(l2));
00043             }
00044         };
00045         
00048         struct PosInserter
00049         {
00050             static void insert(std::vector<ImageRef>& i, const std::pair<float, ImageRef>& p)
00051             {
00052                 i.push_back(p.second);
00053             }
00054         };
00055 
00058         struct PairInserter
00059         {
00060             static void insert(std::vector<std::pair<float, ImageRef> >& i, const std::pair<float, ImageRef>& p)
00061             {
00062                 i.push_back(p);
00063             }
00064         };
00065     }
00066 
00067 
00081     template<class Score, class Inserter, class C, class B> void harrislike_corner_detect(const SubImage<B>& i, C& c, unsigned int N, float blur, float sigmas, BasicImage<float>& xx, BasicImage<float>& xy, BasicImage<float>& yy)
00082     {
00083         using std::pair;
00084         using std::greater;
00085         using std::vector;
00086         using std::make_pair;
00087 
00088         if(! (i.size() == xx.size() && i.size() ==xy.size() && i.size() == yy.size()))
00089             throw Exceptions::Convolution::IncompatibleImageSizes("harrislike_corner_detect");
00090 
00091         zeroBorders(xx);
00092         zeroBorders(xy);
00093         zeroBorders(yy);
00094 
00095         typedef typename Pixel::traits<B>::wider_type gType;
00096         
00097         //Compute gradients
00098         for(int y=1; y < i.size().y - 1; y++)
00099             for(int x=1; x < i.size().x - 1; x++)
00100             {
00101                 
00102                 //FIXME use fast-casting using an arrsy for byte to float conversion.
00103                 gType gx = (gType)i[y][x-1] - i[y][x+1];
00104                 gType gy = (gType)i[y-1][x] - i[y+1][x];
00105                 
00106                 //Compute the gradient moments
00107                 xx[y][x] = gx * gx;
00108                 xy[y][x] = gx * gy;
00109                 yy[y][x] = gy * gy;
00110             }
00111 
00112         convolveGaussian(xx, xx, blur, sigmas);
00113         convolveGaussian(xy, xy, blur, sigmas);
00114         convolveGaussian(yy, yy, blur, sigmas);
00115         
00116         //Avoid computing the score along the image borders where the
00117         //result of the convolution is not valid.
00118         int kspread = (int)ceil(sigmas * blur);
00119 
00120         //Compute harris score
00121         for(int y=kspread; y < i.size().y-kspread; y++)
00122             for(int x=kspread; x <i.size().x-kspread; x++)
00123                 xx[y][x] = Score::Compute(xx[y][x], xy[y][x], yy[y][x]);
00124 
00125         vector<pair<float, ImageRef> > corner_heap;
00126         corner_heap.reserve(N+1);
00127 
00128         typedef greater<pair<float, ImageRef> > minheap_compare;
00129 
00130         //Keep the N best corner scores, using a min-heap. This allows us to always
00131         //remove the smallest element, keeping the largest ones.
00132 
00133         //C++ heap functions use std::less to create a max-heap, growing from the
00134         //beginning of the array. This allows for convenient sorting. pop_heap
00135         //gets the largest element, and the heap is shrunk by 1. This element
00136         //is then put on to the end of the array, ie the spot freed up by shrinking
00137         //the heap by 1. Repeating this procedure will sort the heap, pulling out
00138         //the largest elements and placing them at the end. The resulting array will
00139         //then be sorted by std::less.
00140 
00141         //Therefore we need to use std::greater to create a min-heap
00142 
00143         //The first element in the array will be the smallest value
00144 
00145         //Find local maxima
00146         for(int y=kspread; y < i.size().y-kspread; y++)
00147             for(int x=kspread; x <i.size().x-kspread; x++)
00148             {
00149                 float c = xx[y][x];
00150 
00151                 if( c > xx[y-1][x-1]  &&
00152                     c > xx[y-1][x+0]  &&
00153                     c > xx[y-1][x+1]  &&
00154                     c > xx[y-0][x-1]  &&
00155                     c > xx[y-0][x+1]  &&
00156                     c > xx[y+1][x-1]  &&
00157                     c > xx[y+1][x+0]  &&
00158                     c > xx[y+1][x+1])
00159                 {
00160                     if(corner_heap.size() <= N  || c > corner_heap[0].first)
00161                     {
00162                         corner_heap.push_back(make_pair(c, ImageRef(x,y)));
00163                         push_heap(corner_heap.begin(), corner_heap.end(), minheap_compare());
00164                     }
00165 
00166                     if(corner_heap.size() > N)
00167                     {
00168                         pop_heap(corner_heap.begin(), corner_heap.end(), minheap_compare());
00169                         corner_heap.pop_back();
00170                     }
00171                 }
00172             }
00173 
00174         for(unsigned int i=0; i < corner_heap.size(); i++)
00175             Inserter::insert(c, corner_heap[i]);
00176     }
00177 
00178     template<class C> void harris_corner_detect(const SubImage<C>& i, std::vector<ImageRef>& c, unsigned int N, float blur=1.0, float sigmas = 3.0)
00179     {
00180         Image<float> xx(i.size()), xy(i.size()), yy(i.size());
00181         harrislike_corner_detect<Harris::HarrisScore, Harris::PosInserter>(i, c, N, blur, sigmas, xx, xy, yy);
00182     }
00183 
00184     template<class C> void shitomasi_corner_detect(const SubImage<C>& i, std::vector<ImageRef>& c, unsigned int N, float blur=1.0, float sigmas = 3.0)
00185     {
00186         Image<float> xx(i.size()), xy(i.size()), yy(i.size());
00187         harrislike_corner_detect<Harris::ShiTomasiScore, Harris::PosInserter>(i, c, N, blur, sigmas, xx, xy, yy);
00188     }
00189 }
00190 #endif