modules/video/src/motempl.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 /* motion templates */
  46 CV_IMPL void
  47 cvUpdateMotionHistory( const void* silhouette, void* mhimg,
  48                        double timestamp, double mhi_duration )
  49 {
  50     CvMat  silhstub, *silh = cvGetMat(silhouette, &silhstub);
  51     CvMat  mhistub, *mhi = cvGetMat(mhimg, &mhistub);
  52
  53     if( !CV_IS_MASK_ARR( silh ))
  54         CV_Error( CV_StsBadMask, "" );
  55
  56     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 )
  57         CV_Error( CV_StsUnsupportedFormat, "" );
  58
  59     if( !CV_ARE_SIZES_EQ( mhi, silh ))
  60         CV_Error( CV_StsUnmatchedSizes, "" );
  61
  62     CvSize size = cvGetMatSize( mhi );
  63
  64     if( CV_IS_MAT_CONT( mhi->type & silh->type ))
  65     {
  66         size.width *= size.height;
  67         size.height = 1;
  68     }
  69
  70     float ts = (float)timestamp;
  71     float delbound = (float)(timestamp - mhi_duration);
  72     int x, y;
  73 #if CV_SSE2
  74     volatile bool useSIMD = cv::checkHardwareSupport(CV_CPU_SSE2);
  75 #endif
  76
  77     for( y = 0; y < size.height; y++ )
  78     {
  79         const uchar* silhData = silh->data.ptr + silh->step*y;
  80         float* mhiData = (float*)(mhi->data.ptr + mhi->step*y);
  81         x = 0;
  82
  83 #if CV_SSE2
  84         if( useSIMD )
  85         {
  86             __m128 ts4 = _mm_set1_ps(ts), db4 = _mm_set1_ps(delbound);
  87             for( ; x <= size.width - 8; x += 8 )
  88             {
  89                 __m128i z = _mm_setzero_si128();
  90                 __m128i s = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(silhData + x)), z);
  91                 __m128 s0 = _mm_cvtepi32_ps(_mm_unpacklo_epi16(s, z)), s1 = _mm_cvtepi32_ps(_mm_unpackhi_epi16(s, z));
  92                 __m128 v0 = _mm_loadu_ps(mhiData + x), v1 = _mm_loadu_ps(mhiData + x + 4);
  93                 __m128 fz = _mm_setzero_ps();
  94
  95                 v0 = _mm_and_ps(v0, _mm_cmpge_ps(v0, db4));
  96                 v1 = _mm_and_ps(v1, _mm_cmpge_ps(v1, db4));
  97
  98                 __m128 m0 = _mm_and_ps(_mm_xor_ps(v0, ts4), _mm_cmpneq_ps(s0, fz));
  99                 __m128 m1 = _mm_and_ps(_mm_xor_ps(v1, ts4), _mm_cmpneq_ps(s1, fz));
 100
 101                 v0 = _mm_xor_ps(v0, m0);
 102                 v1 = _mm_xor_ps(v1, m1);
 103
 104                 _mm_storeu_ps(mhiData + x, v0);
 105                 _mm_storeu_ps(mhiData + x + 4, v1);
 106             }
 107         }
 108 #endif
 109
 110         for( ; x < size.width; x++ )
 111         {
 112             float val = mhiData[x];
 113             val = silhData[x] ? ts : val < delbound ? 0 : val;
 114             mhiData[x] = val;
 115         }
 116     }
 117 }
 118
 119
 120 CV_IMPL void
 121 cvCalcMotionGradient( const CvArr* mhiimg, CvArr* maskimg,
 122                       CvArr* orientation,
 123                       double delta1, double delta2,
 124                       int aperture_size )
 125 {
 126     cv::Ptr<CvMat> dX_min, dY_max;
 127
 128     CvMat  mhistub, *mhi = cvGetMat(mhiimg, &mhistub);
 129     CvMat  maskstub, *mask = cvGetMat(maskimg, &maskstub);
 130     CvMat  orientstub, *orient = cvGetMat(orientation, &orientstub);
 131     CvMat  dX_min_row, dY_max_row, orient_row, mask_row;
 132     CvSize size;
 133     int x, y;
 134
 135     float  gradient_epsilon = 1e-4f * aperture_size * aperture_size;
 136     float  min_delta, max_delta;
 137
 138     if( !CV_IS_MASK_ARR( mask ))
 139         CV_Error( CV_StsBadMask, "" );
 140
 141     if( aperture_size < 3 || aperture_size > 7 || (aperture_size & 1) == 0 )
 142         CV_Error( CV_StsOutOfRange, "aperture_size must be 3, 5 or 7" );
 143
 144     if( delta1 <= 0 || delta2 <= 0 )
 145         CV_Error( CV_StsOutOfRange, "both delta's must be positive" );
 146
 147     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( orient->type ) != CV_32FC1 )
 148         CV_Error( CV_StsUnsupportedFormat,
 149         "MHI and orientation must be single-channel floating-point images" );
 150
 151     if( !CV_ARE_SIZES_EQ( mhi, mask ) || !CV_ARE_SIZES_EQ( orient, mhi ))
 152         CV_Error( CV_StsUnmatchedSizes, "" );
 153
 154     if( orient->data.ptr == mhi->data.ptr )
 155         CV_Error( CV_StsInplaceNotSupported, "orientation image must be different from MHI" );
 156
 157     if( delta1 > delta2 )
 158     {
 159         double t;
 160         CV_SWAP( delta1, delta2, t );
 161     }
 162
 163     size = cvGetMatSize( mhi );
 164     min_delta = (float)delta1;
 165     max_delta = (float)delta2;
 166     dX_min = cvCreateMat( mhi->rows, mhi->cols, CV_32F );
 167     dY_max = cvCreateMat( mhi->rows, mhi->cols, CV_32F );
 168
 169     // calc Dx and Dy
 170     cvSobel( mhi, dX_min, 1, 0, aperture_size );
 171     cvSobel( mhi, dY_max, 0, 1, aperture_size );
 172     cvGetRow( dX_min, &dX_min_row, 0 );
 173     cvGetRow( dY_max, &dY_max_row, 0 );
 174     cvGetRow( orient, &orient_row, 0 );
 175     cvGetRow( mask, &mask_row, 0 );
 176
 177     // calc gradient
 178     for( y = 0; y < size.height; y++ )
 179     {
 180         dX_min_row.data.ptr = dX_min->data.ptr + y*dX_min->step;
 181         dY_max_row.data.ptr = dY_max->data.ptr + y*dY_max->step;
 182         orient_row.data.ptr = orient->data.ptr + y*orient->step;
 183         mask_row.data.ptr = mask->data.ptr + y*mask->step;
 184         cvCartToPolar( &dX_min_row, &dY_max_row, 0, &orient_row, 1 );
 185
 186         // make orientation zero where the gradient is very small
 187         for( x = 0; x < size.width; x++ )
 188         {
 189             float dY = dY_max_row.data.fl[x];
 190             float dX = dX_min_row.data.fl[x];
 191
 192             if( fabs(dX) < gradient_epsilon && fabs(dY) < gradient_epsilon )
 193             {
 194                 mask_row.data.ptr[x] = 0;
 195                 orient_row.data.i[x] = 0;
 196             }
 197             else
 198                 mask_row.data.ptr[x] = 1;
 199         }
 200     }
 201
 202     cvErode( mhi, dX_min, 0, (aperture_size-1)/2);
 203     cvDilate( mhi, dY_max, 0, (aperture_size-1)/2);
 204
 205     // mask off pixels which have little motion difference in their neighborhood
 206     for( y = 0; y < size.height; y++ )
 207     {
 208         dX_min_row.data.ptr = dX_min->data.ptr + y*dX_min->step;
 209         dY_max_row.data.ptr = dY_max->data.ptr + y*dY_max->step;
 210         mask_row.data.ptr = mask->data.ptr + y*mask->step;
 211         orient_row.data.ptr = orient->data.ptr + y*orient->step;
 212
 213         for( x = 0; x < size.width; x++ )
 214         {
 215             float d0 = dY_max_row.data.fl[x] - dX_min_row.data.fl[x];
 216
 217             if( mask_row.data.ptr[x] == 0 || d0 < min_delta || max_delta < d0 )
 218             {
 219                 mask_row.data.ptr[x] = 0;
 220                 orient_row.data.i[x] = 0;
 221             }
 222         }
 223     }
 224 }
 225
 226
 227 CV_IMPL double
 228 cvCalcGlobalOrientation( const void* orientation, const void* maskimg, const void* mhiimg,
 229                          double curr_mhi_timestamp, double mhi_duration )
 230 {
 231     int hist_size = 12;
 232     cv::Ptr<CvHistogram> hist;
 233
 234     CvMat  mhistub, *mhi = cvGetMat(mhiimg, &mhistub);
 235     CvMat  maskstub, *mask = cvGetMat(maskimg, &maskstub);
 236     CvMat  orientstub, *orient = cvGetMat(orientation, &orientstub);
 237     void*  _orient;
 238     float _ranges[] = { 0, 360 };
 239     float* ranges = _ranges;
 240     int base_orient;
 241     float shift_orient = 0, shift_weight = 0;
 242     float a, b, fbase_orient;
 243     float delbound;
 244     CvMat mhi_row, mask_row, orient_row;
 245     int x, y, mhi_rows, mhi_cols;
 246
 247     if( !CV_IS_MASK_ARR( mask ))
 248         CV_Error( CV_StsBadMask, "" );
 249
 250     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( orient->type ) != CV_32FC1 )
 251         CV_Error( CV_StsUnsupportedFormat,
 252         "MHI and orientation must be single-channel floating-point images" );
 253
 254     if( !CV_ARE_SIZES_EQ( mhi, mask ) || !CV_ARE_SIZES_EQ( orient, mhi ))
 255         CV_Error( CV_StsUnmatchedSizes, "" );
 256
 257     if( mhi_duration <= 0 )
 258         CV_Error( CV_StsOutOfRange, "MHI duration must be positive" );
 259
 260     if( orient->data.ptr == mhi->data.ptr )
 261         CV_Error( CV_StsInplaceNotSupported, "orientation image must be different from MHI" );
 262
 263     // calculate histogram of different orientation values
 264     hist = cvCreateHist( 1, &hist_size, CV_HIST_ARRAY, &ranges );
 265     _orient = orient;
 266     cvCalcArrHist( &_orient, hist, 0, mask );
 267
 268     // find the maximum index (the dominant orientation)
 269     cvGetMinMaxHistValue( hist, 0, 0, 0, &base_orient );
 270     fbase_orient = base_orient*360.f/hist_size;
 271
 272     // override timestamp with the maximum value in MHI
 273     cvMinMaxLoc( mhi, 0, &curr_mhi_timestamp, 0, 0, mask );
 274
 275     // find the shift relative to the dominant orientation as weighted sum of relative angles
 276     a = (float)(254. / 255. / mhi_duration);
 277     b = (float)(1. - curr_mhi_timestamp * a);
 278     delbound = (float)(curr_mhi_timestamp - mhi_duration);
 279     mhi_rows = mhi->rows;
 280     mhi_cols = mhi->cols;
 281
 282     if( CV_IS_MAT_CONT( mhi->type & mask->type & orient->type ))
 283     {
 284         mhi_cols *= mhi_rows;
 285         mhi_rows = 1;
 286     }
 287
 288     cvGetRow( mhi, &mhi_row, 0 );
 289     cvGetRow( mask, &mask_row, 0 );
 290     cvGetRow( orient, &orient_row, 0 );
 291
 292     /*
 293        a = 254/(255*dt)
 294        b = 1 - t*a = 1 - 254*t/(255*dur) =
 295        (255*dt - 254*t)/(255*dt) =
 296        (dt - (t - dt)*254)/(255*dt);
 297        --------------------------------------------------------
 298        ax + b = 254*x/(255*dt) + (dt - (t - dt)*254)/(255*dt) =
 299        (254*x + dt - (t - dt)*254)/(255*dt) =
 300        ((x - (t - dt))*254 + dt)/(255*dt) =
 301        (((x - (t - dt))/dt)*254 + 1)/255 = (((x - low_time)/dt)*254 + 1)/255
 302      */
 303     for( y = 0; y < mhi_rows; y++ )
 304     {
 305         mhi_row.data.ptr = mhi->data.ptr + mhi->step*y;
 306         mask_row.data.ptr = mask->data.ptr + mask->step*y;
 307         orient_row.data.ptr = orient->data.ptr + orient->step*y;
 308
 309         for( x = 0; x < mhi_cols; x++ )
 310             if( mask_row.data.ptr[x] != 0 && mhi_row.data.fl[x] > delbound )
 311             {
 312                 /*
 313                    orient in 0..360, base_orient in 0..360
 314                    -> (rel_angle = orient - base_orient) in -360..360.
 315                    rel_angle is translated to -180..180
 316                  */
 317                 float weight = mhi_row.data.fl[x] * a + b;
 318                 float rel_angle = orient_row.data.fl[x] - fbase_orient;
 319
 320                 rel_angle += (rel_angle < -180 ? 360 : 0);
 321                 rel_angle += (rel_angle > 180 ? -360 : 0);
 322
 323                 if( fabs(rel_angle) < 45 )
 324                 {
 325                     shift_orient += weight * rel_angle;
 326                     shift_weight += weight;
 327                 }
 328             }
 329     }
 330
 331     // add the dominant orientation and the relative shift
 332     if( shift_weight == 0 )
 333         shift_weight = 0.01f;
 334
 335     fbase_orient += shift_orient / shift_weight;
 336     fbase_orient -= (fbase_orient < 360 ? 0 : 360);
 337     fbase_orient += (fbase_orient >= 0 ? 0 : 360);
 338
 339     return fbase_orient;
 340 }
 341
 342
 343 CV_IMPL CvSeq*
 344 cvSegmentMotion( const CvArr* mhiimg, CvArr* segmask, CvMemStorage* storage,
 345                  double timestamp, double seg_thresh )
 346 {
 347     CvSeq* components = 0;
 348     cv::Ptr<CvMat> mask8u;
 349
 350     CvMat  mhistub, *mhi = cvGetMat(mhiimg, &mhistub);
 351     CvMat  maskstub, *mask = cvGetMat(segmask, &maskstub);
 352     Cv32suf v, comp_idx;
 353     int stub_val, ts;
 354     int x, y;
 355
 356     if( !storage )
 357         CV_Error( CV_StsNullPtr, "NULL memory storage" );
 358
 359     mhi = cvGetMat( mhi, &mhistub );
 360     mask = cvGetMat( mask, &maskstub );
 361
 362     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( mask->type ) != CV_32FC1 )
 363         CV_Error( CV_BadDepth, "Both MHI and the destination mask" );
 364
 365     if( !CV_ARE_SIZES_EQ( mhi, mask ))
 366         CV_Error( CV_StsUnmatchedSizes, "" );
 367
 368     mask8u = cvCreateMat( mhi->rows + 2, mhi->cols + 2, CV_8UC1 );
 369     cvZero( mask8u );
 370     cvZero( mask );
 371     components = cvCreateSeq( CV_SEQ_KIND_GENERIC, sizeof(CvSeq),
 372                               sizeof(CvConnectedComp), storage );
 373
 374     v.f = (float)timestamp; ts = v.i;
 375     v.f = FLT_MAX*0.1f; stub_val = v.i;
 376     comp_idx.f = 1;
 377
 378     for( y = 0; y < mhi->rows; y++ )
 379     {
 380         int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);
 381         for( x = 0; x < mhi->cols; x++ )
 382         {
 383             if( mhi_row[x] == 0 )
 384                 mhi_row[x] = stub_val;
 385         }
 386     }
 387
 388     for( y = 0; y < mhi->rows; y++ )
 389     {
 390         int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);
 391         uchar* mask8u_row = mask8u->data.ptr + (y+1)*mask8u->step + 1;
 392
 393         for( x = 0; x < mhi->cols; x++ )
 394         {
 395             if( mhi_row[x] == ts && mask8u_row[x] == 0 )
 396             {
 397                 CvConnectedComp comp;
 398                 int x1, y1;
 399                 CvScalar _seg_thresh = cvRealScalar(seg_thresh);
 400                 CvPoint seed = cvPoint(x,y);
 401
 402                 cvFloodFill( mhi, seed, cvRealScalar(0), _seg_thresh, _seg_thresh,
 403                             &comp, CV_FLOODFILL_MASK_ONLY + 2*256 + 4, mask8u );
 404
 405                 for( y1 = 0; y1 < comp.rect.height; y1++ )
 406                 {
 407                     int* mask_row1 = (int*)(mask->data.ptr +
 408                                     (comp.rect.y + y1)*mask->step) + comp.rect.x;
 409                     uchar* mask8u_row1 = mask8u->data.ptr +
 410                                     (comp.rect.y + y1+1)*mask8u->step + comp.rect.x+1;
 411
 412                     for( x1 = 0; x1 < comp.rect.width; x1++ )
 413                     {
 414                         if( mask8u_row1[x1] > 1 )
 415                         {
 416                             mask8u_row1[x1] = 1;
 417                             mask_row1[x1] = comp_idx.i;
 418                         }
 419                     }
 420                 }
 421                 comp_idx.f++;
 422                 cvSeqPush( components, &comp );
 423             }
 424         }
 425     }
 426
 427     for( y = 0; y < mhi->rows; y++ )
 428     {
 429         int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);
 430         for( x = 0; x < mhi->cols; x++ )
 431         {
 432             if( mhi_row[x] == stub_val )
 433                 mhi_row[x] = 0;
 434         }
 435     }
 436
 437     return components;
 438 }
 439
 440
 441 void cv::updateMotionHistory( InputArray _silhouette, InputOutputArray _mhi,
 442                               double timestamp, double duration )
 443 {
 444     Mat silhouette = _silhouette.getMat();
 445     CvMat c_silhouette = silhouette, c_mhi = _mhi.getMat();
 446     cvUpdateMotionHistory( &c_silhouette, &c_mhi, timestamp, duration );
 447 }
 448
 449 void cv::calcMotionGradient( InputArray _mhi, OutputArray _mask,
 450                              OutputArray _orientation,
 451                              double delta1, double delta2,
 452                              int aperture_size )
 453 {
 454     Mat mhi = _mhi.getMat();
 455     _mask.create(mhi.size(), CV_8U);
 456     _orientation.create(mhi.size(), CV_32F);
 457     CvMat c_mhi = mhi, c_mask = _mask.getMat(), c_orientation = _orientation.getMat();
 458     cvCalcMotionGradient(&c_mhi, &c_mask, &c_orientation, delta1, delta2, aperture_size);
 459 }
 460
 461 double cv::calcGlobalOrientation( InputArray _orientation, InputArray _mask,
 462                                   InputArray _mhi, double timestamp,
 463                                   double duration )
 464 {
 465     Mat orientation = _orientation.getMat(), mask = _mask.getMat(), mhi = _mhi.getMat();
 466     CvMat c_orientation = orientation, c_mask = mask, c_mhi = mhi;
 467     return cvCalcGlobalOrientation(&c_orientation, &c_mask, &c_mhi, timestamp, duration);
 468 }
 469
 470 void cv::segmentMotion(InputArray _mhi, OutputArray _segmask,
 471                        vector<Rect>& boundingRects,
 472                        double timestamp, double segThresh)
 473 {
 474     Mat mhi = _mhi.getMat();
 475     _segmask.create(mhi.size(), CV_32F);
 476     CvMat c_mhi = mhi, c_segmask = _segmask.getMat();
 477     Ptr<CvMemStorage> storage = cvCreateMemStorage();
 478     Seq<CvConnectedComp> comps = cvSegmentMotion(&c_mhi, &c_segmask, storage, timestamp, segThresh);
 479     Seq<CvConnectedComp>::const_iterator it(comps);
 480     size_t i, ncomps = comps.size();
 481     boundingRects.resize(ncomps);
 482     for( i = 0; i < ncomps; i++, ++it)
 483         boundingRects[i] = (*it).rect;
 484 }
 485
 486 /* End of file. */