faster_corner_12.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

// ---------------------------------------------------------------------------
// LICENSING: This file is a slightly-modified version of part of libcvd,
//             released under LGPL 2.1 by Edward Rosten
// ---------------------------------------------------------------------------

#include <mrpt/utils/utils_defs.h>
#include <mrpt/system/memory.h>
#include "faster_corner_prototypes.h"

#include <mrpt/utils/SSE_types.h>
#include "faster_corner_utilities.h"
#include "corner_12.h"

using namespace std;
using namespace mrpt;
using namespace mrpt::utils;


template <int I, int N> struct BitCheck
{
        template <class C> static inline void eval(unsigned int three, const uint8_t* p, const int w, const int barrier, C& corners) {
                const int BIT = 1<<I;
                if (three & BIT) {
                if (three & (BIT << 16)) {
                        if (is_corner_12<Greater>(p, w, barrier))
                        corners.push_back(p);
                } else {
                        if (is_corner_12<Less>(p, w, barrier))
                        corners.push_back(p);
                }
                }
                BitCheck<I+1,N>::eval(three, p+1, w, barrier, corners);
        }
};

template <int N> struct BitCheck<N,N>
{
        template <class C> static inline void eval(unsigned int, const uint8_t* , const int , const int , C& ) {}
};

template <int CHUNKS, class C> inline void process_16(unsigned int three, const uint8_t* p, const int w, const int barrier, C& corners)
{
three |= (three >> 16);
const int BITS = 16/CHUNKS;
const int mask = ((1<<BITS)-1);
for (int i=0; i<CHUNKS; ++i) {
    if (three & mask)
        BitCheck<0,BITS>::eval(three, p, w, barrier, corners);
    p += BITS;
    three >>= BITS;
}
}


#if MRPT_HAS_SSE2 && MRPT_HAS_OPENCV

