Home | History | Annotate | Download | only in src
      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 
    646