opencv/src/cv/cvmotempl.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                        Intel License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.\r
  14 // Third party copyrights are property of their respective owners.\r
  15 //\r
  16 // Redistribution and use in source and binary forms, with or without modification,\r
  17 // are permitted provided that the following conditions are met:\r
  18 //\r
  19 //   * Redistribution's of source code must retain the above copyright notice,\r
  20 //     this list of conditions and the following disclaimer.\r
  21 //\r
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  23 //     this list of conditions and the following disclaimer in the documentation\r
  24 //     and/or other materials provided with the distribution.\r
  25 //\r
  26 //   * The name of Intel Corporation may not be used to endorse or promote products\r
  27 //     derived from this software without specific prior written permission.\r
  28 //\r
  29 // This software is provided by the copyright holders and contributors "as is" and\r
  30 // any express or implied warranties, including, but not limited to, the implied\r
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  33 // indirect, incidental, special, exemplary, or consequential damages\r
  34 // (including, but not limited to, procurement of substitute goods or services;\r
  35 // loss of use, data, or profits; or business interruption) however caused\r
  36 // and on any theory of liability, whether in contract, strict liability,\r
  37 // or tort (including negligence or otherwise) arising in any way out of\r
  38 // the use of this software, even if advised of the possibility of such damage.\r
  39 //\r
  40 //M*/\r
  41 \r
  42 #include "_cv.h"\r
  43 \r
  44 \r
  45 /* motion templates */\r
  46 CV_IMPL void\r
  47 cvUpdateMotionHistory( const void* silhouette, void* mhimg,\r
  48                        double timestamp, double mhi_duration )\r
  49 {\r
  50     CvMat  silhstub, *silh = cvGetMat(silhouette, &silhstub);\r
  51     CvMat  mhistub, *mhi = cvGetMat(mhimg, &mhistub);\r
  52 \r
  53     if( !CV_IS_MASK_ARR( silh ))\r
  54         CV_Error( CV_StsBadMask, "" );\r
  55 \r
  56     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 )\r
  57         CV_Error( CV_StsUnsupportedFormat, "" );\r
  58 \r
  59     if( !CV_ARE_SIZES_EQ( mhi, silh ))\r
  60         CV_Error( CV_StsUnmatchedSizes, "" );\r
  61 \r
  62     CvSize size = cvGetMatSize( mhi );\r
  63 \r
  64     int mhi_step = mhi->step;\r
  65     int silh_step = silh->step;\r
  66 \r
  67     if( CV_IS_MAT_CONT( mhi->type & silh->type ))\r
  68     {\r
  69         size.width *= size.height;\r
  70         mhi_step = silh_step = CV_STUB_STEP;\r
  71         size.height = 1;\r
  72     }\r
  73 \r
  74     float ts = (float)timestamp;\r
  75     float delbound = (float)(timestamp - mhi_duration);\r
  76     int x, y;\r
  77 #if CV_SSE2\r
  78     volatile bool useSIMD = cv::checkHardwareSupport(CV_CPU_SSE2);\r
  79 #endif\r
  80     \r
  81     for( y = 0; y < size.height; y++ )\r
  82     {\r
  83         const uchar* silhData = silh->data.ptr + silh->step*y;\r
  84         float* mhiData = (float*)(mhi->data.ptr + mhi->step*y);\r
  85         x = 0;\r
  86         \r
  87 #if CV_SSE2\r
  88         if( useSIMD )\r
  89         {\r
  90             __m128 ts4 = _mm_set1_ps(ts), db4 = _mm_set1_ps(delbound);\r
  91             for( ; x <= size.width - 8; x += 8 )\r
  92             {\r
  93                 __m128i z = _mm_setzero_si128();\r
  94                 __m128i s = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(silhData + x)), z);\r
  95                 __m128 s0 = _mm_cvtepi32_ps(_mm_unpacklo_epi16(s, z)), s1 = _mm_cvtepi32_ps(_mm_unpackhi_epi16(s, z));\r
  96                 __m128 v0 = _mm_loadu_ps(mhiData + x), v1 = _mm_loadu_ps(mhiData + x + 4);\r
  97                 __m128 fz = _mm_setzero_ps();\r
  98                 \r
  99                 v0 = _mm_and_ps(v0, _mm_cmpge_ps(v0, db4));\r
 100                 v1 = _mm_and_ps(v1, _mm_cmpge_ps(v1, db4));\r
 101 \r
 102                 __m128 m0 = _mm_and_ps(_mm_xor_ps(v0, ts4), _mm_cmpneq_ps(s0, fz));\r
 103                 __m128 m1 = _mm_and_ps(_mm_xor_ps(v1, ts4), _mm_cmpneq_ps(s1, fz));\r
 104                 \r
 105                 v0 = _mm_xor_ps(v0, m0);\r
 106                 v1 = _mm_xor_ps(v1, m1);\r
 107                 \r
 108                 _mm_storeu_ps(mhiData + x, v0);\r
 109                 _mm_storeu_ps(mhiData + x + 4, v1);\r
 110             }\r
 111         }\r
 112 #endif\r
 113         \r
 114         for( ; x < size.width; x++ )\r
 115         {\r
 116             float val = mhiData[x];\r
 117             val = silhData[x] ? ts : val < delbound ? 0 : val;\r
 118             mhiData[x] = val;\r
 119         }\r
 120     }\r
 121 }\r
 122 \r
 123 \r
 124 CV_IMPL void\r
 125 cvCalcMotionGradient( const CvArr* mhiimg, CvArr* maskimg,\r
 126                       CvArr* orientation,\r
 127                       double delta1, double delta2,\r
 128                       int aperture_size )\r
 129 {\r
 130     cv::Ptr<CvMat> dX_min, dY_max;\r
 131 \r
 132     CvMat  mhistub, *mhi = cvGetMat(mhiimg, &mhistub);\r
 133     CvMat  maskstub, *mask = cvGetMat(maskimg, &maskstub);\r
 134     CvMat  orientstub, *orient = cvGetMat(orientation, &orientstub);\r
 135     CvMat  dX_min_row, dY_max_row, orient_row, mask_row;\r
 136     CvSize size;\r
 137     int x, y;\r
 138 \r
 139     float  gradient_epsilon = 1e-4f * aperture_size * aperture_size;\r
 140     float  min_delta, max_delta;\r
 141 \r
 142     if( !CV_IS_MASK_ARR( mask ))\r
 143         CV_Error( CV_StsBadMask, "" );\r
 144 \r
 145     if( aperture_size < 3 || aperture_size > 7 || (aperture_size & 1) == 0 )\r
 146         CV_Error( CV_StsOutOfRange, "aperture_size must be 3, 5 or 7" );\r
 147 \r
 148     if( delta1 <= 0 || delta2 <= 0 )\r
 149         CV_Error( CV_StsOutOfRange, "both delta's must be positive" );\r
 150 \r
 151     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( orient->type ) != CV_32FC1 )\r
 152         CV_Error( CV_StsUnsupportedFormat,\r
 153         "MHI and orientation must be single-channel floating-point images" );\r
 154 \r
 155     if( !CV_ARE_SIZES_EQ( mhi, mask ) || !CV_ARE_SIZES_EQ( orient, mhi ))\r
 156         CV_Error( CV_StsUnmatchedSizes, "" );\r
 157 \r
 158     if( orient->data.ptr == mhi->data.ptr )\r
 159         CV_Error( CV_StsInplaceNotSupported, "orientation image must be different from MHI" );\r
 160 \r
 161     if( delta1 > delta2 )\r
 162     {\r
 163         double t;\r
 164         CV_SWAP( delta1, delta2, t );\r
 165     }\r
 166 \r
 167     size = cvGetMatSize( mhi );\r
 168     min_delta = (float)delta1;\r
 169     max_delta = (float)delta2;\r
 170     dX_min = cvCreateMat( mhi->rows, mhi->cols, CV_32F );\r
 171     dY_max = cvCreateMat( mhi->rows, mhi->cols, CV_32F );\r
 172 \r
 173     // calc Dx and Dy\r
 174     cvSobel( mhi, dX_min, 1, 0, aperture_size );\r
 175     cvSobel( mhi, dY_max, 0, 1, aperture_size );\r
 176     cvGetRow( dX_min, &dX_min_row, 0 );\r
 177     cvGetRow( dY_max, &dY_max_row, 0 );\r
 178     cvGetRow( orient, &orient_row, 0 );\r
 179     cvGetRow( mask, &mask_row, 0 );\r
 180 \r
 181     // calc gradient\r
 182     for( y = 0; y < size.height; y++ )\r
 183     {\r
 184         dX_min_row.data.ptr = dX_min->data.ptr + y*dX_min->step;\r
 185         dY_max_row.data.ptr = dY_max->data.ptr + y*dY_max->step;\r
 186         orient_row.data.ptr = orient->data.ptr + y*orient->step;\r
 187         mask_row.data.ptr = mask->data.ptr + y*mask->step;\r
 188         cvCartToPolar( &dX_min_row, &dY_max_row, 0, &orient_row, 1 );\r
 189 \r
 190         // make orientation zero where the gradient is very small\r
 191         for( x = 0; x < size.width; x++ )\r
 192         {\r
 193             float dY = dY_max_row.data.fl[x];\r
 194             float dX = dX_min_row.data.fl[x];\r
 195 \r
 196             if( fabs(dX) < gradient_epsilon && fabs(dY) < gradient_epsilon )\r
 197             {\r
 198                 mask_row.data.ptr[x] = 0;\r
 199                 orient_row.data.i[x] = 0;\r
 200             }\r
 201             else\r
 202                 mask_row.data.ptr[x] = 1;\r
 203         }\r
 204     }\r
 205 \r
 206     cvErode( mhi, dX_min, 0, (aperture_size-1)/2);\r
 207     cvDilate( mhi, dY_max, 0, (aperture_size-1)/2);\r
 208 \r
 209     // mask off pixels which have little motion difference in their neighborhood\r
 210     for( y = 0; y < size.height; y++ )\r
 211     {\r
 212         dX_min_row.data.ptr = dX_min->data.ptr + y*dX_min->step;\r
 213         dY_max_row.data.ptr = dY_max->data.ptr + y*dY_max->step;\r
 214         mask_row.data.ptr = mask->data.ptr + y*mask->step;\r
 215         orient_row.data.ptr = orient->data.ptr + y*orient->step;\r
 216         \r
 217         for( x = 0; x < size.width; x++ )\r
 218         {\r
 219             float d0 = dY_max_row.data.fl[x] - dX_min_row.data.fl[x];\r
 220 \r
 221             if( mask_row.data.ptr[x] == 0 || d0 < min_delta || max_delta < d0 )\r
 222             {\r
 223                 mask_row.data.ptr[x] = 0;\r
 224                 orient_row.data.i[x] = 0;\r
 225             }\r
 226         }\r
 227     }\r
 228 }\r
 229 \r
 230 \r
 231 CV_IMPL double\r
 232 cvCalcGlobalOrientation( const void* orientation, const void* maskimg, const void* mhiimg,\r
 233                          double curr_mhi_timestamp, double mhi_duration )\r
 234 {\r
 235     int hist_size = 12;\r
 236     cv::Ptr<CvHistogram> hist;\r
 237 \r
 238     CvMat  mhistub, *mhi = cvGetMat(mhiimg, &mhistub);\r
 239     CvMat  maskstub, *mask = cvGetMat(maskimg, &maskstub);\r
 240     CvMat  orientstub, *orient = cvGetMat(orientation, &orientstub);\r
 241     void*  _orient;\r
 242     float _ranges[] = { 0, 360 };\r
 243     float* ranges = _ranges;\r
 244     int base_orient;\r
 245     double shift_orient = 0, shift_weight = 0, fbase_orient;\r
 246     double a, b;\r
 247     float delbound;\r
 248     CvMat mhi_row, mask_row, orient_row;\r
 249     int x, y, mhi_rows, mhi_cols;\r
 250 \r
 251     if( !CV_IS_MASK_ARR( mask ))\r
 252         CV_Error( CV_StsBadMask, "" );\r
 253 \r
 254     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( orient->type ) != CV_32FC1 )\r
 255         CV_Error( CV_StsUnsupportedFormat,\r
 256         "MHI and orientation must be single-channel floating-point images" );\r
 257 \r
 258     if( !CV_ARE_SIZES_EQ( mhi, mask ) || !CV_ARE_SIZES_EQ( orient, mhi ))\r
 259         CV_Error( CV_StsUnmatchedSizes, "" );\r
 260 \r
 261     if( mhi_duration <= 0 )\r
 262         CV_Error( CV_StsOutOfRange, "MHI duration must be positive" );\r
 263 \r
 264     if( orient->data.ptr == mhi->data.ptr )\r
 265         CV_Error( CV_StsInplaceNotSupported, "orientation image must be different from MHI" );\r
 266 \r
 267     // calculate histogram of different orientation values\r
 268     hist = cvCreateHist( 1, &hist_size, CV_HIST_ARRAY, &ranges );\r
 269     _orient = orient;\r
 270     cvCalcArrHist( &_orient, hist, 0, mask );\r
 271 \r
 272     // find the maximum index (the dominant orientation)\r
 273     cvGetMinMaxHistValue( hist, 0, 0, 0, &base_orient );\r
 274     base_orient *= 360/hist_size;\r
 275 \r
 276     // override timestamp with the maximum value in MHI\r
 277     cvMinMaxLoc( mhi, 0, &curr_mhi_timestamp, 0, 0, mask );\r
 278 \r
 279     // find the shift relative to the dominant orientation as weighted sum of relative angles\r
 280     a = 254. / 255. / mhi_duration;\r
 281     b = 1. - curr_mhi_timestamp * a;\r
 282     fbase_orient = base_orient;\r
 283     delbound = (float)(curr_mhi_timestamp - mhi_duration);\r
 284     mhi_rows = mhi->rows;\r
 285     mhi_cols = mhi->cols;\r
 286 \r
 287     if( CV_IS_MAT_CONT( mhi->type & mask->type & orient->type ))\r
 288     {\r
 289         mhi_cols *= mhi_rows;\r
 290         mhi_rows = 1;\r
 291     }\r
 292 \r
 293     cvGetRow( mhi, &mhi_row, 0 );\r
 294     cvGetRow( mask, &mask_row, 0 );\r
 295     cvGetRow( orient, &orient_row, 0 );\r
 296 \r
 297     /*\r
 298        a = 254/(255*dt)\r
 299        b = 1 - t*a = 1 - 254*t/(255*dur) =\r
 300        (255*dt - 254*t)/(255*dt) =\r
 301        (dt - (t - dt)*254)/(255*dt);\r
 302        --------------------------------------------------------\r
 303        ax + b = 254*x/(255*dt) + (dt - (t - dt)*254)/(255*dt) =\r
 304        (254*x + dt - (t - dt)*254)/(255*dt) =\r
 305        ((x - (t - dt))*254 + dt)/(255*dt) =\r
 306        (((x - (t - dt))/dt)*254 + 1)/255 = (((x - low_time)/dt)*254 + 1)/255\r
 307      */\r
 308     for( y = 0; y < mhi_rows; y++ )\r
 309     {\r
 310         mhi_row.data.ptr = mhi->data.ptr + mhi->step*y;\r
 311         mask_row.data.ptr = mask->data.ptr + mask->step*y;\r
 312         orient_row.data.ptr = orient->data.ptr + orient->step*y;\r
 313 \r
 314         for( x = 0; x < mhi_cols; x++ )\r
 315             if( mask_row.data.ptr[x] != 0 && mhi_row.data.fl[x] > delbound )\r
 316             {\r
 317                 /*\r
 318                    orient in 0..360, base_orient in 0..360\r
 319                    -> (rel_angle = orient - base_orient) in -360..360.\r
 320                    rel_angle is translated to -180..180\r
 321                  */\r
 322                 double weight = mhi_row.data.fl[x] * a + b;\r
 323                 int rel_angle = cvRound( orient_row.data.fl[x] - fbase_orient );\r
 324 \r
 325                 rel_angle += (rel_angle < -180 ? 360 : 0);\r
 326                 rel_angle += (rel_angle > 180 ? -360 : 0);\r
 327 \r
 328                 if( abs(rel_angle) < 90 )\r
 329                 {\r
 330                     shift_orient += weight * rel_angle;\r
 331                     shift_weight += weight;\r
 332                 }\r
 333             }\r
 334     }\r
 335 \r
 336     // add the dominant orientation and the relative shift\r
 337     if( shift_weight == 0 )\r
 338         shift_weight = 0.01;\r
 339 \r
 340     base_orient = base_orient + cvRound( shift_orient / shift_weight );\r
 341     base_orient -= (base_orient < 360 ? 0 : 360);\r
 342     base_orient += (base_orient >= 0 ? 0 : 360);\r
 343 \r
 344     return base_orient;\r
 345 }\r
 346 \r
 347 \r
 348 CV_IMPL CvSeq*\r
 349 cvSegmentMotion( const CvArr* mhiimg, CvArr* segmask, CvMemStorage* storage,\r
 350                  double timestamp, double seg_thresh )\r
 351 {\r
 352     CvSeq* components = 0;\r
 353     cv::Ptr<CvMat> mask8u;\r
 354 \r
 355     CvMat  mhistub, *mhi = cvGetMat(mhiimg, &mhistub);\r
 356     CvMat  maskstub, *mask = cvGetMat(segmask, &maskstub);\r
 357     Cv32suf v, comp_idx;\r
 358     int stub_val, ts;\r
 359     int x, y;\r
 360 \r
 361     if( !storage )\r
 362         CV_Error( CV_StsNullPtr, "NULL memory storage" );\r
 363 \r
 364     mhi = cvGetMat( mhi, &mhistub );\r
 365     mask = cvGetMat( mask, &maskstub );\r
 366 \r
 367     if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( mask->type ) != CV_32FC1 )\r
 368         CV_Error( CV_BadDepth, "Both MHI and the destination mask" );\r
 369 \r
 370     if( !CV_ARE_SIZES_EQ( mhi, mask ))\r
 371         CV_Error( CV_StsUnmatchedSizes, "" );\r
 372 \r
 373     mask8u = cvCreateMat( mhi->rows + 2, mhi->cols + 2, CV_8UC1 );\r
 374     cvZero( mask8u );\r
 375     cvZero( mask );\r
 376     components = cvCreateSeq( CV_SEQ_KIND_GENERIC, sizeof(CvSeq),\r
 377                               sizeof(CvConnectedComp), storage );\r
 378     \r
 379     v.f = (float)timestamp; ts = v.i;\r
 380     v.f = FLT_MAX*0.1f; stub_val = v.i;\r
 381     comp_idx.f = 1;\r
 382 \r
 383     for( y = 0; y < mhi->rows; y++ )\r
 384     {\r
 385         int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);\r
 386         for( x = 0; x < mhi->cols; x++ )\r
 387         {\r
 388             if( mhi_row[x] == 0 )\r
 389                 mhi_row[x] = stub_val;\r
 390         }\r
 391     }\r
 392 \r
 393     for( y = 0; y < mhi->rows; y++ )\r
 394     {\r
 395         int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);\r
 396         uchar* mask8u_row = mask8u->data.ptr + (y+1)*mask8u->step + 1;\r
 397 \r
 398         for( x = 0; x < mhi->cols; x++ )\r
 399         {\r
 400             if( mhi_row[x] == ts && mask8u_row[x] == 0 )\r
 401             {\r
 402                 CvConnectedComp comp;\r
 403                 int x1, y1;\r
 404                 CvScalar _seg_thresh = cvRealScalar(seg_thresh);\r
 405                 CvPoint seed = cvPoint(x,y);\r
 406 \r
 407                 cvFloodFill( mhi, seed, cvRealScalar(0), _seg_thresh, _seg_thresh,\r
 408                             &comp, CV_FLOODFILL_MASK_ONLY + 2*256 + 4, mask8u );\r
 409 \r
 410                 for( y1 = 0; y1 < comp.rect.height; y1++ )\r
 411                 {\r
 412                     int* mask_row1 = (int*)(mask->data.ptr +\r
 413                                     (comp.rect.y + y1)*mask->step) + comp.rect.x;\r
 414                     uchar* mask8u_row1 = mask8u->data.ptr +\r
 415                                     (comp.rect.y + y1+1)*mask8u->step + comp.rect.x+1;\r
 416 \r
 417                     for( x1 = 0; x1 < comp.rect.width; x1++ )\r
 418                     {\r
 419                         if( mask8u_row1[x1] > 1 )\r
 420                         {\r
 421                             mask8u_row1[x1] = 1;\r
 422                             mask_row1[x1] = comp_idx.i;\r
 423                         }\r
 424                     }\r
 425                 }\r
 426                 comp_idx.f++;\r
 427                 cvSeqPush( components, &comp );\r
 428             }\r
 429         }\r
 430     }\r
 431 \r
 432     for( y = 0; y < mhi->rows; y++ )\r
 433     {\r
 434         int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);\r
 435         for( x = 0; x < mhi->cols; x++ )\r
 436         {\r
 437             if( mhi_row[x] == stub_val )\r
 438                 mhi_row[x] = 0;\r
 439         }\r
 440     }\r
 441 \r
 442     return components;\r
 443 }\r
 444 \r
 445 \r
 446 void cv::updateMotionHistory( const Mat& silhouette, Mat& mhi,\r
 447                               double timestamp, double duration )\r
 448 {\r
 449     CvMat _silhouette = silhouette, _mhi = mhi;\r
 450     cvUpdateMotionHistory( &_silhouette, &_mhi, timestamp, duration );\r
 451 }\r
 452 \r
 453 void cv::calcMotionGradient( const Mat& mhi, Mat& mask,\r
 454                              Mat& orientation,\r
 455                              double delta1, double delta2,\r
 456                              int aperture_size )\r
 457 {\r
 458     mask.create(mhi.size(), CV_8U);\r
 459     orientation.create(mhi.size(), CV_32F);\r
 460     CvMat _mhi = mhi, _mask = mask, _orientation = orientation;\r
 461     cvCalcMotionGradient(&_mhi, &_mask, &_orientation, delta1, delta2, aperture_size);\r
 462 }\r
 463 \r
 464 double cv::calcGlobalOrientation( const Mat& orientation, const Mat& mask,\r
 465                                   const Mat& mhi, double timestamp,\r
 466                                   double duration )\r
 467 {\r
 468     CvMat _orientation = orientation, _mask = mask, _mhi = mhi;\r
 469     return cvCalcGlobalOrientation(&_orientation, &_mask, &_mhi, timestamp, duration);\r
 470 }\r
 471 \r
 472 /* End of file. */\r