From a80ebfe28dbc868e876ece3f5aaed38069203290 Mon Sep 17 00:00:00 2001 From: Cody Rigney Date: Tue, 4 Mar 2014 14:47:12 -0500 Subject: [PATCH] Master Version: Added NEON Optimizations for LK Optical Flow. --- modules/video/src/lkpyramid.cpp | 246 ++++++++++++++++++++++++++++++++++++++++ modules/video/src/precomp.hpp | 1 + 2 files changed, 247 insertions(+) diff --git a/modules/video/src/lkpyramid.cpp b/modules/video/src/lkpyramid.cpp index 598e69c..d060575 100644 --- a/modules/video/src/lkpyramid.cpp +++ b/modules/video/src/lkpyramid.cpp @@ -70,6 +70,14 @@ static void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst) __m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10); #endif +#if CV_NEON + const uint16x8_t q8 = vdupq_n_u16(3); + const uint8x8_t d18 = vdup_n_u8(10); + + const int16x8_t q8i = vdupq_n_s16(3); + const int16x8_t q9 = vdupq_n_s16(10); +#endif + for( y = 0; y < rows; y++ ) { const uchar* srow0 = src.ptr(y > 0 ? y-1 : rows > 1 ? 1 : 0); @@ -91,6 +99,24 @@ static void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst) _mm_store_si128((__m128i*)(trow1 + x), t1); } #endif + +#if CV_NEON + for( ; x <= colsn - 8; x += 8) + { + uint8x8_t d0 = vld1_u8((const uint8_t*)&srow0[x]); + uint8x8_t d1 = vld1_u8((const uint8_t*)&srow1[x]); + uint8x8_t d2 = vld1_u8((const uint8_t*)&srow2[x]); + uint16x8_t q4 = vaddl_u8(d0, d2); + uint16x8_t q11 = vsubl_u8(d2, d0); + uint16x8_t q5 = vmulq_u16(q4, q8); + uint16x8_t q6 = vmull_u8(d1, d18); + uint16x8_t q10 = vaddq_u16(q6, q5); + vst1q_u16((uint16_t*)&trow0[x], q10); + vst1q_u16((uint16_t*)&trow1[x], q11); + + } +#endif + for( ; x < colsn; x++ ) { int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10; @@ -127,6 +153,33 @@ static void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst) _mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0); } #endif + +#if CV_NEON + for( ; x <= colsn - 8; x += 8 ) + { + + int16x8_t q0 = vld1q_s16((const int16_t*)&trow0[x+cn]); + int16x8_t q1 = vld1q_s16((const int16_t*)&trow0[x-cn]); + int16x8_t q2 = vld1q_s16((const int16_t*)&trow1[x+cn]); + int16x8_t q3 = vld1q_s16((const int16_t*)&trow1[x-cn]); + int16x8_t q5 = vsubq_s16(q0, q1); + int16x8_t q6 = vaddq_s16(q2, q3); + int16x8_t q4 = vld1q_s16((const int16_t*)&trow1[x]); + int16x8_t q7 = vmulq_s16(q6, q8i); + int16x8_t q10 = vmulq_s16(q4, q9); + int16x8_t q11 = vaddq_s16(q7, q10); + int16x4_t d22 = vget_low_s16(q11); + int16x4_t d23 = vget_high_s16(q11); + int16x4_t d11 = vget_high_s16(q5); + int16x4_t d10 = vget_low_s16(q5); + int16x4x2_t q5x2, q11x2; + q5x2.val[0] = d10; q5x2.val[1] = d22; + q11x2.val[0] = d11; q11x2.val[1] = d23; + vst2_s16((int16_t*)&drow[x*2], q5x2); + vst2_s16((int16_t*)&drow[(x*2)+8], q11x2); + + } +#endif for( ; x < colsn; x++ ) { deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]); @@ -239,6 +292,21 @@ void cv::detail::LKTrackerInvoker::operator()(const Range& range) const __m128 qA11 = _mm_setzero_ps(), qA12 = _mm_setzero_ps(), qA22 = _mm_setzero_ps(); #endif +#if CV_NEON + + int CV_DECL_ALIGNED(16) nA11[] = {0, 0, 0, 0}, nA12[] = {0, 0, 0, 0}, nA22[] = {0, 0, 0, 0}; + const int shifter1 = -(W_BITS - 5); //negative so it shifts right + const int shifter2 = -(W_BITS); + + const int16x4_t d26 = vdup_n_s16((int16_t)iw00); + const int16x4_t d27 = vdup_n_s16((int16_t)iw01); + const int16x4_t d28 = vdup_n_s16((int16_t)iw10); + const int16x4_t d29 = vdup_n_s16((int16_t)iw11); + const int32x4_t q11 = vdupq_n_s32((int32_t)shifter1); + const int32x4_t q12 = vdupq_n_s32((int32_t)shifter2); + +#endif + // extract the patch from the first image, compute covariation matrix of derivatives int x, y; for( y = 0; y < winSize.height; y++ ) @@ -292,6 +360,90 @@ void cv::detail::LKTrackerInvoker::operator()(const Range& range) const } #endif +#if CV_NEON + for( ; x <= winSize.width*cn - 4; x += 4, dsrc += 4*2, dIptr += 4*2 ) + { + + uint8x8_t d0 = vld1_u8(&src[x]); + uint8x8_t d2 = vld1_u8(&src[x+cn]); + uint16x8_t q0 = vmovl_u8(d0); + uint16x8_t q1 = vmovl_u8(d2); + + int32x4_t q5 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q0)), d26); + int32x4_t q6 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q1)), d27); + + uint8x8_t d4 = vld1_u8(&src[x + stepI]); + uint8x8_t d6 = vld1_u8(&src[x + stepI + cn]); + uint16x8_t q2 = vmovl_u8(d4); + uint16x8_t q3 = vmovl_u8(d6); + + int32x4_t q7 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q2)), d28); + int32x4_t q8 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q3)), d29); + + q5 = vaddq_s32(q5, q6); + q7 = vaddq_s32(q7, q8); + q5 = vaddq_s32(q5, q7); + + int16x4x2_t d0d1 = vld2_s16(dsrc); + int16x4x2_t d2d3 = vld2_s16(&dsrc[cn2]); + + q5 = vqrshlq_s32(q5, q11); + + int32x4_t q4 = vmull_s16(d0d1.val[0], d26); + q6 = vmull_s16(d0d1.val[1], d26); + + int16x4_t nd0 = vmovn_s32(q5); + + q7 = vmull_s16(d2d3.val[0], d27); + q8 = vmull_s16(d2d3.val[1], d27); + + vst1_s16(&Iptr[x], nd0); + + int16x4x2_t d4d5 = vld2_s16(&dsrc[dstep]); + int16x4x2_t d6d7 = vld2_s16(&dsrc[dstep+cn2]); + + q4 = vaddq_s32(q4, q7); + q6 = vaddq_s32(q6, q8); + + q7 = vmull_s16(d4d5.val[0], d28); + int32x4_t nq0 = vmull_s16(d4d5.val[1], d28); + q8 = vmull_s16(d6d7.val[0], d29); + int32x4_t q15 = vmull_s16(d6d7.val[1], d29); + + q7 = vaddq_s32(q7, q8); + nq0 = vaddq_s32(nq0, q15); + + q4 = vaddq_s32(q4, q7); + q6 = vaddq_s32(q6, nq0); + + int32x4_t nq1 = vld1q_s32(nA12); + int32x4_t nq2 = vld1q_s32(nA22); + nq0 = vld1q_s32(nA11); + + q4 = vqrshlq_s32(q4, q12); + q6 = vqrshlq_s32(q6, q12); + + q7 = vmulq_s32(q4, q4); + q8 = vmulq_s32(q4, q6); + q15 = vmulq_s32(q6, q6); + + nq0 = vaddq_s32(nq0, q7); + nq1 = vaddq_s32(nq1, q8); + nq2 = vaddq_s32(nq2, q15); + + vst1q_s32(nA11, nq0); + vst1q_s32(nA12, nq1); + vst1q_s32(nA22, nq2); + + int16x4_t d8 = vmovn_s32(q4); + int16x4_t d12 = vmovn_s32(q6); + + int16x4x2_t d8d12; + d8d12.val[0] = d8; d8d12.val[1] = d12; + vst2_s16(dIptr, d8d12); + } +#endif + for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 ) { int ival = CV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 + @@ -321,6 +473,12 @@ void cv::detail::LKTrackerInvoker::operator()(const Range& range) const iA22 += A22buf[0] + A22buf[1] + A22buf[2] + A22buf[3]; #endif +#if CV_NEON + iA11 += (float)(nA11[0] + nA11[1] + nA11[2] + nA11[3]); + iA12 += (float)(nA12[0] + nA12[1] + nA12[2] + nA12[3]); + iA22 += (float)(nA22[0] + nA22[1] + nA22[2] + nA22[3]); +#endif + A11 = iA11*FLT_SCALE; A12 = iA12*FLT_SCALE; A22 = iA22*FLT_SCALE; @@ -371,6 +529,16 @@ void cv::detail::LKTrackerInvoker::operator()(const Range& range) const __m128 qb0 = _mm_setzero_ps(), qb1 = _mm_setzero_ps(); #endif +#if CV_NEON + int CV_DECL_ALIGNED(16) nB1[] = {0,0,0,0}, nB2[] = {0,0,0,0}; + + const int16x4_t d26_2 = vdup_n_s16((int16_t)iw00); + const int16x4_t d27_2 = vdup_n_s16((int16_t)iw01); + const int16x4_t d28_2 = vdup_n_s16((int16_t)iw10); + const int16x4_t d29_2 = vdup_n_s16((int16_t)iw11); + +#endif + for( y = 0; y < winSize.height; y++ ) { const uchar* Jptr = (const uchar*)J.data + (y + inextPt.y)*stepJ + inextPt.x*cn; @@ -414,6 +582,78 @@ void cv::detail::LKTrackerInvoker::operator()(const Range& range) const } #endif +#if CV_NEON + for( ; x <= winSize.width*cn - 8; x += 8, dIptr += 8*2 ) + { + + uint8x8_t d0 = vld1_u8(&Jptr[x]); + uint8x8_t d2 = vld1_u8(&Jptr[x+cn]); + uint8x8_t d4 = vld1_u8(&Jptr[x+stepJ]); + uint8x8_t d6 = vld1_u8(&Jptr[x+stepJ+cn]); + + uint16x8_t q0 = vmovl_u8(d0); + uint16x8_t q1 = vmovl_u8(d2); + uint16x8_t q2 = vmovl_u8(d4); + uint16x8_t q3 = vmovl_u8(d6); + + int32x4_t nq4 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q0)), d26_2); + int32x4_t nq5 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q0)), d26_2); + + int32x4_t nq6 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q1)), d27_2); + int32x4_t nq7 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q1)), d27_2); + + int32x4_t nq8 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q2)), d28_2); + int32x4_t nq9 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q2)), d28_2); + + int32x4_t nq10 = vmull_s16(vget_low_s16(vreinterpretq_s16_u16(q3)), d29_2); + int32x4_t nq11 = vmull_s16(vget_high_s16(vreinterpretq_s16_u16(q3)), d29_2); + + nq4 = vaddq_s32(nq4, nq6); + nq5 = vaddq_s32(nq5, nq7); + nq8 = vaddq_s32(nq8, nq10); + nq9 = vaddq_s32(nq9, nq11); + + int16x8_t q6 = vld1q_s16(&Iptr[x]); + + nq4 = vaddq_s32(nq4, nq8); + nq5 = vaddq_s32(nq5, nq9); + + nq8 = vmovl_s16(vget_high_s16(q6)); + nq6 = vmovl_s16(vget_low_s16(q6)); + + nq4 = vqrshlq_s32(nq4, q11); + nq5 = vqrshlq_s32(nq5, q11); + + int16x8x2_t q0q1 = vld2q_s16(dIptr); + nq11 = vld1q_s32(nB1); + int32x4_t nq15 = vld1q_s32(nB2); + + nq4 = vsubq_s32(nq4, nq6); + nq5 = vsubq_s32(nq5, nq8); + + int32x4_t nq2 = vmovl_s16(vget_low_s16(q0q1.val[0])); + int32x4_t nq3 = vmovl_s16(vget_high_s16(q0q1.val[0])); + + nq7 = vmovl_s16(vget_low_s16(q0q1.val[1])); + nq8 = vmovl_s16(vget_high_s16(q0q1.val[1])); + + nq9 = vmulq_s32(nq4, nq2); + nq10 = vmulq_s32(nq5, nq3); + + nq4 = vmulq_s32(nq4, nq7); + nq5 = vmulq_s32(nq5, nq8); + + nq9 = vaddq_s32(nq9, nq10); + nq4 = vaddq_s32(nq4, nq5); + + nq11 = vaddq_s32(nq11, nq9); + nq15 = vaddq_s32(nq15, nq4); + + vst1q_s32(nB1, nq11); + vst1q_s32(nB2, nq15); + } +#endif + for( ; x < winSize.width*cn; x++, dIptr += 2 ) { int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 + @@ -431,6 +671,12 @@ void cv::detail::LKTrackerInvoker::operator()(const Range& range) const ib2 += bbuf[1] + bbuf[3]; #endif +#if CV_NEON + + ib1 += (float)(nB1[0] + nB1[1] + nB1[2] + nB1[3]); + ib2 += (float)(nB2[0] + nB2[1] + nB2[2] + nB2[3]); +#endif + b1 = ib1*FLT_SCALE; b2 = ib2*FLT_SCALE; diff --git a/modules/video/src/precomp.hpp b/modules/video/src/precomp.hpp index ba0c931..5f1bbf8 100644 --- a/modules/video/src/precomp.hpp +++ b/modules/video/src/precomp.hpp @@ -47,6 +47,7 @@ #include "opencv2/core/utility.hpp" #include "opencv2/core/private.hpp" #include "opencv2/core/ocl.hpp" +#include "opencv2/core.hpp" #ifdef HAVE_TEGRA_OPTIMIZATION #include "opencv2/video/video_tegra.hpp" -- 2.7.4