template <bool Aligned>
void faster_corner_detect_12(const IplImage* I, mrpt::vision::TSimpleFeatureList & corners, int barrier, uint8_t octave, std::vector<size_t> * out_feats_index_by_row)
{
        corners.reserve(corners.size()+500);
        //corners.mark_kdtree_as_outdated();

        size_t *ptr_feat_index_by_row, *ptr_feat_index_end;
        if (out_feats_index_by_row)
        {
                out_feats_index_by_row->resize(I->height);
                ptr_feat_index_by_row = &(*out_feats_index_by_row)[0];
                ptr_feat_index_end = ptr_feat_index_by_row+out_feats_index_by_row->size();
        }
        else {
                ptr_feat_index_by_row = NULL;
                ptr_feat_index_end=NULL;
        }

const int w = I->width;
const int stride = 3*I->widthStep; // 3*w;
typedef std::list<const uint8_t*> Passed;
Passed passed;

// The compiler refuses to reserve a register for this,
// even though xmm6 and xmm7 go unused.
// It loads it from memory each time.  I am stymied.
const __m128i barriers = _mm_set1_epi8((uint8_t)barrier);

for (int i=3; i<I->height-3; ++i) {
        const uint8_t* p = (const uint8_t*)I->imageData + I->widthStep*i; //  I[i];
    for (int j=0; j<w/16; ++j, p+=16) {
        __m128i lo, hi;
        {
            const __m128i here = load_si128<Aligned>((const __m128i*)(p));
            lo = _mm_subs_epu8(here, barriers);
            hi = _mm_adds_epu8(barriers, here);
        }
        const __m128i above = load_si128<Aligned>((const __m128i*)(p-stride));
        const __m128i below = load_si128<Aligned>((const __m128i*)(p+stride));
        unsigned int up_flags, down_flags;
        CHECK_BARRIER(lo, hi, above, up_flags);
        CHECK_BARRIER(lo, hi, below, down_flags);
        const unsigned int either_ud = up_flags | down_flags;
        if (either_ud) {
            unsigned int left_flags;
            {
                const __m128i other = _mm_loadu_si128((const __m128i*)(p-3));
                CHECK_BARRIER(lo, hi, other, left_flags);
            }
            const unsigned int both_ud = up_flags & down_flags;
            if (both_ud | (either_ud&left_flags)) {
                unsigned int right_flags;
                {
                    const __m128i other = _mm_loadu_si128((const __m128i*)(p+3));
                    CHECK_BARRIER(lo, hi, other, right_flags);
                }
                const unsigned int at_least_three = (either_ud & (left_flags & right_flags)) | (both_ud & (left_flags | right_flags));
                if (at_least_three) {
                    process_16<4>(at_least_three, p, w, barrier, passed);
                }
            }
        }
    }

    //Do the edge of the row, using the old-fasioned 4 point test
    for(int j=(w/16) * 16; j < w-3; j++, p++)
    {
        int cb = *p + barrier;
        int c_b = *p - barrier;
        int num_above= (p[stride] > cb) + (p[-stride] > cb);
        int num_below= (p[stride] < c_b) + (p[-stride] < c_b);

        if(!num_above && !num_below)
            continue;

        //Look left
        num_above+= p[-3] > cb;
        num_below+= p[-3] < c_b;

        if(num_above & 2) //num_above is 2 or 3
        {
            if(!(num_above & 1)) //num_above is 2
                num_above += p[3] > cb;


            //Only do a complete check if num_above is 3
            if((num_above&1) && is_corner_12<Greater>(p, w, barrier))
                passed.push_back(p);
        }
        else if(num_below & 2)
        {
            if(!(num_below & 1))
                num_below += p[3] < c_b;


            if((num_below&1) && is_corner_12<Less>(p, w, barrier))
                passed.push_back(p);
        }
    }

    passed.push_back(0);
}

        // 3 first rows have no features:
        if (ptr_feat_index_by_row) {
                *ptr_feat_index_by_row++ = corners.size();
                *ptr_feat_index_by_row++ = corners.size();
                *ptr_feat_index_by_row++ = corners.size();
        }

        int row = 3;

        const uint8_t* row_start = (const uint8_t*)I->imageData + I->widthStep*3;
        for (Passed::iterator it = passed.begin(); it != passed.end(); ++it) {
                if (*it == 0)
                {
                        // next row:
                        if (ptr_feat_index_by_row)  // save index by row:
                                *ptr_feat_index_by_row++=corners.size();

                        row_start= (const uint8_t*)I->imageData + I->widthStep*(++row); //  I[++row];
                        continue;
                }
                int x = *it - row_start;
                if (x > 2 && x < w-3)
                corners.push_back_fast(x<<octave, row<<octave);
        }

        // 3 last rows have no features:
        if (ptr_feat_index_by_row) {
                *ptr_feat_index_by_row++ = corners.size();
                *ptr_feat_index_by_row++ = corners.size();
                *ptr_feat_index_by_row++ = corners.size();
                ASSERT_(ptr_feat_index_by_row==ptr_feat_index_end)
        }

}


#endif // MRPT_HAS_SSE2 && MRPT_HAS_OPENCV


#if MRPT_HAS_OPENCV

void fast_corner_detect_12(const IplImage* I, mrpt::vision::TSimpleFeatureList & corners, int barrier, uint8_t octave, std::vector<size_t> * out_feats_index_by_row)
{
        if (I->width < 22)
        {
                fast_corner_detect_plain_12(I,corners,barrier, octave,out_feats_index_by_row);
                return;
        }
        else if (I->width < 22 || I->height < 7)
                return;

#if MRPT_HAS_SSE2
        if (mrpt::system::is_aligned<16>(I->imageData) && mrpt::system::is_aligned<16>(I->imageData+I->widthStep))
                faster_corner_detect_12<true>(I, corners, barrier, octave,out_feats_index_by_row);
        else
                faster_corner_detect_12<false>(I, corners, barrier, octave,out_feats_index_by_row);
#else
        fast_corner_detect_plain_12(I,corners,barrier, octave,out_feats_index_by_row);
#endif
}
#endif