git.maemo.org Git - opencv/blob - cvaux/src/cvfindhandregion.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 #include "_cvaux.h"
  42
  43 #define _CV_NORM_L2(a) (float)(icvSqrt32f(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]))
  44 #define _CV_NORM_L22(a) (float)(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])
  45
  46 /****************************************************************************************\
  47
  48    find region where hand is   (for gesture recognition)
  49    flag = 0 (use left bucket)  flag = 1 (use right bucket)
  50
  51 \****************************************************************************************/
  52
  53 static CvStatus CV_STDCALL
  54 icvFindHandRegion( CvPoint3D32f * points, int count,
  55                    CvSeq * indexs,
  56                    float *line, CvSize2D32f size, int flag,
  57                    CvPoint3D32f * center,
  58                    CvMemStorage * storage, CvSeq ** numbers )
  59 {
  60
  61 /*    IppmVect32f sub, cros;   */
  62     float *sub, *cros;
  63     CvSeqWriter writer;
  64     CvSeqReader reader;
  65
  66     CvStatus status;
  67     int nbins = 20, i, l, i_point, left, right;
  68     int *bin_counts = 0;        //  pointer to the point's counter in the bickets
  69     int low_count;              //  low threshold
  70
  71     CvPoint *tmp_number = 0, *pt;
  72     float value, vmin, vmax, vl, bsize, vc;
  73     float hand_length, hand_length2, hand_left, hand_right;
  74     float threshold, threshold2;
  75     float *vv = 0;
  76     float a[3];
  77
  78     status = CV_OK;
  79
  80     hand_length = size.width;
  81     hand_length2 = hand_length / 2;
  82
  83     threshold = (float) (size.height * 3 / 5.);
  84     threshold2 = threshold * threshold;
  85
  86 /*    low_count = count/nbins;     */
  87     low_count = (int) (count / 60.);
  88
  89     assert( points != NULL && line != NULL );
  90     if( points == NULL || line == NULL )
  91         return CV_NULLPTR_ERR;
  92
  93     assert( count > 5 );
  94     if( count < 5 )
  95         return CV_BADFLAG_ERR;
  96
  97     assert( flag == 0 || flag == 1 );
  98     if( flag != 0 && flag != 1 )
  99         return CV_BADFLAG_ERR;
 100
 101 /*  create vectors         */
 102     sub = icvCreateVector_32f( 3 );
 103     cros = icvCreateVector_32f( 3 );
 104     if( sub == NULL || cros == NULL )
 105         return CV_OUTOFMEM_ERR;
 106
 107 /*  alloc memory for the point's projections on the line    */
 108     vv = (float *) cvAlloc( count * sizeof( float ));
 109
 110     if( vv == NULL )
 111         return CV_OUTOFMEM_ERR;
 112
 113 /*  alloc memory for the point's counter in the bickets     */
 114     bin_counts = (int *) cvAlloc( nbins * sizeof( int ));
 115
 116     if( bin_counts == NULL )
 117     {
 118         status = CV_OUTOFMEM_ERR;
 119         goto M_END;
 120     }
 121     memset( bin_counts, 0, nbins * sizeof( int ));
 122
 123     cvStartReadSeq( indexs, &reader, 0 );
 124
 125 /*  alloc memory for the temporale point's numbers      */
 126     tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint ));
 127     if( tmp_number == NULL )
 128     {
 129         status = CV_OUTOFMEM_ERR;
 130         goto M_END;
 131     }
 132
 133 /*  find min and max point's projection on the line     */
 134     vmin = 1000;
 135     vmax = -1000;
 136     i_point = 0;
 137     for( i = 0; i < count; i++ )
 138     {
 139 /*
 140         icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3);
 141
 142         icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros);
 143 */
 144
 145         sub[0] = points[i].x - line[3];
 146         sub[1] = points[i].y - line[4];
 147         sub[2] = points[i].z - line[5];
 148         a[0] = sub[0] * line[1] - sub[1] * line[0];
 149         a[1] = sub[1] * line[2] - sub[2] * line[1];
 150         a[2] = sub[2] * line[0] - sub[0] * line[2];
 151
 152 /*      if(IPPI_NORM_L22 ( cros ) < threshold2)    */
 153         if( _CV_NORM_L22( a ) < threshold2 )
 154         {
 155             value = (float)icvDotProduct_32f( sub, &line[0], 3 );
 156             if( value > vmax )
 157                 vmax = value;
 158             if( value < vmin )
 159                 vmin = value;
 160
 161             vv[i_point] = value;
 162
 163             pt = (CvPoint*)cvGetSeqElem( indexs, i );
 164             tmp_number[i_point] = *pt;
 165             i_point++;
 166         }
 167     }
 168
 169 /*  compute the length of one bucket             */
 170     vl = vmax - vmin;
 171     bsize = vl / nbins;
 172
 173 /*  compute the number of points in each bucket   */
 174     for( i = 0; i < i_point; i++ )
 175     {
 176         l = cvRound( (vv[i] - vmin) / bsize );
 177         bin_counts[l]++;
 178     }
 179
 180     *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage );
 181     assert( numbers != 0 );
 182     if( numbers == NULL )
 183     {
 184         status = CV_OUTOFMEM_ERR;
 185         goto M_END;
 186     }
 187
 188     cvStartAppendToSeq( *numbers, &writer );
 189
 190     if( flag == 0 )
 191     {
 192 /*  find the leftmost bucket           */
 193         for( l = 0; l < nbins; l++ )
 194         {
 195             if( bin_counts[l] > low_count )
 196                 break;
 197         }
 198         left = l;
 199
 200 /*  compute center point of the left hand     */
 201         hand_left = vmin + left * bsize;
 202         vc = hand_left + hand_length2;
 203         hand_right = hand_left + hand_length;
 204     }
 205     else
 206     {
 207 /*  find the rightmost bucket                */
 208         for( l = nbins - 1; l >= 0; l-- )
 209         {
 210             if( bin_counts[l] > low_count )
 211                 break;
 212         }
 213         right = l;
 214
 215 /*  compute center point of the right hand    */
 216         hand_right = vmax - (nbins - right - 1) * bsize;
 217         vc = hand_right - hand_length2;
 218         hand_left = hand_right - hand_length;
 219     }
 220
 221     icvScaleVector_32f( &line[0], sub, 3, vc );
 222     icvAddVector_32f( &line[3], sub, (float *) center, 3 );
 223
 224 /*  select hand's points and calculate mean value     */
 225
 226     //ss.x = ss.y = ss.z = 0;
 227     for( l = 0; l < i_point; l++ )
 228     {
 229         if( vv[l] >= hand_left && vv[l] <= hand_right )
 230         {
 231             CV_WRITE_SEQ_ELEM( tmp_number[l], writer );
 232
 233         }
 234     }
 235
 236     cvEndWriteSeq( &writer );
 237
 238   M_END:
 239     if( tmp_number != NULL )
 240         cvFree( &tmp_number );
 241     if( bin_counts != NULL )
 242         cvFree( &bin_counts );
 243     if( vv != NULL )
 244         cvFree( &vv );
 245     if( sub != NULL ) icvDeleteVector (sub);
 246     if( cros != NULL ) icvDeleteVector (cros);
 247
 248     return status;
 249
 250 }
 251
 252
 253 //////////////////////////////////////////////////////////////////////////////////////////
 254 //////////////////////////////////////////////////////////////////////////////////////////
 255 //////////////////////////////////////////////////////////////////////////////////////////
 256
 257
 258 #define _CV_NORM_L31(a) (float)(icvSqrt32f(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]))
 259 #define _CV_NORM_L32(a) (float)(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])
 260
 261 /****************************************************************************************\
 262
 263    find region where hand is   (for gesture recognition)
 264    flag = 0 (use left bucket)  flag = 1 (use right bucket)
 265
 266 \****************************************************************************************/
 267
 268 static CvStatus CV_STDCALL
 269 icvFindHandRegionA( CvPoint3D32f * points, int count,
 270                     CvSeq * indexs,
 271                     float *line, CvSize2D32f size, int jc,
 272                     CvPoint3D32f * center,
 273                     CvMemStorage * storage, CvSeq ** numbers )
 274 {
 275
 276 /*    IppmVect32f sub, cros;   */
 277     float *sub, *cros;
 278     float eps = (float) 0.01;
 279     CvSeqWriter writer;
 280     CvSeqReader reader;
 281
 282     CvStatus status;
 283     float gor[3] = { 1, 0, 0 };
 284     float ver[3] = { 0, 1, 0 };
 285
 286     int nbins = 20, i, l, i_point, left, right, jmin, jmax, jl;
 287     int j_left, j_right;
 288     int *bin_counts = 0;        //  pointer to the point's counter in the bickets
 289
 290 //    int *bin_countsj = 0;   //  pointer to the index's counter in the bickets
 291     int low_count;              //  low threshold
 292
 293     CvPoint *tmp_number = 0, *pt;
 294     float value, vmin, vmax, vl, bsize, bsizej, vc, vcl, vcr;
 295     double v_ver, v_gor;
 296     float hand_length, hand_length2, hand_left, hand_right;
 297     float threshold, threshold2;
 298     float *vv = 0;
 299     float a[3];
 300     char log;
 301
 302     status = CV_OK;
 303
 304     hand_length = size.width;
 305     hand_length2 = hand_length / 2;
 306
 307     threshold = (float) (size.height * 3 / 5.);
 308     threshold2 = threshold * threshold;
 309
 310 /*    low_count = count/nbins;     */
 311     low_count = (int) (count / 60.);
 312
 313     assert( points != NULL && line != NULL );
 314     if( points == NULL || line == NULL )
 315         return CV_NULLPTR_ERR;
 316
 317     assert( count > 5 );
 318     if( count < 5 )
 319         return CV_BADFLAG_ERR;
 320
 321 /*  create vectors         */
 322     sub = icvCreateVector_32f( 3 );
 323     cros = icvCreateVector_32f( 3 );
 324     if( sub == NULL || cros == NULL )
 325         return CV_OUTOFMEM_ERR;
 326
 327 /*  alloc memory for the point's projections on the line    */
 328     vv = (float *) cvAlloc( count * sizeof( float ));
 329
 330     if( vv == NULL )
 331         return CV_OUTOFMEM_ERR;
 332
 333 /*  alloc memory for the point's counter in the bickets     */
 334     bin_counts = (int *) cvAlloc( nbins * sizeof( int ));
 335
 336     if( bin_counts == NULL )
 337     {
 338         status = CV_OUTOFMEM_ERR;
 339         goto M_END;
 340     }
 341     memset( bin_counts, 0, nbins * sizeof( int ));
 342
 343 /*  alloc memory for the point's counter in the bickets     */
 344 //    bin_countsj = (int*) icvAlloc(nbins*sizeof(int));
 345 //    if(bin_countsj == NULL) {status = CV_OUTOFMEM_ERR; goto M_END;}
 346 //    memset(bin_countsj,0,nbins*sizeof(int));
 347
 348     cvStartReadSeq( indexs, &reader, 0 );
 349
 350 /*  alloc memory for the temporale point's numbers      */
 351     tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint ));
 352     if( tmp_number == NULL )
 353     {
 354         status = CV_OUTOFMEM_ERR;
 355         goto M_END;
 356     }
 357
 358 /*  find min and max point's projection on the line     */
 359     vmin = 1000;
 360     vmax = -1000;
 361     jmin = 1000;
 362     jmax = -1000;
 363     i_point = 0;
 364     for( i = 0; i < count; i++ )
 365     {
 366 /*
 367         icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3);
 368
 369         icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros);
 370 */
 371
 372         sub[0] = points[i].x - line[3];
 373         sub[1] = points[i].y - line[4];
 374         sub[2] = points[i].z - line[5];
 375
 376 //      if(fabs(sub[0])<eps||fabs(sub[1])<eps||fabs(sub[2])<eps) continue;
 377
 378         a[0] = sub[0] * line[1] - sub[1] * line[0];
 379         a[1] = sub[1] * line[2] - sub[2] * line[1];
 380         a[2] = sub[2] * line[0] - sub[0] * line[2];
 381
 382         v_gor = icvDotProduct_32f( gor, &line[0], 3 );
 383         v_ver = icvDotProduct_32f( ver, &line[0], 3 );
 384
 385         if( v_ver > v_gor )
 386             log = true;
 387         else
 388             log = false;
 389
 390
 391 /*      if(IPPI_NORM_L22 ( cros ) < threshold2)    */
 392 /*
 393         if(fabs(a[0])<eps && fabs(a[1])<eps && fabs(a[2])<eps)
 394         {
 395             icvDotProduct_32f( sub, &line[0], 3, &value);
 396             if(value > vmax) vmax = value;
 397             if(value < vmin) vmin = value;
 398
 399             vv[i_point] = value;
 400
 401             pt = (CvPoint* )icvGetSeqElem ( indexs, i, 0);
 402
 403             if(pt->x > jmax) jmax = pt->x;
 404             if(pt->x < jmin) jmin = pt->x;
 405
 406             tmp_number[i_point] = *pt;
 407             i_point++;
 408         }
 409         else
 410 */
 411         {
 412             if( _CV_NORM_L32( a ) < threshold2 )
 413             {
 414                 value = (float)icvDotProduct_32f( sub, &line[0], 3 );
 415                 if( value > vmax )
 416                     vmax = value;
 417                 if( value < vmin )
 418                     vmin = value;
 419
 420                 vv[i_point] = value;
 421
 422                 pt = (CvPoint*)cvGetSeqElem( indexs, i );
 423
 424                 if( !log )
 425                 {
 426                     if( pt->x > jmax )
 427                         jmax = pt->x;
 428                     if( pt->x < jmin )
 429                         jmin = pt->x;
 430                 }
 431                 else
 432                 {
 433                     if( pt->y > jmax )
 434                         jmax = pt->y;
 435                     if( pt->y < jmin )
 436                         jmin = pt->y;
 437                 }
 438
 439
 440                 tmp_number[i_point] = *pt;
 441                 i_point++;
 442             }
 443         }
 444     }
 445
 446 /*  compute the length of one bucket along the line        */
 447     vl = vmax - vmin;
 448
 449 /*  examining on the arm's existence  */
 450     if( vl < eps )
 451     {
 452         *numbers = NULL;
 453         status = CV_OK;
 454         goto M_END;
 455     }
 456
 457     bsize = vl / nbins;
 458
 459 /*  compute the number of points in each bucket along the line  */
 460     for( i = 0; i < i_point; i++ )
 461     {
 462         l = cvRound( (vv[i] - vmin) / bsize );
 463         bin_counts[l]++;
 464     }
 465
 466     /*  compute the length of one bucket along the X axe        */
 467     jl = jmax - jmin;
 468     if( jl <= 1 )
 469     {
 470         *numbers = NULL;
 471         status = CV_OK;
 472         goto M_END;
 473     }
 474
 475     bsizej = (float) (jl / (nbins + 0.));
 476
 477 /*  compute the number of points in each bucket along the X axe */
 478 //    for(i=0;i<i_point;i++)
 479 //    {
 480 //        l = cvRound((tmp_number[i].x - jmin)/bsizej);
 481 //        bin_countsj[l]++;
 482 //    }
 483
 484
 485     left = right = -1;
 486
 487 /*  find the leftmost and the rightmost buckets           */
 488     for( l = 0; l < nbins; l++ )
 489     {
 490         if( bin_counts[l] > low_count && left == -1 )
 491             left = l;
 492         else if( bin_counts[l] > low_count && left >= 0 )
 493             right = l;
 494
 495     }
 496
 497 /*  compute center point of the left hand     */
 498     if( left == -1 && right == -1 )
 499     {
 500         *numbers = NULL;
 501         status = CV_OK;
 502         goto M_END;
 503     }
 504
 505     hand_left = vmin + left * bsize;
 506     j_left = (int) (jmin + left * bsizej);
 507
 508     vcl = hand_left + hand_length2;
 509
 510 /*  compute center point of the right hand    */
 511     hand_right = vmax - (nbins - right - 1) * bsize;
 512     vcr = hand_right - hand_length2;
 513
 514     j_right = (int) (jmax - (nbins - right - 1) * bsizej);
 515
 516     j_left = abs( j_left - jc );
 517     j_right = abs( j_right - jc );
 518
 519     if( j_left <= j_right )
 520     {
 521         hand_right = hand_left + hand_length;
 522         vc = vcl;
 523     }
 524     else
 525     {
 526         hand_left = hand_right - hand_length;
 527         vc = vcr;
 528     }
 529
 530     icvScaleVector_32f( &line[0], sub, 3, vc );
 531     icvAddVector_32f( &line[3], sub, (float *) center, 3 );
 532
 533 /*  select hand's points and calculate mean value     */
 534     *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage );
 535     assert( *numbers != 0 );
 536     if( *numbers == NULL )
 537     {
 538         status = CV_OUTOFMEM_ERR;
 539         goto M_END;
 540     }
 541
 542     cvStartAppendToSeq( *numbers, &writer );
 543
 544     for( l = 0; l < i_point; l++ )
 545     {
 546         if( vv[l] >= hand_left && vv[l] <= hand_right )
 547         {
 548             CV_WRITE_SEQ_ELEM( tmp_number[l], writer );
 549
 550         }
 551     }
 552
 553     cvEndWriteSeq( &writer );
 554
 555   M_END:
 556     if( tmp_number != NULL )
 557         cvFree( &tmp_number );
 558 //    if(bin_countsj != NULL) cvFree( &bin_countsj );
 559     if( bin_counts != NULL )
 560         cvFree( &bin_counts );
 561
 562     if( vv != NULL )
 563         cvFree( &vv );
 564
 565     if( sub != NULL ) icvDeleteVector (sub);
 566     if( cros != NULL ) icvDeleteVector (cros);
 567
 568     return status;
 569 }
 570
 571
 572 /*F///////////////////////////////////////////////////////////////////////////////////////
 573 //    Name:     cvFindHandRegion
 574 //    Purpose:  finds hand region in range image data
 575 //    Context:
 576 //    Parameters:
 577 //      points - pointer to the input point's set.
 578 //      count  - the number of the input points.
 579 //      indexs - pointer to the input sequence of the point's indexes
 580 //      line   - pointer to the 3D-line
 581 //      size   - size of the hand in meters
 582 //      flag   - hand direction's flag (0 - left, -1 - right,
 583 //               otherwise j-index of the initial image center)
 584 //      center - pointer to the output hand center
 585 //      storage - pointer to the memory storage
 586 //      numbers - pointer to the output sequence of the point's indexes inside
 587 //                hand region
 588 //
 589 //    Notes:
 590 //F*/
 591 CV_IMPL void
 592 cvFindHandRegion( CvPoint3D32f * points, int count,
 593                   CvSeq * indexs,
 594                   float *line, CvSize2D32f size, int flag,
 595                   CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers )
 596 {
 597     CV_FUNCNAME( "cvFindHandRegion" );
 598     __BEGIN__;
 599
 600     if(flag == 0 || flag == -1)
 601         {
 602                 IPPI_CALL( icvFindHandRegion( points, count, indexs, line, size, -flag,
 603                                                    center, storage, numbers ));
 604         }
 605         else
 606                 IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, flag,
 607                                                     center, storage, numbers ));
 608
 609     __CLEANUP__;
 610     __END__;
 611 }
 612
 613 /*F///////////////////////////////////////////////////////////////////////////////////////
 614 //    Name:     cvFindHandRegionA
 615 //    Purpose:  finds hand region in range image data
 616 //    Context:
 617 //    Parameters:
 618 //      points - pointer to the input point's set.
 619 //      count  - the number of the input points.
 620 //      indexs - pointer to the input sequence of the point's indexes
 621 //      line   - pointer to the 3D-line
 622 //      size   - size of the hand in meters
 623 //      jc     - j-index of the initial image center
 624 //      center - pointer to the output hand center
 625 //      storage - pointer to the memory storage
 626 //      numbers - pointer to the output sequence of the point's indexes inside
 627 //                hand region
 628 //
 629 //    Notes:
 630 //F*/
 631 CV_IMPL void
 632 cvFindHandRegionA( CvPoint3D32f * points, int count,
 633                    CvSeq * indexs,
 634                    float *line, CvSize2D32f size, int jc,
 635                    CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers )
 636 {
 637     CV_FUNCNAME( "cvFindHandRegionA" );
 638     __BEGIN__;
 639
 640     IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, jc,
 641                                     center, storage, numbers ));
 642     __CLEANUP__;
 643     __END__;
 644 }
 645