harris_corner.h

#ifndef CVD_HARRIS_CORNER_H
#define CVD_HARRIS_CORNER_H

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <utility>
#include <vector>

#include <cvd/convolution.h>
#include <cvd/image.h>

namespace CVD
{

namespace Harris
{
    template <class C>
    inline C sq(const C& c)
    {
        return c * c;
    }

    struct HarrisScore
    {
        static float Compute(float xx, float xy, float yy)
        {
            return (xx * yy - xy * xy) - 0.04 * sq(xx + yy);
        }
    };

    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));
        }
    };

    struct PosInserter
    {
        static void insert(std::vector<ImageRef>& i, const std::pair<float, ImageRef>& p)
        {
            i.push_back(p.second);
        }
    };

    struct PairInserter
    {
        static void insert(std::vector<std::pair<float, ImageRef>>& i, const std::pair<float, ImageRef>& p)
        {
            i.push_back(p);
        }
    };
}

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::greater;
    using std::make_pair;
    using std::pair;
    using std::vector;

    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 BasicImage<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 BasicImage<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