From 1c580151f473894332bb4fe613be9ef713cf1dd0 Mon Sep 17 00:00:00 2001 From: baudenri Date: Thu, 19 Nov 2015 13:45:35 +0100 Subject: [PATCH 1/2] Enabled FAST feature detection on specific image locations Some applications only want to know if there are feature at specific locations. To fit these needs the FAST approach is extended in FASTForPointSet to recieve a vector of locations and calculates the FAST response on these positions. If it is below the threshold, it will be removed from the list. --- .../include/opencv2/xfeatures2d.hpp | 26 + modules/xfeatures2d/src/fast.cpp | 513 ++++++++++++++++++ 2 files changed, 539 insertions(+) create mode 100644 modules/xfeatures2d/src/fast.cpp diff --git a/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp b/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp index a194c9662..9d0d2e5ec 100644 --- a/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp +++ b/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp @@ -259,6 +259,32 @@ public: }; + +/** @brief Estimates cornerness for prespecified KeyPoints using the FAST algorithm + +@param image grayscale image where keypoints (corners) are detected. +@param keypoints keypoints which should be tested to fit the FAST criteria. Keypoints not beeing +detected as corners are removed. +@param threshold threshold on difference between intensity of the central pixel and pixels of a +circle around this pixel. +@param nonmaxSuppression if true, non-maximum suppression is applied to detected corners +(keypoints). +@param type one of the three neighborhoods as defined in the paper: +FastFeatureDetector::TYPE_9_16, FastFeatureDetector::TYPE_7_12, +FastFeatureDetector::TYPE_5_8 + +Detects corners using the FAST algorithm by @cite Rosten06 . + */ +CV_EXPORTS void FASTForPointSet( InputArray image, CV_IN_OUT std::vector& keypoints, + int threshold, bool nonmaxSuppression=true, int type=FastFeatureDetector::TYPE_9_16); + + +void makeOffsets(int pixel[25], int row_stride, int patternSize); + +template +int cornerScore(const uchar* ptr, const int pixel[], int threshold); + + //! @} } diff --git a/modules/xfeatures2d/src/fast.cpp b/modules/xfeatures2d/src/fast.cpp new file mode 100644 index 000000000..0699a3b38 --- /dev/null +++ b/modules/xfeatures2d/src/fast.cpp @@ -0,0 +1,513 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include + +#ifndef VERIFY_CORNERS +#define VERIFY_CORNERS 0 +#endif + +namespace cv { + namespace xfeatures2d { + + + #if VERIFY_CORNERS + static void testCorner(const uchar* ptr, const int pixel[], int K, int N, int threshold) { + // check that with the computed "threshold" the pixel is still a corner + // and that with the increased-by-1 "threshold" the pixel is not a corner anymore + for( int delta = 0; delta <= 1; delta++ ) + { + int v0 = std::min(ptr[0] + threshold + delta, 255); + int v1 = std::max(ptr[0] - threshold - delta, 0); + int c0 = 0, c1 = 0; + + for( int k = 0; k < N; k++ ) + { + int x = ptr[pixel[k]]; + if(x > v0) + { + if( ++c0 > K ) + break; + c1 = 0; + } + else if( x < v1 ) + { + if( ++c1 > K ) + break; + c0 = 0; + } + else + { + c0 = c1 = 0; + } + } + CV_Assert( (delta == 0 && std::max(c0, c1) > K) || + (delta == 1 && std::max(c0, c1) <= K) ); + } + } + #endif + + template<> + int cornerScore<16>(const uchar* ptr, const int pixel[], int threshold) + { + const int K = 8, N = K*3 + 1; + int k, v = ptr[0]; + short d[N]; + for( k = 0; k < N; k++ ) + d[k] = (short)(v - ptr[pixel[k]]); + + #if CV_SSE2 + __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000); + for( k = 0; k < 16; k += 8 ) + { + __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1)); + __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2)); + __m128i a = _mm_min_epi16(v0, v1); + __m128i b = _mm_max_epi16(v0, v1); + v0 = _mm_loadu_si128((__m128i*)(d+k+3)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+4)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+5)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+6)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+7)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+8)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + v0 = _mm_loadu_si128((__m128i*)(d+k+9)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + } + q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); + q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); + threshold = (short)_mm_cvtsi128_si32(q0) - 1; + #else + int a0 = threshold; + for( k = 0; k < 16; k += 2 ) + { + int a = std::min((int)d[k+1], (int)d[k+2]); + a = std::min(a, (int)d[k+3]); + if( a <= a0 ) + continue; + a = std::min(a, (int)d[k+4]); + a = std::min(a, (int)d[k+5]); + a = std::min(a, (int)d[k+6]); + a = std::min(a, (int)d[k+7]); + a = std::min(a, (int)d[k+8]); + a0 = std::max(a0, std::min(a, (int)d[k])); + a0 = std::max(a0, std::min(a, (int)d[k+9])); + } + + int b0 = -a0; + for( k = 0; k < 16; k += 2 ) + { + int b = std::max((int)d[k+1], (int)d[k+2]); + b = std::max(b, (int)d[k+3]); + b = std::max(b, (int)d[k+4]); + b = std::max(b, (int)d[k+5]); + if( b >= b0 ) + continue; + b = std::max(b, (int)d[k+6]); + b = std::max(b, (int)d[k+7]); + b = std::max(b, (int)d[k+8]); + + b0 = std::min(b0, std::max(b, (int)d[k])); + b0 = std::min(b0, std::max(b, (int)d[k+9])); + } + + threshold = -b0-1; + #endif + + #if VERIFY_CORNERS + testCorner(ptr, pixel, K, N, threshold); + #endif + return threshold; + } + + template<> + int cornerScore<12>(const uchar* ptr, const int pixel[], int threshold) + { + const int K = 6, N = K*3 + 1; + int k, v = ptr[0]; + short d[N + 4]; + for( k = 0; k < N; k++ ) + d[k] = (short)(v - ptr[pixel[k]]); + #if CV_SSE2 + for( k = 0; k < 4; k++ ) + d[N+k] = d[k]; + #endif + + #if CV_SSE2 + __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000); + for( k = 0; k < 16; k += 8 ) + { + __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1)); + __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2)); + __m128i a = _mm_min_epi16(v0, v1); + __m128i b = _mm_max_epi16(v0, v1); + v0 = _mm_loadu_si128((__m128i*)(d+k+3)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+4)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+5)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+6)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + v0 = _mm_loadu_si128((__m128i*)(d+k+7)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + } + q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); + q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); + threshold = (short)_mm_cvtsi128_si32(q0) - 1; + #else + int a0 = threshold; + for( k = 0; k < 12; k += 2 ) + { + int a = std::min((int)d[k+1], (int)d[k+2]); + if( a <= a0 ) + continue; + a = std::min(a, (int)d[k+3]); + a = std::min(a, (int)d[k+4]); + a = std::min(a, (int)d[k+5]); + a = std::min(a, (int)d[k+6]); + a0 = std::max(a0, std::min(a, (int)d[k])); + a0 = std::max(a0, std::min(a, (int)d[k+7])); + } + + int b0 = -a0; + for( k = 0; k < 12; k += 2 ) + { + int b = std::max((int)d[k+1], (int)d[k+2]); + b = std::max(b, (int)d[k+3]); + b = std::max(b, (int)d[k+4]); + if( b >= b0 ) + continue; + b = std::max(b, (int)d[k+5]); + b = std::max(b, (int)d[k+6]); + + b0 = std::min(b0, std::max(b, (int)d[k])); + b0 = std::min(b0, std::max(b, (int)d[k+7])); + } + + threshold = -b0-1; + #endif + + #if VERIFY_CORNERS + testCorner(ptr, pixel, K, N, threshold); + #endif + return threshold; + } + + template<> + int cornerScore<8>(const uchar* ptr, const int pixel[], int threshold) + { + const int K = 4, N = K*3 + 1; + int k, v = ptr[0]; + short d[N]; + for( k = 0; k < N; k++ ) + d[k] = (short)(v - ptr[pixel[k]]); + + #if CV_SSE2 + __m128i v0 = _mm_loadu_si128((__m128i*)(d+1)); + __m128i v1 = _mm_loadu_si128((__m128i*)(d+2)); + __m128i a = _mm_min_epi16(v0, v1); + __m128i b = _mm_max_epi16(v0, v1); + v0 = _mm_loadu_si128((__m128i*)(d+3)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+4)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d)); + __m128i q0 = _mm_min_epi16(a, v0); + __m128i q1 = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+5)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); + q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); + threshold = (short)_mm_cvtsi128_si32(q0) - 1; + #else + int a0 = threshold; + for( k = 0; k < 8; k += 2 ) + { + int a = std::min((int)d[k+1], (int)d[k+2]); + if( a <= a0 ) + continue; + a = std::min(a, (int)d[k+3]); + a = std::min(a, (int)d[k+4]); + a0 = std::max(a0, std::min(a, (int)d[k])); + a0 = std::max(a0, std::min(a, (int)d[k+5])); + } + + int b0 = -a0; + for( k = 0; k < 8; k += 2 ) + { + int b = std::max((int)d[k+1], (int)d[k+2]); + b = std::max(b, (int)d[k+3]); + if( b >= b0 ) + continue; + b = std::max(b, (int)d[k+4]); + + b0 = std::min(b0, std::max(b, (int)d[k])); + b0 = std::min(b0, std::max(b, (int)d[k+5])); + } + + threshold = -b0-1; + #endif + + #if VERIFY_CORNERS + testCorner(ptr, pixel, K, N, threshold); + #endif + return threshold; + } + + void makeOffsets(int pixel[25], int rowStride, int patternSize) + { + static const int offsets16[][2] = + { + {0, 3}, { 1, 3}, { 2, 2}, { 3, 1}, { 3, 0}, { 3, -1}, { 2, -2}, { 1, -3}, + {0, -3}, {-1, -3}, {-2, -2}, {-3, -1}, {-3, 0}, {-3, 1}, {-2, 2}, {-1, 3} + }; + + static const int offsets12[][2] = + { + {0, 2}, { 1, 2}, { 2, 1}, { 2, 0}, { 2, -1}, { 1, -2}, + {0, -2}, {-1, -2}, {-2, -1}, {-2, 0}, {-2, 1}, {-1, 2} + }; + + static const int offsets8[][2] = + { + {0, 1}, { 1, 1}, { 1, 0}, { 1, -1}, + {0, -1}, {-1, -1}, {-1, 0}, {-1, 1} + }; + + const int (*offsets)[2] = patternSize == 16 ? offsets16 : + patternSize == 12 ? offsets12 : + patternSize == 8 ? offsets8 : 0; + + CV_Assert(pixel && offsets); + + int k = 0; + for( ; k < patternSize; k++ ) + pixel[k] = offsets[k][0] + offsets[k][1] * rowStride; + for( ; k < 25; k++ ) + pixel[k] = pixel[k - patternSize]; + } + + template + void FASTForPointSet_t( InputArray image, std::vector& keypoints, int threshold, bool nonmaxSuppression ) { + + Mat img = image.getMat(); + const int K = patternSize/2, N = patternSize + K + 1; + + int i, k, pixel[25]; + makeOffsets(pixel, (int)img.step, patternSize); + + keypoints.clear(); + + threshold = std::min(std::max(threshold, 0), 255); + + uchar threshold_tab[512]; + for( i = -255; i <= 255; i++ ) + threshold_tab[i+255] = (uchar)(i < -threshold ? 1 : i > threshold ? 2 : 0); + + AutoBuffer _buf((img.cols+16)*3*(sizeof(int) + sizeof(uchar)) + 128); + uchar* buf[3]; + buf[0] = _buf; buf[1] = buf[0] + img.cols; buf[2] = buf[1] + img.cols; + int* cpbuf[3]; + cpbuf[0] = (int*)alignPtr(buf[2] + img.cols, sizeof(int)) + 1; + cpbuf[1] = cpbuf[0] + img.cols + 1; + cpbuf[2] = cpbuf[1] + img.cols + 1; + memset(buf[0], 0, img.cols*3); + + // Calculate threshold for the keypoints + for (size_t keyPointIdx=0; keyPointIdx < keypoints.size(); keyPointIdx++) { + // Set response to -1: + // All keypoints with response <= 0 will be removed afterwards + keypoints[keyPointIdx].response = -1; + + // Poiter to keyPoint in image + Point keyPoint = keypoints[keyPointIdx].pt; + const uchar* ptr = img.ptr(keyPoint.y, keyPoint.x); + + // value of the pixel at certain position + int v = ptr[0]; + + // Initialize Lookup table + // If k=v --> tab[k] is at the center of the thrshold table + // The threshold table is made as follows: + // -255 -threshold 0 +threshold 255 + // 111111111111111111|0000000000000|0000000000000|222222222222222 + const uchar* tab = &threshold_tab[0] - v + 255; + + // Calculate the fast value + int d = tab[ptr[pixel[0]]] | tab[ptr[pixel[8]]]; + + if( d == 0 ) + continue; + + d &= tab[ptr[pixel[2]]] | tab[ptr[pixel[10]]]; + d &= tab[ptr[pixel[4]]] | tab[ptr[pixel[12]]]; + d &= tab[ptr[pixel[6]]] | tab[ptr[pixel[14]]]; + + if( d == 0 ) + continue; + + d &= tab[ptr[pixel[1]]] | tab[ptr[pixel[9]]]; + d &= tab[ptr[pixel[3]]] | tab[ptr[pixel[11]]]; + d &= tab[ptr[pixel[5]]] | tab[ptr[pixel[13]]]; + d &= tab[ptr[pixel[7]]] | tab[ptr[pixel[15]]]; + + // For at least half pixels darker than v count the number + if( d & 1 ) + { + int vt = v - threshold, count = 0; + + for(k = 0; k < N; k++ ) + { + int x = ptr[pixel[k]]; + if(x < vt) + { + if( ++count > K ) + { + // Calculate score + keypoints[keyPointIdx].response = (uchar)cornerScore(ptr, pixel, threshold); + // Non Maxima Supression I + if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) { + keypoints[keyPointIdx-1].response = -1; + } + break; + } + } + else + count = 0; + } + } + + // For at least half pixels brighter than v count the number + if(d & 2 ) + { + int vt = v + threshold, count = 0; + + for(k = 0; k < N; k++ ) + { + int x = ptr[pixel[k]]; + if(x > vt) + { + if( ++count > K ) + { + // Calculate score + keypoints[keyPointIdx].response = (uchar)cornerScore(ptr, pixel, threshold); + // Non Maxima Suppression I + if (nonmaxSuppression && keyPointIdx>0 &&keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) { + keypoints[keyPointIdx-1].response = -1; + } + break; + } + } + else + count = 0; + } + } + + } + + // Remove unused Keypoints + size_t maxKeypointSize = keypoints.size(); + for (size_t keyPointIdx=maxKeypointSize; keyPointIdx > 0;) { + keyPointIdx--; + if (keypoints[keyPointIdx].response <= 0) { + keypoints.erase(keypoints.begin() + keyPointIdx); + } else if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response > keypoints[keyPointIdx].response) { + // Non Maxima Suppression II + keypoints.erase(keypoints.begin() + keyPointIdx); + } + } + } + + void FASTForPointSet(InputArray _img, std::vector& keypoints, int threshold, bool nonmax_suppression, int type) + { + if (keypoints.empty()) { + FAST(_img, keypoints, threshold, nonmax_suppression, type); + return; + } + + switch(type) { + case FastFeatureDetector::TYPE_5_8: + FASTForPointSet_t<8>(_img, keypoints, threshold, nonmax_suppression); + break; + case FastFeatureDetector::TYPE_7_12: + FASTForPointSet_t<12>(_img, keypoints, threshold, nonmax_suppression); + break; + case FastFeatureDetector::TYPE_9_16: + FASTForPointSet_t<16>(_img, keypoints, threshold, nonmax_suppression); + break; + } + } + } +} From 7e46ea166c295d6145d48c626b627ad39279085d Mon Sep 17 00:00:00 2001 From: Vitaly Tuzov Date: Fri, 29 Sep 2017 17:40:45 +0300 Subject: [PATCH 2/2] Removed helper functions from header. Updated module license text. --- .../include/opencv2/xfeatures2d.hpp | 6 - modules/xfeatures2d/src/fast.cpp | 799 +++++++++--------- 2 files changed, 384 insertions(+), 421 deletions(-) diff --git a/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp b/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp index 9d0d2e5ec..af86cf4cc 100644 --- a/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp +++ b/modules/xfeatures2d/include/opencv2/xfeatures2d.hpp @@ -279,12 +279,6 @@ CV_EXPORTS void FASTForPointSet( InputArray image, CV_IN_OUT std::vector -int cornerScore(const uchar* ptr, const int pixel[], int threshold); - - //! @} } diff --git a/modules/xfeatures2d/src/fast.cpp b/modules/xfeatures2d/src/fast.cpp index 0699a3b38..6fbae3029 100644 --- a/modules/xfeatures2d/src/fast.cpp +++ b/modules/xfeatures2d/src/fast.cpp @@ -1,44 +1,6 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. -// Copyright (C) 2009, Willow Garage Inc., all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of the copyright holders may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html. #include @@ -46,449 +8,455 @@ #define VERIFY_CORNERS 0 #endif -namespace cv { - namespace xfeatures2d { - +namespace { + using namespace cv; + #if VERIFY_CORNERS + void testCorner(const uchar* ptr, const int pixel[], int K, int N, int threshold) { + // check that with the computed "threshold" the pixel is still a corner + // and that with the increased-by-1 "threshold" the pixel is not a corner anymore + for( int delta = 0; delta <= 1; delta++ ) + { + int v0 = std::min(ptr[0] + threshold + delta, 255); + int v1 = std::max(ptr[0] - threshold - delta, 0); + int c0 = 0, c1 = 0; - #if VERIFY_CORNERS - static void testCorner(const uchar* ptr, const int pixel[], int K, int N, int threshold) { - // check that with the computed "threshold" the pixel is still a corner - // and that with the increased-by-1 "threshold" the pixel is not a corner anymore - for( int delta = 0; delta <= 1; delta++ ) + for( int k = 0; k < N; k++ ) { - int v0 = std::min(ptr[0] + threshold + delta, 255); - int v1 = std::max(ptr[0] - threshold - delta, 0); - int c0 = 0, c1 = 0; - - for( int k = 0; k < N; k++ ) + int x = ptr[pixel[k]]; + if(x > v0) { - int x = ptr[pixel[k]]; - if(x > v0) - { - if( ++c0 > K ) - break; - c1 = 0; - } - else if( x < v1 ) - { - if( ++c1 > K ) - break; - c0 = 0; - } - else - { - c0 = c1 = 0; - } + if( ++c0 > K ) + break; + c1 = 0; + } + else if( x < v1 ) + { + if( ++c1 > K ) + break; + c0 = 0; + } + else + { + c0 = c1 = 0; } - CV_Assert( (delta == 0 && std::max(c0, c1) > K) || - (delta == 1 && std::max(c0, c1) <= K) ); } + CV_Assert( (delta == 0 && std::max(c0, c1) > K) || + (delta == 1 && std::max(c0, c1) <= K) ); } - #endif - - template<> - int cornerScore<16>(const uchar* ptr, const int pixel[], int threshold) + } + #endif + + template + int cornerScore(const uchar* ptr, const int pixel[], int threshold); + + template<> + int cornerScore<16>(const uchar* ptr, const int pixel[], int threshold) + { + const int K = 8, N = K*3 + 1; + int k, v = ptr[0]; + short d[N]; + for( k = 0; k < N; k++ ) + d[k] = (short)(v - ptr[pixel[k]]); + + #if CV_SSE2 + __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000); + for( k = 0; k < 16; k += 8 ) { - const int K = 8, N = K*3 + 1; - int k, v = ptr[0]; - short d[N]; - for( k = 0; k < N; k++ ) - d[k] = (short)(v - ptr[pixel[k]]); - - #if CV_SSE2 - __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000); - for( k = 0; k < 16; k += 8 ) - { - __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1)); - __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2)); - __m128i a = _mm_min_epi16(v0, v1); - __m128i b = _mm_max_epi16(v0, v1); - v0 = _mm_loadu_si128((__m128i*)(d+k+3)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+4)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+5)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+6)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+7)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+8)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k)); - q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); - q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); - v0 = _mm_loadu_si128((__m128i*)(d+k+9)); - q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); - q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); - } - q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); - q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); - q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); - q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); - threshold = (short)_mm_cvtsi128_si32(q0) - 1; - #else - int a0 = threshold; - for( k = 0; k < 16; k += 2 ) - { - int a = std::min((int)d[k+1], (int)d[k+2]); - a = std::min(a, (int)d[k+3]); - if( a <= a0 ) - continue; - a = std::min(a, (int)d[k+4]); - a = std::min(a, (int)d[k+5]); - a = std::min(a, (int)d[k+6]); - a = std::min(a, (int)d[k+7]); - a = std::min(a, (int)d[k+8]); - a0 = std::max(a0, std::min(a, (int)d[k])); - a0 = std::max(a0, std::min(a, (int)d[k+9])); - } - - int b0 = -a0; - for( k = 0; k < 16; k += 2 ) - { - int b = std::max((int)d[k+1], (int)d[k+2]); - b = std::max(b, (int)d[k+3]); - b = std::max(b, (int)d[k+4]); - b = std::max(b, (int)d[k+5]); - if( b >= b0 ) - continue; - b = std::max(b, (int)d[k+6]); - b = std::max(b, (int)d[k+7]); - b = std::max(b, (int)d[k+8]); - - b0 = std::min(b0, std::max(b, (int)d[k])); - b0 = std::min(b0, std::max(b, (int)d[k+9])); - } - - threshold = -b0-1; - #endif - - #if VERIFY_CORNERS - testCorner(ptr, pixel, K, N, threshold); - #endif - return threshold; + __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1)); + __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2)); + __m128i a = _mm_min_epi16(v0, v1); + __m128i b = _mm_max_epi16(v0, v1); + v0 = _mm_loadu_si128((__m128i*)(d+k+3)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+4)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+5)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+6)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+7)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+8)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + v0 = _mm_loadu_si128((__m128i*)(d+k+9)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); } - - template<> - int cornerScore<12>(const uchar* ptr, const int pixel[], int threshold) + q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); + q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); + threshold = (short)_mm_cvtsi128_si32(q0) - 1; + #else + int a0 = threshold; + for( k = 0; k < 16; k += 2 ) { - const int K = 6, N = K*3 + 1; - int k, v = ptr[0]; - short d[N + 4]; - for( k = 0; k < N; k++ ) - d[k] = (short)(v - ptr[pixel[k]]); - #if CV_SSE2 - for( k = 0; k < 4; k++ ) - d[N+k] = d[k]; - #endif - - #if CV_SSE2 - __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000); - for( k = 0; k < 16; k += 8 ) - { - __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1)); - __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2)); - __m128i a = _mm_min_epi16(v0, v1); - __m128i b = _mm_max_epi16(v0, v1); - v0 = _mm_loadu_si128((__m128i*)(d+k+3)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+4)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+5)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k+6)); - a = _mm_min_epi16(a, v0); - b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+k)); - q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); - q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); - v0 = _mm_loadu_si128((__m128i*)(d+k+7)); - q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); - q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); - } - q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); - q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); - q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); - q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); - threshold = (short)_mm_cvtsi128_si32(q0) - 1; - #else - int a0 = threshold; - for( k = 0; k < 12; k += 2 ) - { - int a = std::min((int)d[k+1], (int)d[k+2]); - if( a <= a0 ) - continue; - a = std::min(a, (int)d[k+3]); - a = std::min(a, (int)d[k+4]); - a = std::min(a, (int)d[k+5]); - a = std::min(a, (int)d[k+6]); - a0 = std::max(a0, std::min(a, (int)d[k])); - a0 = std::max(a0, std::min(a, (int)d[k+7])); - } + int a = std::min((int)d[k+1], (int)d[k+2]); + a = std::min(a, (int)d[k+3]); + if( a <= a0 ) + continue; + a = std::min(a, (int)d[k+4]); + a = std::min(a, (int)d[k+5]); + a = std::min(a, (int)d[k+6]); + a = std::min(a, (int)d[k+7]); + a = std::min(a, (int)d[k+8]); + a0 = std::max(a0, std::min(a, (int)d[k])); + a0 = std::max(a0, std::min(a, (int)d[k+9])); + } - int b0 = -a0; - for( k = 0; k < 12; k += 2 ) - { - int b = std::max((int)d[k+1], (int)d[k+2]); - b = std::max(b, (int)d[k+3]); - b = std::max(b, (int)d[k+4]); - if( b >= b0 ) - continue; - b = std::max(b, (int)d[k+5]); - b = std::max(b, (int)d[k+6]); - - b0 = std::min(b0, std::max(b, (int)d[k])); - b0 = std::min(b0, std::max(b, (int)d[k+7])); - } + int b0 = -a0; + for( k = 0; k < 16; k += 2 ) + { + int b = std::max((int)d[k+1], (int)d[k+2]); + b = std::max(b, (int)d[k+3]); + b = std::max(b, (int)d[k+4]); + b = std::max(b, (int)d[k+5]); + if( b >= b0 ) + continue; + b = std::max(b, (int)d[k+6]); + b = std::max(b, (int)d[k+7]); + b = std::max(b, (int)d[k+8]); + + b0 = std::min(b0, std::max(b, (int)d[k])); + b0 = std::min(b0, std::max(b, (int)d[k+9])); + } - threshold = -b0-1; - #endif + threshold = -b0-1; + #endif - #if VERIFY_CORNERS - testCorner(ptr, pixel, K, N, threshold); - #endif - return threshold; - } + #if VERIFY_CORNERS + testCorner(ptr, pixel, K, N, threshold); + #endif + return threshold; + } - template<> - int cornerScore<8>(const uchar* ptr, const int pixel[], int threshold) + template<> + int cornerScore<12>(const uchar* ptr, const int pixel[], int threshold) + { + const int K = 6, N = K*3 + 1; + int k, v = ptr[0]; + short d[N + 4]; + for( k = 0; k < N; k++ ) + d[k] = (short)(v - ptr[pixel[k]]); + #if CV_SSE2 + for( k = 0; k < 4; k++ ) + d[N+k] = d[k]; + #endif + + #if CV_SSE2 + __m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000); + for( k = 0; k < 16; k += 8 ) { - const int K = 4, N = K*3 + 1; - int k, v = ptr[0]; - short d[N]; - for( k = 0; k < N; k++ ) - d[k] = (short)(v - ptr[pixel[k]]); - - #if CV_SSE2 - __m128i v0 = _mm_loadu_si128((__m128i*)(d+1)); - __m128i v1 = _mm_loadu_si128((__m128i*)(d+2)); + __m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1)); + __m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2)); __m128i a = _mm_min_epi16(v0, v1); __m128i b = _mm_max_epi16(v0, v1); - v0 = _mm_loadu_si128((__m128i*)(d+3)); + v0 = _mm_loadu_si128((__m128i*)(d+k+3)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k+4)); a = _mm_min_epi16(a, v0); b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+4)); + v0 = _mm_loadu_si128((__m128i*)(d+k+5)); a = _mm_min_epi16(a, v0); b = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d)); - __m128i q0 = _mm_min_epi16(a, v0); - __m128i q1 = _mm_max_epi16(b, v0); - v0 = _mm_loadu_si128((__m128i*)(d+5)); + v0 = _mm_loadu_si128((__m128i*)(d+k+6)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+k)); q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); - q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); - q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); - q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); - q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); - threshold = (short)_mm_cvtsi128_si32(q0) - 1; - #else - int a0 = threshold; - for( k = 0; k < 8; k += 2 ) - { - int a = std::min((int)d[k+1], (int)d[k+2]); - if( a <= a0 ) - continue; - a = std::min(a, (int)d[k+3]); - a = std::min(a, (int)d[k+4]); - a0 = std::max(a0, std::min(a, (int)d[k])); - a0 = std::max(a0, std::min(a, (int)d[k+5])); - } + v0 = _mm_loadu_si128((__m128i*)(d+k+7)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + } + q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); + q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); + threshold = (short)_mm_cvtsi128_si32(q0) - 1; + #else + int a0 = threshold; + for( k = 0; k < 12; k += 2 ) + { + int a = std::min((int)d[k+1], (int)d[k+2]); + if( a <= a0 ) + continue; + a = std::min(a, (int)d[k+3]); + a = std::min(a, (int)d[k+4]); + a = std::min(a, (int)d[k+5]); + a = std::min(a, (int)d[k+6]); + a0 = std::max(a0, std::min(a, (int)d[k])); + a0 = std::max(a0, std::min(a, (int)d[k+7])); + } - int b0 = -a0; - for( k = 0; k < 8; k += 2 ) - { - int b = std::max((int)d[k+1], (int)d[k+2]); - b = std::max(b, (int)d[k+3]); - if( b >= b0 ) - continue; - b = std::max(b, (int)d[k+4]); - - b0 = std::min(b0, std::max(b, (int)d[k])); - b0 = std::min(b0, std::max(b, (int)d[k+5])); - } + int b0 = -a0; + for( k = 0; k < 12; k += 2 ) + { + int b = std::max((int)d[k+1], (int)d[k+2]); + b = std::max(b, (int)d[k+3]); + b = std::max(b, (int)d[k+4]); + if( b >= b0 ) + continue; + b = std::max(b, (int)d[k+5]); + b = std::max(b, (int)d[k+6]); + + b0 = std::min(b0, std::max(b, (int)d[k])); + b0 = std::min(b0, std::max(b, (int)d[k+7])); + } - threshold = -b0-1; - #endif + threshold = -b0-1; + #endif + + #if VERIFY_CORNERS + testCorner(ptr, pixel, K, N, threshold); + #endif + return threshold; + } - #if VERIFY_CORNERS - testCorner(ptr, pixel, K, N, threshold); - #endif - return threshold; + template<> + int cornerScore<8>(const uchar* ptr, const int pixel[], int threshold) + { + const int K = 4, N = K*3 + 1; + int k, v = ptr[0]; + short d[N]; + for( k = 0; k < N; k++ ) + d[k] = (short)(v - ptr[pixel[k]]); + + #if CV_SSE2 + __m128i v0 = _mm_loadu_si128((__m128i*)(d+1)); + __m128i v1 = _mm_loadu_si128((__m128i*)(d+2)); + __m128i a = _mm_min_epi16(v0, v1); + __m128i b = _mm_max_epi16(v0, v1); + v0 = _mm_loadu_si128((__m128i*)(d+3)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+4)); + a = _mm_min_epi16(a, v0); + b = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d)); + __m128i q0 = _mm_min_epi16(a, v0); + __m128i q1 = _mm_max_epi16(b, v0); + v0 = _mm_loadu_si128((__m128i*)(d+5)); + q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0)); + q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0)); + q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1)); + q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4)); + q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2)); + threshold = (short)_mm_cvtsi128_si32(q0) - 1; + #else + int a0 = threshold; + for( k = 0; k < 8; k += 2 ) + { + int a = std::min((int)d[k+1], (int)d[k+2]); + if( a <= a0 ) + continue; + a = std::min(a, (int)d[k+3]); + a = std::min(a, (int)d[k+4]); + a0 = std::max(a0, std::min(a, (int)d[k])); + a0 = std::max(a0, std::min(a, (int)d[k+5])); } - void makeOffsets(int pixel[25], int rowStride, int patternSize) + int b0 = -a0; + for( k = 0; k < 8; k += 2 ) { - static const int offsets16[][2] = - { - {0, 3}, { 1, 3}, { 2, 2}, { 3, 1}, { 3, 0}, { 3, -1}, { 2, -2}, { 1, -3}, - {0, -3}, {-1, -3}, {-2, -2}, {-3, -1}, {-3, 0}, {-3, 1}, {-2, 2}, {-1, 3} - }; + int b = std::max((int)d[k+1], (int)d[k+2]); + b = std::max(b, (int)d[k+3]); + if( b >= b0 ) + continue; + b = std::max(b, (int)d[k+4]); + + b0 = std::min(b0, std::max(b, (int)d[k])); + b0 = std::min(b0, std::max(b, (int)d[k+5])); + } - static const int offsets12[][2] = - { - {0, 2}, { 1, 2}, { 2, 1}, { 2, 0}, { 2, -1}, { 1, -2}, - {0, -2}, {-1, -2}, {-2, -1}, {-2, 0}, {-2, 1}, {-1, 2} - }; + threshold = -b0-1; + #endif - static const int offsets8[][2] = - { - {0, 1}, { 1, 1}, { 1, 0}, { 1, -1}, - {0, -1}, {-1, -1}, {-1, 0}, {-1, 1} - }; + #if VERIFY_CORNERS + testCorner(ptr, pixel, K, N, threshold); + #endif + return threshold; + } - const int (*offsets)[2] = patternSize == 16 ? offsets16 : - patternSize == 12 ? offsets12 : - patternSize == 8 ? offsets8 : 0; + void makeOffsets(int pixel[25], int rowStride, int patternSize) + { + static const int offsets16[][2] = + { + {0, 3}, { 1, 3}, { 2, 2}, { 3, 1}, { 3, 0}, { 3, -1}, { 2, -2}, { 1, -3}, + {0, -3}, {-1, -3}, {-2, -2}, {-3, -1}, {-3, 0}, {-3, 1}, {-2, 2}, {-1, 3} + }; - CV_Assert(pixel && offsets); + static const int offsets12[][2] = + { + {0, 2}, { 1, 2}, { 2, 1}, { 2, 0}, { 2, -1}, { 1, -2}, + {0, -2}, {-1, -2}, {-2, -1}, {-2, 0}, {-2, 1}, {-1, 2} + }; - int k = 0; - for( ; k < patternSize; k++ ) - pixel[k] = offsets[k][0] + offsets[k][1] * rowStride; - for( ; k < 25; k++ ) - pixel[k] = pixel[k - patternSize]; - } + static const int offsets8[][2] = + { + {0, 1}, { 1, 1}, { 1, 0}, { 1, -1}, + {0, -1}, {-1, -1}, {-1, 0}, {-1, 1} + }; - template - void FASTForPointSet_t( InputArray image, std::vector& keypoints, int threshold, bool nonmaxSuppression ) { + const int (*offsets)[2] = patternSize == 16 ? offsets16 : + patternSize == 12 ? offsets12 : + patternSize == 8 ? offsets8 : 0; - Mat img = image.getMat(); - const int K = patternSize/2, N = patternSize + K + 1; + CV_Assert(pixel && offsets); - int i, k, pixel[25]; - makeOffsets(pixel, (int)img.step, patternSize); + int k = 0; + for( ; k < patternSize; k++ ) + pixel[k] = offsets[k][0] + offsets[k][1] * rowStride; + for( ; k < 25; k++ ) + pixel[k] = pixel[k - patternSize]; + } - keypoints.clear(); + template + void FASTForPointSet_t( InputArray image, std::vector& keypoints, int threshold, bool nonmaxSuppression ) { - threshold = std::min(std::max(threshold, 0), 255); + Mat img = image.getMat(); + const int K = patternSize/2, N = patternSize + K + 1; - uchar threshold_tab[512]; - for( i = -255; i <= 255; i++ ) - threshold_tab[i+255] = (uchar)(i < -threshold ? 1 : i > threshold ? 2 : 0); + int i, k, pixel[25]; + makeOffsets(pixel, (int)img.step, patternSize); - AutoBuffer _buf((img.cols+16)*3*(sizeof(int) + sizeof(uchar)) + 128); - uchar* buf[3]; - buf[0] = _buf; buf[1] = buf[0] + img.cols; buf[2] = buf[1] + img.cols; - int* cpbuf[3]; - cpbuf[0] = (int*)alignPtr(buf[2] + img.cols, sizeof(int)) + 1; - cpbuf[1] = cpbuf[0] + img.cols + 1; - cpbuf[2] = cpbuf[1] + img.cols + 1; - memset(buf[0], 0, img.cols*3); + keypoints.clear(); - // Calculate threshold for the keypoints - for (size_t keyPointIdx=0; keyPointIdx < keypoints.size(); keyPointIdx++) { - // Set response to -1: - // All keypoints with response <= 0 will be removed afterwards - keypoints[keyPointIdx].response = -1; + threshold = std::min(std::max(threshold, 0), 255); - // Poiter to keyPoint in image - Point keyPoint = keypoints[keyPointIdx].pt; - const uchar* ptr = img.ptr(keyPoint.y, keyPoint.x); + uchar threshold_tab[512]; + for( i = -255; i <= 255; i++ ) + threshold_tab[i+255] = (uchar)(i < -threshold ? 1 : i > threshold ? 2 : 0); - // value of the pixel at certain position - int v = ptr[0]; + AutoBuffer _buf((img.cols+16)*3*(sizeof(int) + sizeof(uchar)) + 128); + uchar* buf[3]; + buf[0] = _buf; buf[1] = buf[0] + img.cols; buf[2] = buf[1] + img.cols; + int* cpbuf[3]; + cpbuf[0] = (int*)alignPtr(buf[2] + img.cols, sizeof(int)) + 1; + cpbuf[1] = cpbuf[0] + img.cols + 1; + cpbuf[2] = cpbuf[1] + img.cols + 1; + memset(buf[0], 0, img.cols*3); - // Initialize Lookup table - // If k=v --> tab[k] is at the center of the thrshold table - // The threshold table is made as follows: - // -255 -threshold 0 +threshold 255 - // 111111111111111111|0000000000000|0000000000000|222222222222222 - const uchar* tab = &threshold_tab[0] - v + 255; + // Calculate threshold for the keypoints + for (size_t keyPointIdx=0; keyPointIdx < keypoints.size(); keyPointIdx++) { + // Set response to -1: + // All keypoints with response <= 0 will be removed afterwards + keypoints[keyPointIdx].response = -1; - // Calculate the fast value - int d = tab[ptr[pixel[0]]] | tab[ptr[pixel[8]]]; + // Poiter to keyPoint in image + Point keyPoint = keypoints[keyPointIdx].pt; + const uchar* ptr = img.ptr(keyPoint.y, keyPoint.x); - if( d == 0 ) - continue; + // value of the pixel at certain position + int v = ptr[0]; - d &= tab[ptr[pixel[2]]] | tab[ptr[pixel[10]]]; - d &= tab[ptr[pixel[4]]] | tab[ptr[pixel[12]]]; - d &= tab[ptr[pixel[6]]] | tab[ptr[pixel[14]]]; + // Initialize Lookup table + // If k=v --> tab[k] is at the center of the thrshold table + // The threshold table is made as follows: + // -255 -threshold 0 +threshold 255 + // 111111111111111111|0000000000000|0000000000000|222222222222222 + const uchar* tab = &threshold_tab[0] - v + 255; - if( d == 0 ) - continue; + // Calculate the fast value + int d = tab[ptr[pixel[0]]] | tab[ptr[pixel[8]]]; - d &= tab[ptr[pixel[1]]] | tab[ptr[pixel[9]]]; - d &= tab[ptr[pixel[3]]] | tab[ptr[pixel[11]]]; - d &= tab[ptr[pixel[5]]] | tab[ptr[pixel[13]]]; - d &= tab[ptr[pixel[7]]] | tab[ptr[pixel[15]]]; + if( d == 0 ) + continue; - // For at least half pixels darker than v count the number - if( d & 1 ) - { - int vt = v - threshold, count = 0; + d &= tab[ptr[pixel[2]]] | tab[ptr[pixel[10]]]; + d &= tab[ptr[pixel[4]]] | tab[ptr[pixel[12]]]; + d &= tab[ptr[pixel[6]]] | tab[ptr[pixel[14]]]; + + if( d == 0 ) + continue; + + d &= tab[ptr[pixel[1]]] | tab[ptr[pixel[9]]]; + d &= tab[ptr[pixel[3]]] | tab[ptr[pixel[11]]]; + d &= tab[ptr[pixel[5]]] | tab[ptr[pixel[13]]]; + d &= tab[ptr[pixel[7]]] | tab[ptr[pixel[15]]]; - for(k = 0; k < N; k++ ) + // For at least half pixels darker than v count the number + if( d & 1 ) + { + int vt = v - threshold, count = 0; + + for(k = 0; k < N; k++ ) + { + int x = ptr[pixel[k]]; + if(x < vt) { - int x = ptr[pixel[k]]; - if(x < vt) + if( ++count > K ) { - if( ++count > K ) - { - // Calculate score - keypoints[keyPointIdx].response = (uchar)cornerScore(ptr, pixel, threshold); - // Non Maxima Supression I - if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) { - keypoints[keyPointIdx-1].response = -1; - } - break; + // Calculate score + keypoints[keyPointIdx].response = (uchar)cornerScore(ptr, pixel, threshold); + // Non Maxima Supression I + if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) { + keypoints[keyPointIdx-1].response = -1; } + break; } - else - count = 0; } + else + count = 0; } + } - // For at least half pixels brighter than v count the number - if(d & 2 ) - { - int vt = v + threshold, count = 0; + // For at least half pixels brighter than v count the number + if(d & 2 ) + { + int vt = v + threshold, count = 0; - for(k = 0; k < N; k++ ) + for(k = 0; k < N; k++ ) + { + int x = ptr[pixel[k]]; + if(x > vt) { - int x = ptr[pixel[k]]; - if(x > vt) + if( ++count > K ) { - if( ++count > K ) - { - // Calculate score - keypoints[keyPointIdx].response = (uchar)cornerScore(ptr, pixel, threshold); - // Non Maxima Suppression I - if (nonmaxSuppression && keyPointIdx>0 &&keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) { - keypoints[keyPointIdx-1].response = -1; - } - break; + // Calculate score + keypoints[keyPointIdx].response = (uchar)cornerScore(ptr, pixel, threshold); + // Non Maxima Suppression I + if (nonmaxSuppression && keyPointIdx>0 &&keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) { + keypoints[keyPointIdx-1].response = -1; } + break; } - else - count = 0; } + else + count = 0; } - } - // Remove unused Keypoints - size_t maxKeypointSize = keypoints.size(); - for (size_t keyPointIdx=maxKeypointSize; keyPointIdx > 0;) { - keyPointIdx--; - if (keypoints[keyPointIdx].response <= 0) { - keypoints.erase(keypoints.begin() + keyPointIdx); - } else if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response > keypoints[keyPointIdx].response) { - // Non Maxima Suppression II - keypoints.erase(keypoints.begin() + keyPointIdx); - } + } + + // Remove unused Keypoints + size_t maxKeypointSize = keypoints.size(); + for (size_t keyPointIdx=maxKeypointSize; keyPointIdx > 0;) { + keyPointIdx--; + if (keypoints[keyPointIdx].response <= 0) { + keypoints.erase(keypoints.begin() + keyPointIdx); + } else if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response > keypoints[keyPointIdx].response) { + // Non Maxima Suppression II + keypoints.erase(keypoints.begin() + keyPointIdx); } } + } + +} + +namespace cv { + namespace xfeatures2d { void FASTForPointSet(InputArray _img, std::vector& keypoints, int threshold, bool nonmax_suppression, int type) { @@ -509,5 +477,6 @@ namespace cv { break; } } + } }