refactored video module; use the new-style algorithms now

modules/video/src/affineflow.cpp

+/*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<uchar>* 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 = <size for patches> + <size for pyramids> */
+    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<char> _status;
+    cv::AutoBuffer<uchar> buffer;
+    cv::AutoBuffer<uchar> 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 = <size for patches> + <size for pyramids> */
+    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);
+            }
+        }
+    }
+}

modules/video/src/compat_video.cpp

+/*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<cv::Rect> 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<float>(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;
+}

modules/video/src/simpleflow.hpp

-/*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 <vector>
-
-#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<class T>
-inline static T min(T t1, T t2, T t3) {
-  return (t1 <= t2 && t1 <= t3) ? t1 : min(t2, t3);
-}
-
-}
-
-#endif