modules/legacy/src/optflowbm.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "precomp.hpp"
  43
  44
  45 static inline int cmpBlocks(const uchar* A, const uchar* B, int Bstep, CvSize blockSize )
  46 {
  47     int x, s = 0;
  48     for( ; blockSize.height--; A += blockSize.width, B += Bstep )
  49     {
  50         for( x = 0; x <= blockSize.width - 4; x += 4 )
  51             s += std::abs(A[x] - B[x]) + std::abs(A[x+1] - B[x+1]) +
  52                 std::abs(A[x+2] - B[x+2]) + std::abs(A[x+3] - B[x+3]);
  53         for( ; x < blockSize.width; x++ )
  54             s += std::abs(A[x] - B[x]);
  55     }
  56     return s;
  57 }
  58
  59
  60 CV_IMPL void
  61 cvCalcOpticalFlowBM( const void* srcarrA, const void* srcarrB,
  62                      CvSize blockSize, CvSize shiftSize,
  63                      CvSize maxRange, int usePrevious,
  64                      void* velarrx, void* velarry )
  65 {
  66     CvMat stubA, *srcA = cvGetMat( srcarrA, &stubA );
  67     CvMat stubB, *srcB = cvGetMat( srcarrB, &stubB );
  68
  69     CvMat stubx, *velx = cvGetMat( velarrx, &stubx );
  70     CvMat stuby, *vely = cvGetMat( velarry, &stuby );
  71
  72     if( !CV_ARE_TYPES_EQ( srcA, srcB ))
  73         CV_Error( CV_StsUnmatchedFormats, "Source images have different formats" );
  74
  75     if( !CV_ARE_TYPES_EQ( velx, vely ))
  76         CV_Error( CV_StsUnmatchedFormats, "Destination images have different formats" );
  77
  78     CvSize velSize(
  79         (srcA->width - blockSize.width + shiftSize.width)/shiftSize.width,
  80         (srcA->height - blockSize.height + shiftSize.height)/shiftSize.height
  81     );
  82
  83     if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
  84         !CV_ARE_SIZES_EQ( velx, vely ) ||
  85         velx->width != velSize.width ||
  86         vely->height != velSize.height )
  87         CV_Error( CV_StsUnmatchedSizes, "" );
  88
  89     if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
  90         CV_MAT_TYPE( velx->type ) != CV_32FC1 )
  91         CV_Error( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
  92                                            "destination images must have 32fC1 type" );
  93
  94     if( srcA->step != srcB->step || velx->step != vely->step )
  95         CV_Error( CV_BadStep, "two source or two destination images have different steps" );
  96
  97     const int SMALL_DIFF=2;
  98     const int BIG_DIFF=128;
  99
 100     // scanning scheme coordinates
 101     std::vector<CvPoint> _ss((2 * maxRange.width + 1) * (2 * maxRange.height + 1));
 102     CvPoint* ss = &_ss[0];
 103     int ss_count = 0;
 104
 105     int blWidth = blockSize.width, blHeight = blockSize.height;
 106     int blSize = blWidth*blHeight;
 107     int acceptLevel = blSize * SMALL_DIFF;
 108     int escapeLevel = blSize * BIG_DIFF;
 109
 110     int i, j;
 111
 112     std::vector<uchar> _blockA(cvAlign(blSize + 16, 16));
 113     uchar* blockA = (uchar*)cvAlignPtr(&_blockA[0], 16);
 114
 115     // Calculate scanning scheme
 116     int min_count = MIN( maxRange.width, maxRange.height );
 117
 118     // use spiral search pattern
 119     //
 120     //     9 10 11 12
 121     //     8  1  2 13
 122     //     7  *  3 14
 123     //     6  5  4 15
 124     //... 20 19 18 17
 125     //
 126
 127     for( i = 0; i < min_count; i++ )
 128     {
 129         // four cycles along sides
 130         int x = -i-1, y = x;
 131
 132         // upper side
 133         for( j = -i; j <= i + 1; j++, ss_count++ )
 134         {
 135             ss[ss_count].x = ++x;
 136             ss[ss_count].y = y;
 137         }
 138
 139         // right side
 140         for( j = -i; j <= i + 1; j++, ss_count++ )
 141         {
 142             ss[ss_count].x = x;
 143             ss[ss_count].y = ++y;
 144         }
 145
 146         // bottom side
 147         for( j = -i; j <= i + 1; j++, ss_count++ )
 148         {
 149             ss[ss_count].x = --x;
 150             ss[ss_count].y = y;
 151         }
 152
 153         // left side
 154         for( j = -i; j <= i + 1; j++, ss_count++ )
 155         {
 156             ss[ss_count].x = x;
 157             ss[ss_count].y = --y;
 158         }
 159     }
 160
 161     // the rest part
 162     if( maxRange.width < maxRange.height )
 163     {
 164         int xleft = -min_count;
 165
 166         // cycle by neighbor rings
 167         for( i = min_count; i < maxRange.height; i++ )
 168         {
 169             // two cycles by x
 170             int y = -(i + 1);
 171             int x = xleft;
 172
 173             // upper side
 174             for( j = -maxRange.width; j <= maxRange.width; j++, ss_count++, x++ )
 175             {
 176                 ss[ss_count].x = x;
 177                 ss[ss_count].y = y;
 178             }
 179
 180             x = xleft;
 181             y = -y;
 182             // bottom side
 183             for( j = -maxRange.width; j <= maxRange.width; j++, ss_count++, x++ )
 184             {
 185                 ss[ss_count].x = x;
 186                 ss[ss_count].y = y;
 187             }
 188         }
 189     }
 190     else if( maxRange.width > maxRange.height )
 191     {
 192         int yupper = -min_count;
 193
 194         // cycle by neighbor rings
 195         for( i = min_count; i < maxRange.width; i++ )
 196         {
 197             // two cycles by y
 198             int x = -(i + 1);
 199             int y = yupper;
 200
 201             // left side
 202             for( j = -maxRange.height; j <= maxRange.height; j++, ss_count++, y++ )
 203             {
 204                 ss[ss_count].x = x;
 205                 ss[ss_count].y = y;
 206             }
 207
 208             y = yupper;
 209             x = -x;
 210             // right side
 211             for( j = -maxRange.height; j <= maxRange.height; j++, ss_count++, y++ )
 212             {
 213                 ss[ss_count].x = x;
 214                 ss[ss_count].y = y;
 215             }
 216         }
 217     }
 218
 219     int maxX = srcB->cols - blockSize.width, maxY = srcB->rows - blockSize.height;
 220     const uchar* Adata = srcA->data.ptr;
 221     const uchar* Bdata = srcB->data.ptr;
 222     int Astep = srcA->step, Bstep = srcB->step;
 223
 224     // compute the flow
 225     for( i = 0; i < velx->rows; i++ )
 226     {
 227         float* vx = (float*)(velx->data.ptr + velx->step*i);
 228         float* vy = (float*)(vely->data.ptr + vely->step*i);
 229
 230         for( j = 0; j < velx->cols; j++ )
 231         {
 232             int X1 = j*shiftSize.width, Y1 = i*shiftSize.height, X2, Y2;
 233             int offX = 0, offY = 0;
 234
 235             if( usePrevious )
 236             {
 237                 offX = cvRound(vx[j]);
 238                 offY = cvRound(vy[j]);
 239             }
 240
 241             int k;
 242             for( k = 0; k < blHeight; k++ )
 243                 memcpy( blockA + k*blWidth, Adata + Astep*(Y1 + k) + X1, blWidth );
 244
 245             X2 = X1 + offX;
 246             Y2 = Y1 + offY;
 247             int dist = INT_MAX;
 248             if( 0 <= X2 && X2 <= maxX && 0 <= Y2 && Y2 <= maxY )
 249                 dist = cmpBlocks( blockA, Bdata + Bstep*Y2 + X2, Bstep, blockSize );
 250
 251             int countMin = 1;
 252             int sumx = offX, sumy = offY;
 253
 254             if( dist > acceptLevel )
 255             {
 256                 // do brute-force search
 257                 for( k = 0; k < ss_count; k++ )
 258                 {
 259                     int dx = offX + ss[k].x;
 260                     int dy = offY + ss[k].y;
 261                     X2 = X1 + dx;
 262                     Y2 = Y1 + dy;
 263
 264                     if( !(0 <= X2 && X2 <= maxX && 0 <= Y2 && Y2 <= maxY) )
 265                         continue;
 266
 267                     int tmpDist = cmpBlocks( blockA, Bdata + Bstep*Y2 + X2, Bstep, blockSize );
 268                     if( tmpDist < acceptLevel )
 269                     {
 270                         sumx = dx; sumy = dy;
 271                         countMin = 1;
 272                         break;
 273                     }
 274
 275                     if( tmpDist < dist )
 276                     {
 277                         dist = tmpDist;
 278                         sumx = dx; sumy = dy;
 279                         countMin = 1;
 280                     }
 281                     else if( tmpDist == dist )
 282                     {
 283                         sumx += dx; sumy += dy;
 284                         countMin++;
 285                     }
 286                 }
 287
 288                 if( dist > escapeLevel )
 289                 {
 290                     sumx = offX;
 291                     sumy = offY;
 292                     countMin = 1;
 293                 }
 294             }
 295
 296             vx[j] = (float)sumx/countMin;
 297             vy[j] = (float)sumy/countMin;
 298         }
 299     }
 300 }
 301
 302 /* End of file. */