git.maemo.org Git - opencv/blob - src/cv/cvoptflowbm.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 "_cv.h"
  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     {
  80         (srcA->width - blockSize.width)/shiftSize.width,
  81         (srcA->height - blockSize.height)/shiftSize.height
  82     };
  83
  84     if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
  85         !CV_ARE_SIZES_EQ( velx, vely ) ||
  86         velx->width != velSize.width ||
  87         vely->height != velSize.height )
  88         CV_Error( CV_StsUnmatchedSizes, "" );
  89
  90     if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
  91         CV_MAT_TYPE( velx->type ) != CV_32FC1 )
  92         CV_Error( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
  93                                            "destination images must have 32fC1 type" );
  94
  95     if( srcA->step != srcB->step || velx->step != vely->step )
  96         CV_Error( CV_BadStep, "two source or two destination images have different steps" );
  97
  98     const int SMALL_DIFF=2;
  99     const int BIG_DIFF=128;
 100
 101     // scanning scheme coordinates
 102     cv::vector<CvPoint> _ss((2 * maxRange.width + 1) * (2 * maxRange.height + 1));
 103     CvPoint* ss = &_ss[0];
 104     int ss_count = 0;
 105
 106     int blWidth = blockSize.width, blHeight = blockSize.height;
 107     int blSize = blWidth*blHeight;
 108     int acceptLevel = blSize * SMALL_DIFF;
 109     int escapeLevel = blSize * BIG_DIFF;
 110
 111     int i, j;
 112
 113     cv::vector<uchar> _blockA(cvAlign(blSize + 16, 16));
 114     uchar* blockA = (uchar*)cvAlignPtr(&_blockA[0], 16);
 115
 116     // Calculate scanning scheme
 117     int min_count = MIN( maxRange.width, maxRange.height );
 118
 119     // use spiral search pattern
 120     //
 121     //     9 10 11 12
 122     //     8  1  2 13
 123     //     7  *  3 14
 124     //     6  5  4 15
 125     //... 20 19 18 17
 126     //
 127
 128     for( i = 0; i < min_count; i++ )
 129     {
 130         // four cycles along sides
 131         int x = -i-1, y = x;
 132
 133         // upper side
 134         for( j = -i; j <= i + 1; j++, ss_count++ )
 135         {
 136             ss[ss_count].x = ++x;
 137             ss[ss_count].y = y;
 138         }
 139
 140         // right side
 141         for( j = -i; j <= i + 1; j++, ss_count++ )
 142         {
 143             ss[ss_count].x = x;
 144             ss[ss_count].y = ++y;
 145         }
 146
 147         // bottom side
 148         for( j = -i; j <= i + 1; j++, ss_count++ )
 149         {
 150             ss[ss_count].x = --x;
 151             ss[ss_count].y = y;
 152         }
 153
 154         // left side
 155         for( j = -i; j <= i + 1; j++, ss_count++ )
 156         {
 157             ss[ss_count].x = x;
 158             ss[ss_count].y = --y;
 159         }
 160     }
 161
 162     // the rest part
 163     if( maxRange.width < maxRange.height )
 164     {
 165         int xleft = -min_count;
 166
 167         // cycle by neighbor rings
 168         for( i = min_count; i < maxRange.height; i++ )
 169         {
 170             // two cycles by x
 171             int y = -(i + 1);
 172             int x = xleft;
 173
 174             // upper side
 175             for( j = -maxRange.width; j <= maxRange.width; j++, ss_count++, x++ )
 176             {
 177                 ss[ss_count].x = x;
 178                 ss[ss_count].y = y;
 179             }
 180
 181             x = xleft;
 182             y = -y;
 183             // bottom side
 184             for( j = -maxRange.width; j <= maxRange.width; j++, ss_count++, x++ )
 185             {
 186                 ss[ss_count].x = x;
 187                 ss[ss_count].y = y;
 188             }
 189         }
 190     }
 191     else if( maxRange.width > maxRange.height )
 192     {
 193         int yupper = -min_count;
 194
 195         // cycle by neighbor rings
 196         for( i = min_count; i < maxRange.width; i++ )
 197         {
 198             // two cycles by y
 199             int x = -(i + 1);
 200             int y = yupper;
 201
 202             // left side
 203             for( j = -maxRange.height; j <= maxRange.height; j++, ss_count++, y++ )
 204             {
 205                 ss[ss_count].x = x;
 206                 ss[ss_count].y = y;
 207             }
 208
 209             y = yupper;
 210             x = -x;
 211             // right side
 212             for( j = -maxRange.height; j <= maxRange.height; j++, ss_count++, y++ )
 213             {
 214                 ss[ss_count].x = x;
 215                 ss[ss_count].y = y;
 216             }
 217         }
 218     }
 219
 220     int maxX = srcB->cols - blockSize.width, maxY = srcB->rows - blockSize.height;
 221     const uchar* Adata = srcA->data.ptr;
 222     const uchar* Bdata = srcB->data.ptr;
 223     int Astep = srcA->step, Bstep = srcB->step;
 224
 225     // compute the flow
 226     for( i = 0; i < velx->rows; i++ )
 227     {
 228         float* vx = (float*)(velx->data.ptr + velx->step*i);
 229         float* vy = (float*)(vely->data.ptr + vely->step*i);
 230
 231         for( j = 0; j < velx->cols; j++ )
 232         {
 233             int X1 = j*shiftSize.width, Y1 = i*shiftSize.height, X2, Y2;
 234             int offX = 0, offY = 0;
 235
 236             if( usePrevious )
 237             {
 238                 offX = cvRound(vx[j]);
 239                 offY = cvRound(vy[j]);
 240             }
 241
 242             int k;
 243             for( k = 0; k < blHeight; k++ )
 244                 memcpy( blockA + k*blWidth, Adata + Astep*(Y1 + k) + X1, blWidth );
 245
 246             X2 = X1 + offX;
 247             Y2 = Y1 + offY;
 248             int dist = INT_MAX;
 249             if( 0 <= X2 && X2 <= maxX && 0 <= Y2 && Y2 <= maxY )
 250                 dist = cmpBlocks( blockA, Bdata + Bstep*Y2 + X2, Bstep, blockSize );
 251
 252             int countMin = 1;
 253             int sumx = offX, sumy = offY;
 254
 255             if( dist > acceptLevel )
 256             {
 257                 // do brute-force search
 258                 for( k = 0; k < ss_count; k++ )
 259                 {
 260                     int dx = offX + ss[k].x;
 261                     int dy = offY + ss[k].y;
 262                     X2 = X1 + dx;
 263                     Y2 = Y1 + dy;
 264
 265                     if( !(0 <= X2 && X2 <= maxX && 0 <= Y2 && Y2 <= maxY) )
 266                         continue;
 267
 268                     int tmpDist = cmpBlocks( blockA, Bdata + Bstep*Y2 + X2, Bstep, blockSize );
 269                     if( tmpDist < acceptLevel )
 270                     {
 271                         sumx = dx; sumy = dy;
 272                         countMin = 1;
 273                         break;
 274                     }
 275
 276                     if( tmpDist < dist )
 277                     {
 278                         dist = tmpDist;
 279                         sumx = dx; sumy = dy;
 280                         countMin = 1;
 281                     }
 282                     else if( tmpDist == dist )
 283                     {
 284                         sumx += dx; sumy += dy;
 285                         countMin++;
 286                     }
 287                 }
 288
 289                 if( dist > escapeLevel )
 290                 {
 291                     sumx = offX;
 292                     sumy = offY;
 293                     countMin = 1;
 294                 }
 295             }
 296
 297             vx[j] = (float)sumx/countMin;
 298             vy[j] = (float)sumy/countMin;
 299         }
 300     }
 301 }
 302
 303 /* End of file. */