From 14a0abbfa9ce6900ce4febd2ab95679445a5ca1f Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Wed, 20 Mar 2013 19:51:33 +0400 Subject: [PATCH] refactored video module; use the new-style algorithms now --- modules/video/src/affineflow.cpp | 850 +++++++++++++++++++++++++++++ modules/video/src/compat_video.cpp | 388 +++++++++++++ modules/video/src/simpleflow.hpp | 86 --- 3 files changed, 1238 insertions(+), 86 deletions(-) create mode 100644 modules/video/src/affineflow.cpp create mode 100644 modules/video/src/compat_video.cpp delete mode 100644 modules/video/src/simpleflow.hpp diff --git a/modules/video/src/affineflow.cpp b/modules/video/src/affineflow.cpp new file mode 100644 index 0000000000..f833a89ca8 --- /dev/null +++ b/modules/video/src/affineflow.cpp @@ -0,0 +1,850 @@ +/*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, Intel Corporation, all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, 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 "precomp.hpp" + +// to be moved to legacy + +static int icvMinimalPyramidSize( CvSize imgSize ) +{ + return cvAlign(imgSize.width,8) * imgSize.height / 3; +} + + +static void +icvInitPyramidalAlgorithm( const CvMat* imgA, const CvMat* imgB, + CvMat* pyrA, CvMat* pyrB, + int level, CvTermCriteria * criteria, + int max_iters, int flags, + uchar *** imgI, uchar *** imgJ, + int **step, CvSize** size, + double **scale, cv::AutoBuffer* buffer ) +{ + const int ALIGN = 8; + int pyrBytes, bufferBytes = 0, elem_size; + int level1 = level + 1; + + int i; + CvSize imgSize, levelSize; + + *imgI = *imgJ = 0; + *step = 0; + *scale = 0; + *size = 0; + + /* check input arguments */ + if( ((flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA) || + ((flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB) ) + CV_Error( CV_StsNullPtr, "Some of the precomputed pyramids are missing" ); + + if( level < 0 ) + CV_Error( CV_StsOutOfRange, "The number of pyramid levels is negative" ); + + switch( criteria->type ) + { + case CV_TERMCRIT_ITER: + criteria->epsilon = 0.f; + break; + case CV_TERMCRIT_EPS: + criteria->max_iter = max_iters; + break; + case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: + break; + default: + assert( 0 ); + CV_Error( CV_StsBadArg, "Invalid termination criteria" ); + } + + /* compare squared values */ + criteria->epsilon *= criteria->epsilon; + + /* set pointers and step for every level */ + pyrBytes = 0; + + imgSize = cvGetSize(imgA); + elem_size = CV_ELEM_SIZE(imgA->type); + levelSize = imgSize; + + for( i = 1; i < level1; i++ ) + { + levelSize.width = (levelSize.width + 1) >> 1; + levelSize.height = (levelSize.height + 1) >> 1; + + int tstep = cvAlign(levelSize.width,ALIGN) * elem_size; + pyrBytes += tstep * levelSize.height; + } + + assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 ); + + /* buffer_size = + */ + bufferBytes = (int)((level1 >= 0) * ((pyrA->data.ptr == 0) + + (pyrB->data.ptr == 0)) * pyrBytes + + (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) + + sizeof(size[0][0]) + sizeof(scale[0][0])) * level1); + + buffer->allocate( bufferBytes ); + + *imgI = (uchar **) (uchar*)(*buffer); + *imgJ = *imgI + level1; + *step = (int *) (*imgJ + level1); + *scale = (double *) (*step + level1); + *size = (CvSize *)(*scale + level1); + + imgI[0][0] = imgA->data.ptr; + imgJ[0][0] = imgB->data.ptr; + step[0][0] = imgA->step; + scale[0][0] = 1; + size[0][0] = imgSize; + + if( level > 0 ) + { + uchar *bufPtr = (uchar *) (*size + level1); + uchar *ptrA = pyrA->data.ptr; + uchar *ptrB = pyrB->data.ptr; + + if( !ptrA ) + { + ptrA = bufPtr; + bufPtr += pyrBytes; + } + + if( !ptrB ) + ptrB = bufPtr; + + levelSize = imgSize; + + /* build pyramids for both frames */ + for( i = 1; i <= level; i++ ) + { + int levelBytes; + CvMat prev_level, next_level; + + levelSize.width = (levelSize.width + 1) >> 1; + levelSize.height = (levelSize.height + 1) >> 1; + + size[0][i] = levelSize; + step[0][i] = cvAlign( levelSize.width, ALIGN ) * elem_size; + scale[0][i] = scale[0][i - 1] * 0.5; + + levelBytes = step[0][i] * levelSize.height; + imgI[0][i] = (uchar *) ptrA; + ptrA += levelBytes; + + if( !(flags & CV_LKFLOW_PYR_A_READY) ) + { + prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); + next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); + cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] ); + cvSetData( &next_level, imgI[0][i], step[0][i] ); + cvPyrDown( &prev_level, &next_level ); + } + + imgJ[0][i] = (uchar *) ptrB; + ptrB += levelBytes; + + if( !(flags & CV_LKFLOW_PYR_B_READY) ) + { + prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); + next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); + cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] ); + cvSetData( &next_level, imgJ[0][i], step[0][i] ); + cvPyrDown( &prev_level, &next_level ); + } + } + } +} + + +/* compute dI/dx and dI/dy */ +static void +icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step, + CvSize src_size, const float* smooth_k, float* buffer0 ) +{ + int src_width = src_size.width, dst_width = src_size.width-2; + int x, height = src_size.height - 2; + float* buffer1 = buffer0 + src_width; + + src_step /= sizeof(src[0]); + dst_step /= sizeof(dstX[0]); + + for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step ) + { + const float* src2 = src + src_step; + const float* src3 = src + src_step*2; + + for( x = 0; x < src_width; x++ ) + { + float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1]; + float t1 = src3[x] - src[x]; + buffer0[x] = t0; buffer1[x] = t1; + } + + for( x = 0; x < dst_width; x++ ) + { + float t0 = buffer0[x+2] - buffer0[x]; + float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1]; + dstX[x] = t0; dstY[x] = t1; + } + } +} + + +#undef CV_8TO32F +#define CV_8TO32F(a) (a) + +static const void* +icvAdjustRect( const void* srcptr, int src_step, int pix_size, + CvSize src_size, CvSize win_size, + CvPoint ip, CvRect* pRect ) +{ + CvRect rect; + const char* src = (const char*)srcptr; + + if( ip.x >= 0 ) + { + src += ip.x*pix_size; + rect.x = 0; + } + else + { + rect.x = -ip.x; + if( rect.x > win_size.width ) + rect.x = win_size.width; + } + + if( ip.x + win_size.width < src_size.width ) + rect.width = win_size.width; + else + { + rect.width = src_size.width - ip.x - 1; + if( rect.width < 0 ) + { + src += rect.width*pix_size; + rect.width = 0; + } + assert( rect.width <= win_size.width ); + } + + if( ip.y >= 0 ) + { + src += ip.y * src_step; + rect.y = 0; + } + else + rect.y = -ip.y; + + if( ip.y + win_size.height < src_size.height ) + rect.height = win_size.height; + else + { + rect.height = src_size.height - ip.y - 1; + if( rect.height < 0 ) + { + src += rect.height*src_step; + rect.height = 0; + } + } + + *pRect = rect; + return src - rect.x*pix_size; +} + + +static CvStatus CV_STDCALL icvGetRectSubPix_8u32f_C1R +( const uchar* src, int src_step, CvSize src_size, + float* dst, int dst_step, CvSize win_size, CvPoint2D32f center ) +{ + CvPoint ip; + float a12, a22, b1, b2; + float a, b; + double s = 0; + int i, j; + + center.x -= (win_size.width-1)*0.5f; + center.y -= (win_size.height-1)*0.5f; + + ip.x = cvFloor( center.x ); + ip.y = cvFloor( center.y ); + + if( win_size.width <= 0 || win_size.height <= 0 ) + return CV_BADRANGE_ERR; + + a = center.x - ip.x; + b = center.y - ip.y; + a = MAX(a,0.0001f); + a12 = a*(1.f-b); + a22 = a*b; + b1 = 1.f - b; + b2 = b; + s = (1. - a)/a; + + src_step /= sizeof(src[0]); + dst_step /= sizeof(dst[0]); + + if( 0 <= ip.x && ip.x + win_size.width < src_size.width && + 0 <= ip.y && ip.y + win_size.height < src_size.height ) + { + // extracted rectangle is totally inside the image + src += ip.y * src_step + ip.x; + +#if 0 + if( icvCopySubpix_8u32f_C1R_p && + icvCopySubpix_8u32f_C1R_p( src, src_step, dst, + dst_step*sizeof(dst[0]), win_size, a, b ) >= 0 ) + return CV_OK; +#endif + + for( ; win_size.height--; src += src_step, dst += dst_step ) + { + float prev = (1 - a)*(b1*CV_8TO32F(src[0]) + b2*CV_8TO32F(src[src_step])); + for( j = 0; j < win_size.width; j++ ) + { + float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src[j+1+src_step]); + dst[j] = prev + t; + prev = (float)(t*s); + } + } + } + else + { + CvRect r; + + src = (const uchar*)icvAdjustRect( src, src_step*sizeof(*src), + sizeof(*src), src_size, win_size,ip, &r); + + for( i = 0; i < win_size.height; i++, dst += dst_step ) + { + const uchar *src2 = src + src_step; + + if( i < r.y || i >= r.height ) + src2 -= src_step; + + for( j = 0; j < r.x; j++ ) + { + float s0 = CV_8TO32F(src[r.x])*b1 + + CV_8TO32F(src2[r.x])*b2; + + dst[j] = (float)(s0); + } + + if( j < r.width ) + { + float prev = (1 - a)*(b1*CV_8TO32F(src[j]) + b2*CV_8TO32F(src2[j])); + + for( ; j < r.width; j++ ) + { + float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src2[j+1]); + dst[j] = prev + t; + prev = (float)(t*s); + } + } + + for( ; j < win_size.width; j++ ) + { + float s0 = CV_8TO32F(src[r.width])*b1 + + CV_8TO32F(src2[r.width])*b2; + + dst[j] = (float)(s0); + } + + if( i < r.height ) + src = src2; + } + } + + return CV_OK; +} + + +#define ICV_32F8U(x) ((uchar)cvRound(x)) + +#define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( flavor, srctype, dsttype, \ +worktype, cast_macro, cvt ) \ +static CvStatus CV_STDCALL \ +icvGetQuadrangleSubPix_##flavor##_C1R \ +( const srctype * src, int src_step, CvSize src_size, \ +dsttype *dst, int dst_step, CvSize win_size, const float *matrix ) \ +{ \ +int x, y; \ +double dx = (win_size.width - 1)*0.5; \ +double dy = (win_size.height - 1)*0.5; \ +double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \ +double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \ +\ +src_step /= sizeof(srctype); \ +dst_step /= sizeof(dsttype); \ +\ +for( y = 0; y < win_size.height; y++, dst += dst_step ) \ +{ \ +double xs = A12*y + A13; \ +double ys = A22*y + A23; \ +double xe = A11*(win_size.width-1) + A12*y + A13; \ +double ye = A21*(win_size.width-1) + A22*y + A23; \ +\ +if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) && \ +(unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) && \ +(unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) && \ +(unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3)) \ +{ \ +for( x = 0; x < win_size.width; x++ ) \ +{ \ +int ixs = cvFloor( xs ); \ +int iys = cvFloor( ys ); \ +const srctype *ptr = src + src_step*iys + ixs; \ +double a = xs - ixs, b = ys - iys, a1 = 1.f - a; \ +worktype p0 = cvt(ptr[0])*a1 + cvt(ptr[1])*a; \ +worktype p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+1])*a;\ +xs += A11; \ +ys += A21; \ +\ +dst[x] = cast_macro(p0 + b * (p1 - p0)); \ +} \ +} \ +else \ +{ \ +for( x = 0; x < win_size.width; x++ ) \ +{ \ +int ixs = cvFloor( xs ), iys = cvFloor( ys ); \ +double a = xs - ixs, b = ys - iys, a1 = 1.f - a; \ +const srctype *ptr0, *ptr1; \ +worktype p0, p1; \ +xs += A11; ys += A21; \ +\ +if( (unsigned)iys < (unsigned)(src_size.height-1) ) \ +ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step; \ +else \ +ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \ +\ +if( (unsigned)ixs < (unsigned)(src_size.width-1) ) \ +{ \ +p0 = cvt(ptr0[ixs])*a1 + cvt(ptr0[ixs+1])*a; \ +p1 = cvt(ptr1[ixs])*a1 + cvt(ptr1[ixs+1])*a; \ +} \ +else \ +{ \ +ixs = ixs < 0 ? 0 : src_size.width - 1; \ +p0 = cvt(ptr0[ixs]); p1 = cvt(ptr1[ixs]); \ +} \ +dst[x] = cast_macro(p0 + b * (p1 - p0)); \ +} \ +} \ +} \ +\ +return CV_OK; \ +} + +ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F ) + +/* Affine tracking algorithm */ + +CV_IMPL void +cvCalcAffineFlowPyrLK( const void* arrA, const void* arrB, + void* pyrarrA, void* pyrarrB, + const CvPoint2D32f * featuresA, + CvPoint2D32f * featuresB, + float *matrices, int count, + CvSize winSize, int level, + char *status, float *error, + CvTermCriteria criteria, int flags ) +{ + const int MAX_ITERS = 100; + + cv::AutoBuffer _status; + cv::AutoBuffer buffer; + cv::AutoBuffer pyr_buffer; + + CvMat stubA, *imgA = (CvMat*)arrA; + CvMat stubB, *imgB = (CvMat*)arrB; + CvMat pstubA, *pyrA = (CvMat*)pyrarrA; + CvMat pstubB, *pyrB = (CvMat*)pyrarrB; + + static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ + + int bufferBytes = 0; + + uchar **imgI = 0; + uchar **imgJ = 0; + int *step = 0; + double *scale = 0; + CvSize* size = 0; + + float *patchI; + float *patchJ; + float *Ix; + float *Iy; + + int i, j, k, l; + + CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); + int patchLen = patchSize.width * patchSize.height; + int patchStep = patchSize.width * sizeof( patchI[0] ); + + CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 ); + int srcPatchLen = srcPatchSize.width * srcPatchSize.height; + int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] ); + CvSize imgSize; + float eps = (float)MIN(winSize.width, winSize.height); + + imgA = cvGetMat( imgA, &stubA ); + imgB = cvGetMat( imgB, &stubB ); + + if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) + CV_Error( CV_StsUnsupportedFormat, "" ); + + if( !CV_ARE_TYPES_EQ( imgA, imgB )) + CV_Error( CV_StsUnmatchedFormats, "" ); + + if( !CV_ARE_SIZES_EQ( imgA, imgB )) + CV_Error( CV_StsUnmatchedSizes, "" ); + + if( imgA->step != imgB->step ) + CV_Error( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); + + if( !matrices ) + CV_Error( CV_StsNullPtr, "" ); + + imgSize = cvGetMatSize( imgA ); + + if( pyrA ) + { + pyrA = cvGetMat( pyrA, &pstubA ); + + if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) + CV_Error( CV_StsBadArg, "pyramid A has insufficient size" ); + } + else + { + pyrA = &pstubA; + pyrA->data.ptr = 0; + } + + if( pyrB ) + { + pyrB = cvGetMat( pyrB, &pstubB ); + + if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) + CV_Error( CV_StsBadArg, "pyramid B has insufficient size" ); + } + else + { + pyrB = &pstubB; + pyrB->data.ptr = 0; + } + + if( count == 0 ) + return; + + /* check input arguments */ + if( !featuresA || !featuresB || !matrices ) + CV_Error( CV_StsNullPtr, "" ); + + if( winSize.width <= 1 || winSize.height <= 1 ) + CV_Error( CV_StsOutOfRange, "the search window is too small" ); + + if( count < 0 ) + CV_Error( CV_StsOutOfRange, "" ); + + icvInitPyramidalAlgorithm( imgA, imgB, + pyrA, pyrB, level, &criteria, MAX_ITERS, flags, + &imgI, &imgJ, &step, &size, &scale, &pyr_buffer ); + + /* buffer_size = + */ + bufferBytes = (srcPatchLen + patchLen*3)*sizeof(patchI[0]) + (36*2 + 6)*sizeof(double); + + buffer.allocate(bufferBytes); + + if( !status ) + { + _status.allocate(count); + status = _status; + } + + patchI = (float *)(uchar*)buffer; + patchJ = patchI + srcPatchLen; + Ix = patchJ + patchLen; + Iy = Ix + patchLen; + + if( status ) + memset( status, 1, count ); + + if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) + { + memcpy( featuresB, featuresA, count * sizeof( featuresA[0] )); + for( i = 0; i < count * 4; i += 4 ) + { + matrices[i] = matrices[i + 3] = 1.f; + matrices[i + 1] = matrices[i + 2] = 0.f; + } + } + + for( i = 0; i < count; i++ ) + { + featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5); + featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5); + } + + /* do processing from top pyramid level (smallest image) + to the bottom (original image) */ + for( l = level; l >= 0; l-- ) + { + CvSize levelSize = size[l]; + int levelStep = step[l]; + + /* find flow for each given point at the particular level */ + for( i = 0; i < count; i++ ) + { + CvPoint2D32f u; + float Av[6]; + double G[36]; + double meanI = 0, meanJ = 0; + int x, y; + int pt_status = status[i]; + CvMat mat; + + if( !pt_status ) + continue; + + Av[0] = matrices[i*4]; + Av[1] = matrices[i*4+1]; + Av[3] = matrices[i*4+2]; + Av[4] = matrices[i*4+3]; + + Av[2] = featuresB[i].x += featuresB[i].x; + Av[5] = featuresB[i].y += featuresB[i].y; + + u.x = (float) (featuresA[i].x * scale[l]); + u.y = (float) (featuresA[i].y * scale[l]); + + if( u.x < -eps || u.x >= levelSize.width+eps || + u.y < -eps || u.y >= levelSize.height+eps || + icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, + levelSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 ) + { + /* point is outside the image. take the next */ + if( l == 0 ) + status[i] = 0; + continue; + } + + icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy, + (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize, + smoothKernel, patchJ ); + + /* repack patchI (remove borders) */ + for( k = 0; k < patchSize.height; k++ ) + memcpy( patchI + k * patchSize.width, + patchI + (k + 1) * srcPatchSize.width + 1, patchStep ); + + memset( G, 0, sizeof( G )); + + /* calculate G matrix */ + for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) + { + for( x = -winSize.width; x <= winSize.width; x++, k++ ) + { + double ixix = ((double) Ix[k]) * Ix[k]; + double ixiy = ((double) Ix[k]) * Iy[k]; + double iyiy = ((double) Iy[k]) * Iy[k]; + + double xx, xy, yy; + + G[0] += ixix; + G[1] += ixiy; + G[2] += x * ixix; + G[3] += y * ixix; + G[4] += x * ixiy; + G[5] += y * ixiy; + + // G[6] == G[1] + G[7] += iyiy; + // G[8] == G[4] + // G[9] == G[5] + G[10] += x * iyiy; + G[11] += y * iyiy; + + xx = x * x; + xy = x * y; + yy = y * y; + + // G[12] == G[2] + // G[13] == G[8] == G[4] + G[14] += xx * ixix; + G[15] += xy * ixix; + G[16] += xx * ixiy; + G[17] += xy * ixiy; + + // G[18] == G[3] + // G[19] == G[9] + // G[20] == G[15] + G[21] += yy * ixix; + // G[22] == G[17] + G[23] += yy * ixiy; + + // G[24] == G[4] + // G[25] == G[10] + // G[26] == G[16] + // G[27] == G[22] + G[28] += xx * iyiy; + G[29] += xy * iyiy; + + // G[30] == G[5] + // G[31] == G[11] + // G[32] == G[17] + // G[33] == G[23] + // G[34] == G[29] + G[35] += yy * iyiy; + + meanI += patchI[k]; + } + } + + meanI /= patchSize.width*patchSize.height; + + G[8] = G[4]; + G[9] = G[5]; + G[22] = G[17]; + + // fill part of G below its diagonal + for( y = 1; y < 6; y++ ) + for( x = 0; x < y; x++ ) + G[y * 6 + x] = G[x * 6 + y]; + + cvInitMatHeader( &mat, 6, 6, CV_64FC1, G ); + + if( cvInvert( &mat, &mat, CV_SVD ) < 1e-4 ) + { + /* bad matrix. take the next point */ + if( l == 0 ) + status[i] = 0; + continue; + } + + for( j = 0; j < criteria.max_iter; j++ ) + { + double b[6] = {0,0,0,0,0,0}, eta[6]; + double t0, t1, s = 0; + + if( Av[2] < -eps || Av[2] >= levelSize.width+eps || + Av[5] < -eps || Av[5] >= levelSize.height+eps || + icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], levelStep, + levelSize, patchJ, patchStep, patchSize, Av ) < 0 ) + { + pt_status = 0; + break; + } + + for( y = -winSize.height, k = 0, meanJ = 0; y <= winSize.height; y++ ) + for( x = -winSize.width; x <= winSize.width; x++, k++ ) + meanJ += patchJ[k]; + + meanJ = meanJ / (patchSize.width * patchSize.height) - meanI; + + for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) + { + for( x = -winSize.width; x <= winSize.width; x++, k++ ) + { + double t = patchI[k] - patchJ[k] + meanJ; + double ixt = Ix[k] * t; + double iyt = Iy[k] * t; + + s += t; + + b[0] += ixt; + b[1] += iyt; + b[2] += x * ixt; + b[3] += y * ixt; + b[4] += x * iyt; + b[5] += y * iyt; + } + } + + for( k = 0; k < 6; k++ ) + eta[k] = G[k*6]*b[0] + G[k*6+1]*b[1] + G[k*6+2]*b[2] + + G[k*6+3]*b[3] + G[k*6+4]*b[4] + G[k*6+5]*b[5]; + + Av[2] = (float)(Av[2] + Av[0] * eta[0] + Av[1] * eta[1]); + Av[5] = (float)(Av[5] + Av[3] * eta[0] + Av[4] * eta[1]); + + t0 = Av[0] * (1 + eta[2]) + Av[1] * eta[4]; + t1 = Av[0] * eta[3] + Av[1] * (1 + eta[5]); + Av[0] = (float)t0; + Av[1] = (float)t1; + + t0 = Av[3] * (1 + eta[2]) + Av[4] * eta[4]; + t1 = Av[3] * eta[3] + Av[4] * (1 + eta[5]); + Av[3] = (float)t0; + Av[4] = (float)t1; + + if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon ) + break; + } + + if( pt_status != 0 || l == 0 ) + { + status[i] = (char)pt_status; + featuresB[i].x = Av[2]; + featuresB[i].y = Av[5]; + + matrices[i*4] = Av[0]; + matrices[i*4+1] = Av[1]; + matrices[i*4+2] = Av[3]; + matrices[i*4+3] = Av[4]; + } + + if( pt_status && l == 0 && error ) + { + /* calc error */ + double err = 0; + + for( y = 0, k = 0; y < patchSize.height; y++ ) + { + for( x = 0; x < patchSize.width; x++, k++ ) + { + double t = patchI[k] - patchJ[k] + meanJ; + err += t * t; + } + } + error[i] = (float)std::sqrt(err); + } + } + } +} diff --git a/modules/video/src/compat_video.cpp b/modules/video/src/compat_video.cpp new file mode 100644 index 0000000000..5f70c893ea --- /dev/null +++ b/modules/video/src/compat_video.cpp @@ -0,0 +1,388 @@ +/*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, Intel Corporation, all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, 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 "precomp.hpp" + + +/////////////////////////// Meanshift & CAMShift /////////////////////////// + +CV_IMPL int +cvMeanShift( const void* imgProb, CvRect windowIn, + CvTermCriteria criteria, CvConnectedComp* comp ) +{ + cv::Mat img = cv::cvarrToMat(imgProb); + cv::Rect window = windowIn; + int iters = cv::meanShift(img, window, criteria); + + if( comp ) + { + comp->rect = window; + comp->area = cvRound(cv::sum(img(window))[0]); + } + + return iters; +} + + +CV_IMPL int +cvCamShift( const void* imgProb, CvRect windowIn, + CvTermCriteria criteria, + CvConnectedComp* comp, + CvBox2D* box ) +{ + cv::Mat img = cv::cvarrToMat(imgProb); + cv::Rect window = windowIn; + cv::RotatedRect rr = cv::CamShift(img, window, criteria); + + if( comp ) + { + comp->rect = window; + cv::Rect roi = rr.boundingRect() & cv::Rect(0, 0, img.cols, img.rows); + comp->area = cvRound(cv::sum(img(roi))[0]); + } + + if( box ) + *box = rr; + + return rr.size.width*rr.size.height > 0.f ? 1 : -1; +} + + +///////////////////////// Motion Templates //////////////////////////// + +CV_IMPL void +cvUpdateMotionHistory( const void* silhouette, void* mhimg, + double timestamp, double mhi_duration ) +{ + cv::Mat silh = cv::cvarrToMat(silhouette), mhi = cv::cvarrToMat(mhimg); + cv::updateMotionHistory(silh, mhi, timestamp, mhi_duration); +} + + +CV_IMPL void +cvCalcMotionGradient( const CvArr* mhimg, CvArr* maskimg, + CvArr* orientation, + double delta1, double delta2, + int aperture_size ) +{ + cv::Mat mhi = cv::cvarrToMat(mhimg); + const cv::Mat mask = cv::cvarrToMat(maskimg), orient = cv::cvarrToMat(orientation); + cv::calcMotionGradient(mhi, mask, orient, delta1, delta2, aperture_size); +} + + +CV_IMPL double +cvCalcGlobalOrientation( const void* orientation, const void* maskimg, const void* mhimg, + double curr_mhi_timestamp, double mhi_duration ) +{ + cv::Mat mhi = cv::cvarrToMat(mhimg); + cv::Mat mask = cv::cvarrToMat(maskimg), orient = cv::cvarrToMat(orientation); + return cv::calcGlobalOrientation(orient, mask, mhi, curr_mhi_timestamp, mhi_duration); +} + + +CV_IMPL CvSeq* +cvSegmentMotion( const CvArr* mhimg, CvArr* segmaskimg, CvMemStorage* storage, + double timestamp, double segThresh ) +{ + cv::Mat mhi = cv::cvarrToMat(mhimg); + const cv::Mat segmask = cv::cvarrToMat(segmaskimg); + std::vector brs; + cv::segmentMotion(mhi, segmask, brs, timestamp, segThresh); + CvSeq* seq = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvConnectedComp), storage); + + CvConnectedComp comp; + memset(&comp, 0, sizeof(comp)); + for( size_t i = 0; i < brs.size(); i++ ) + { + cv::Rect roi = brs[i]; + float compLabel = (float)(i+1); + int x, y, area = 0; + + cv::Mat part = segmask(roi); + for( y = 0; y < roi.height; y++ ) + { + const float* partptr = part.ptr(y); + for( x = 0; x < roi.width; x++ ) + area += partptr[x] == compLabel; + } + + comp.value = cv::Scalar(compLabel); + comp.rect = roi; + comp.area = area; + cvSeqPush(seq, &comp); + } + + return seq; +} + + +///////////////////////////////// Kalman /////////////////////////////// + +CV_IMPL CvKalman* +cvCreateKalman( int DP, int MP, int CP ) +{ + CvKalman *kalman = 0; + + if( DP <= 0 || MP <= 0 ) + CV_Error( CV_StsOutOfRange, + "state and measurement vectors must have positive number of dimensions" ); + + if( CP < 0 ) + CP = DP; + + /* allocating memory for the structure */ + kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )); + memset( kalman, 0, sizeof(*kalman)); + + kalman->DP = DP; + kalman->MP = MP; + kalman->CP = CP; + + kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ); + cvZero( kalman->state_pre ); + + kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ); + cvZero( kalman->state_post ); + + kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ); + cvSetIdentity( kalman->transition_matrix ); + + kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ); + cvSetIdentity( kalman->process_noise_cov ); + + kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ); + cvZero( kalman->measurement_matrix ); + + kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ); + cvSetIdentity( kalman->measurement_noise_cov ); + + kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ); + + kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ); + cvZero( kalman->error_cov_post ); + + kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ); + + if( CP > 0 ) + { + kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ); + cvZero( kalman->control_matrix ); + } + + kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ); + kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ); + kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ); + kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ); + kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ); + +#if 1 + kalman->PosterState = kalman->state_pre->data.fl; + kalman->PriorState = kalman->state_post->data.fl; + kalman->DynamMatr = kalman->transition_matrix->data.fl; + kalman->MeasurementMatr = kalman->measurement_matrix->data.fl; + kalman->MNCovariance = kalman->measurement_noise_cov->data.fl; + kalman->PNCovariance = kalman->process_noise_cov->data.fl; + kalman->KalmGainMatr = kalman->gain->data.fl; + kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl; + kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl; +#endif + + return kalman; +} + + +CV_IMPL void +cvReleaseKalman( CvKalman** _kalman ) +{ + CvKalman *kalman; + + if( !_kalman ) + CV_Error( CV_StsNullPtr, "" ); + + kalman = *_kalman; + if( !kalman ) + return; + + /* freeing the memory */ + cvReleaseMat( &kalman->state_pre ); + cvReleaseMat( &kalman->state_post ); + cvReleaseMat( &kalman->transition_matrix ); + cvReleaseMat( &kalman->control_matrix ); + cvReleaseMat( &kalman->measurement_matrix ); + cvReleaseMat( &kalman->process_noise_cov ); + cvReleaseMat( &kalman->measurement_noise_cov ); + cvReleaseMat( &kalman->error_cov_pre ); + cvReleaseMat( &kalman->gain ); + cvReleaseMat( &kalman->error_cov_post ); + cvReleaseMat( &kalman->temp1 ); + cvReleaseMat( &kalman->temp2 ); + cvReleaseMat( &kalman->temp3 ); + cvReleaseMat( &kalman->temp4 ); + cvReleaseMat( &kalman->temp5 ); + + memset( kalman, 0, sizeof(*kalman)); + + /* deallocating the structure */ + cvFree( _kalman ); +} + + +CV_IMPL const CvMat* +cvKalmanPredict( CvKalman* kalman, const CvMat* control ) +{ + if( !kalman ) + CV_Error( CV_StsNullPtr, "" ); + + /* update the state */ + /* x'(k) = A*x(k) */ + cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ); + + if( control && kalman->CP > 0 ) + /* x'(k) = x'(k) + B*u(k) */ + cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ); + + /* update error covariance matrices */ + /* temp1 = A*P(k) */ + cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ); + + /* P'(k) = temp1*At + Q */ + cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1, + kalman->error_cov_pre, CV_GEMM_B_T ); + + /* handle the case when there will be measurement before the next predict */ + cvCopy(kalman->state_pre, kalman->state_post); + + return kalman->state_pre; +} + + +CV_IMPL const CvMat* +cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement ) +{ + if( !kalman || !measurement ) + CV_Error( CV_StsNullPtr, "" ); + + /* temp2 = H*P'(k) */ + cvMatMulAdd( kalman->measurement_matrix, kalman->error_cov_pre, 0, kalman->temp2 ); + /* temp3 = temp2*Ht + R */ + cvGEMM( kalman->temp2, kalman->measurement_matrix, 1, + kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ); + + /* temp4 = inv(temp3)*temp2 = Kt(k) */ + cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ); + + /* K(k) */ + cvTranspose( kalman->temp4, kalman->gain ); + + /* temp5 = z(k) - H*x'(k) */ + cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ); + + /* x(k) = x'(k) + K(k)*temp5 */ + cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ); + + /* P(k) = P'(k) - K(k)*temp2 */ + cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1, + kalman->error_cov_post, 0 ); + + return kalman->state_post; +} + +///////////////////////////////////// Optical Flow //////////////////////////////// + +CV_IMPL void +cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB, + void* /*pyrarrA*/, void* /*pyrarrB*/, + const CvPoint2D32f * featuresA, + CvPoint2D32f * featuresB, + int count, CvSize winSize, int level, + char *status, float *error, + CvTermCriteria criteria, int flags ) +{ + if( count <= 0 ) + return; + CV_Assert( featuresA && featuresB ); + cv::Mat A = cv::cvarrToMat(arrA), B = cv::cvarrToMat(arrB); + cv::Mat ptA(count, 1, CV_32FC2, (void*)featuresA); + cv::Mat ptB(count, 1, CV_32FC2, (void*)featuresB); + cv::Mat st, err; + + if( status ) + st = cv::Mat(count, 1, CV_8U, (void*)status); + if( error ) + err = cv::Mat(count, 1, CV_32F, (void*)error); + cv::calcOpticalFlowPyrLK( A, B, ptA, ptB, st, + error ? cv::_OutputArray(err) : cv::noArray(), + winSize, level, criteria, flags); +} + + +CV_IMPL void cvCalcOpticalFlowFarneback( + const CvArr* _prev, const CvArr* _next, + CvArr* _flow, double pyr_scale, int levels, + int winsize, int iterations, int poly_n, + double poly_sigma, int flags ) +{ + cv::Mat prev = cv::cvarrToMat(_prev), next = cv::cvarrToMat(_next); + cv::Mat flow = cv::cvarrToMat(_flow); + CV_Assert( flow.size() == prev.size() && flow.type() == CV_32FC2 ); + cv::calcOpticalFlowFarneback( prev, next, flow, pyr_scale, levels, + winsize, iterations, poly_n, poly_sigma, flags ); +} + + +CV_IMPL int +cvEstimateRigidTransform( const CvArr* arrA, const CvArr* arrB, CvMat* arrM, int full_affine ) +{ + cv::Mat matA = cv::cvarrToMat(arrA), matB = cv::cvarrToMat(arrB); + const cv::Mat matM0 = cv::cvarrToMat(arrM); + + cv::Mat matM = cv::estimateRigidTransform(matA, matB, full_affine != 0); + if( matM.empty() ) + { + matM = cv::cvarrToMat(arrM); + matM.setTo(cv::Scalar::all(0)); + return 0; + } + matM.convertTo(matM0, matM0.type()); + return 1; +} diff --git a/modules/video/src/simpleflow.hpp b/modules/video/src/simpleflow.hpp deleted file mode 100644 index 363fdea168..0000000000 --- a/modules/video/src/simpleflow.hpp +++ /dev/null @@ -1,86 +0,0 @@ -/*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*/ - -#ifndef __OPENCV_SIMPLEFLOW_H__ -#define __OPENCV_SIMPLEFLOW_H__ - -#include - -#define MASK_TRUE_VALUE 255 -#define UNKNOWN_FLOW_THRESH 1e9 - -namespace cv { - -inline static float dist(const Vec3b& p1, const Vec3b& p2) { - return (float)((p1[0] - p2[0]) * (p1[0] - p2[0]) + - (p1[1] - p2[1]) * (p1[1] - p2[1]) + - (p1[2] - p2[2]) * (p1[2] - p2[2])); -} - -inline static float dist(const Vec2f& p1, const Vec2f& p2) { - return (p1[0] - p2[0]) * (p1[0] - p2[0]) + - (p1[1] - p2[1]) * (p1[1] - p2[1]); -} - -inline static float dist(const Point2f& p1, const Point2f& p2) { - return (p1.x - p2.x) * (p1.x - p2.x) + - (p1.y - p2.y) * (p1.y - p2.y); -} - -inline static float dist(float x1, float y1, float x2, float y2) { - return (x1 - x2) * (x1 - x2) + - (y1 - y2) * (y1 - y2); -} - -inline static int dist(int x1, int y1, int x2, int y2) { - return (x1 - x2) * (x1 - x2) + - (y1 - y2) * (y1 - y2); -} - -template -inline static T min(T t1, T t2, T t3) { - return (t1 <= t2 && t1 <= t3) ? t1 : min(t2, t3); -} - -} - -#endif