| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| #include "_cvaux.h" |
| |
| #define _CV_NORM_L2(a) (float)(icvSqrt32f(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])) |
| #define _CV_NORM_L22(a) (float)(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]) |
| |
| /****************************************************************************************\ |
| |
| find region where hand is (for gesture recognition) |
| flag = 0 (use left bucket) flag = 1 (use right bucket) |
| |
| \****************************************************************************************/ |
| |
| static CvStatus CV_STDCALL |
| icvFindHandRegion( CvPoint3D32f * points, int count, |
| CvSeq * indexs, |
| float *line, CvSize2D32f size, int flag, |
| CvPoint3D32f * center, |
| CvMemStorage * storage, CvSeq ** numbers ) |
| { |
| |
| /* IppmVect32f sub, cros; */ |
| float *sub, *cros; |
| CvSeqWriter writer; |
| CvSeqReader reader; |
| |
| CvStatus status; |
| int nbins = 20, i, l, i_point, left, right; |
| int *bin_counts = 0; // pointer to the point's counter in the bickets |
| int low_count; // low threshold |
| |
| CvPoint *tmp_number = 0, *pt; |
| float value, vmin, vmax, vl, bsize, vc; |
| float hand_length, hand_length2, hand_left, hand_right; |
| float threshold, threshold2; |
| float *vv = 0; |
| float a[3]; |
| |
| status = CV_OK; |
| |
| hand_length = size.width; |
| hand_length2 = hand_length / 2; |
| |
| threshold = (float) (size.height * 3 / 5.); |
| threshold2 = threshold * threshold; |
| |
| /* low_count = count/nbins; */ |
| low_count = (int) (count / 60.); |
| |
| assert( points != NULL && line != NULL ); |
| if( points == NULL || line == NULL ) |
| return CV_NULLPTR_ERR; |
| |
| assert( count > 5 ); |
| if( count < 5 ) |
| return CV_BADFLAG_ERR; |
| |
| assert( flag == 0 || flag == 1 ); |
| if( flag != 0 && flag != 1 ) |
| return CV_BADFLAG_ERR; |
| |
| /* create vectors */ |
| sub = icvCreateVector_32f( 3 ); |
| cros = icvCreateVector_32f( 3 ); |
| if( sub == NULL || cros == NULL ) |
| return CV_OUTOFMEM_ERR; |
| |
| /* alloc memory for the point's projections on the line */ |
| vv = (float *) cvAlloc( count * sizeof( float )); |
| |
| if( vv == NULL ) |
| return CV_OUTOFMEM_ERR; |
| |
| /* alloc memory for the point's counter in the bickets */ |
| bin_counts = (int *) cvAlloc( nbins * sizeof( int )); |
| |
| if( bin_counts == NULL ) |
| { |
| status = CV_OUTOFMEM_ERR; |
| goto M_END; |
| } |
| memset( bin_counts, 0, nbins * sizeof( int )); |
| |
| cvStartReadSeq( indexs, &reader, 0 ); |
| |
| /* alloc memory for the temporale point's numbers */ |
| tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint )); |
| if( tmp_number == NULL ) |
| { |
| status = CV_OUTOFMEM_ERR; |
| goto M_END; |
| } |
| |
| /* find min and max point's projection on the line */ |
| vmin = 1000; |
| vmax = -1000; |
| i_point = 0; |
| for( i = 0; i < count; i++ ) |
| { |
| /* |
| icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3); |
| |
| icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros); |
| */ |
| |
| sub[0] = points[i].x - line[3]; |
| sub[1] = points[i].y - line[4]; |
| sub[2] = points[i].z - line[5]; |
| a[0] = sub[0] * line[1] - sub[1] * line[0]; |
| a[1] = sub[1] * line[2] - sub[2] * line[1]; |
| a[2] = sub[2] * line[0] - sub[0] * line[2]; |
| |
| /* if(IPPI_NORM_L22 ( cros ) < threshold2) */ |
| if( _CV_NORM_L22( a ) < threshold2 ) |
| { |
| value = (float)icvDotProduct_32f( sub, &line[0], 3 ); |
| if( value > vmax ) |
| vmax = value; |
| if( value < vmin ) |
| vmin = value; |
| |
| vv[i_point] = value; |
| |
| pt = (CvPoint*)cvGetSeqElem( indexs, i ); |
| tmp_number[i_point] = *pt; |
| i_point++; |
| } |
| } |
| |
| /* compute the length of one bucket */ |
| vl = vmax - vmin; |
| bsize = vl / nbins; |
| |
| /* compute the number of points in each bucket */ |
| for( i = 0; i < i_point; i++ ) |
| { |
| l = cvRound( (vv[i] - vmin) / bsize ); |
| bin_counts[l]++; |
| } |
| |
| *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage ); |
| assert( numbers != 0 ); |
| if( numbers == NULL ) |
| { |
| status = CV_OUTOFMEM_ERR; |
| goto M_END; |
| } |
| |
| cvStartAppendToSeq( *numbers, &writer ); |
| |
| if( flag == 0 ) |
| { |
| /* find the leftmost bucket */ |
| for( l = 0; l < nbins; l++ ) |
| { |
| if( bin_counts[l] > low_count ) |
| break; |
| } |
| left = l; |
| |
| /* compute center point of the left hand */ |
| hand_left = vmin + left * bsize; |
| vc = hand_left + hand_length2; |
| hand_right = hand_left + hand_length; |
| } |
| else |
| { |
| /* find the rightmost bucket */ |
| for( l = nbins - 1; l >= 0; l-- ) |
| { |
| if( bin_counts[l] > low_count ) |
| break; |
| } |
| right = l; |
| |
| /* compute center point of the right hand */ |
| hand_right = vmax - (nbins - right - 1) * bsize; |
| vc = hand_right - hand_length2; |
| hand_left = hand_right - hand_length; |
| } |
| |
| icvScaleVector_32f( &line[0], sub, 3, vc ); |
| icvAddVector_32f( &line[3], sub, (float *) center, 3 ); |
| |
| /* select hand's points and calculate mean value */ |
| |
| //ss.x = ss.y = ss.z = 0; |
| for( l = 0; l < i_point; l++ ) |
| { |
| if( vv[l] >= hand_left && vv[l] <= hand_right ) |
| { |
| CV_WRITE_SEQ_ELEM( tmp_number[l], writer ); |
| |
| } |
| } |
| |
| cvEndWriteSeq( &writer ); |
| |
| M_END: |
| if( tmp_number != NULL ) |
| cvFree( &tmp_number ); |
| if( bin_counts != NULL ) |
| cvFree( &bin_counts ); |
| if( vv != NULL ) |
| cvFree( &vv ); |
| if( sub != NULL ) icvDeleteVector (sub); |
| if( cros != NULL ) icvDeleteVector (cros); |
| |
| return status; |
| |
| } |
| |
| |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| |
| |
| #define _CV_NORM_L31(a) (float)(icvSqrt32f(a[0]*a[0] + a[1]*a[1] + a[2]*a[2])) |
| #define _CV_NORM_L32(a) (float)(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]) |
| |
| /****************************************************************************************\ |
| |
| find region where hand is (for gesture recognition) |
| flag = 0 (use left bucket) flag = 1 (use right bucket) |
| |
| \****************************************************************************************/ |
| |
| static CvStatus CV_STDCALL |
| icvFindHandRegionA( CvPoint3D32f * points, int count, |
| CvSeq * indexs, |
| float *line, CvSize2D32f size, int jc, |
| CvPoint3D32f * center, |
| CvMemStorage * storage, CvSeq ** numbers ) |
| { |
| |
| /* IppmVect32f sub, cros; */ |
| float *sub, *cros; |
| float eps = (float) 0.01; |
| CvSeqWriter writer; |
| CvSeqReader reader; |
| |
| CvStatus status; |
| float gor[3] = { 1, 0, 0 }; |
| float ver[3] = { 0, 1, 0 }; |
| |
| int nbins = 20, i, l, i_point, left, right, jmin, jmax, jl; |
| int j_left, j_right; |
| int *bin_counts = 0; // pointer to the point's counter in the bickets |
| |
| // int *bin_countsj = 0; // pointer to the index's counter in the bickets |
| int low_count; // low threshold |
| |
| CvPoint *tmp_number = 0, *pt; |
| float value, vmin, vmax, vl, bsize, bsizej, vc, vcl, vcr; |
| double v_ver, v_gor; |
| float hand_length, hand_length2, hand_left, hand_right; |
| float threshold, threshold2; |
| float *vv = 0; |
| float a[3]; |
| char log; |
| |
| status = CV_OK; |
| |
| hand_length = size.width; |
| hand_length2 = hand_length / 2; |
| |
| threshold = (float) (size.height * 3 / 5.); |
| threshold2 = threshold * threshold; |
| |
| /* low_count = count/nbins; */ |
| low_count = (int) (count / 60.); |
| |
| assert( points != NULL && line != NULL ); |
| if( points == NULL || line == NULL ) |
| return CV_NULLPTR_ERR; |
| |
| assert( count > 5 ); |
| if( count < 5 ) |
| return CV_BADFLAG_ERR; |
| |
| /* create vectors */ |
| sub = icvCreateVector_32f( 3 ); |
| cros = icvCreateVector_32f( 3 ); |
| if( sub == NULL || cros == NULL ) |
| return CV_OUTOFMEM_ERR; |
| |
| /* alloc memory for the point's projections on the line */ |
| vv = (float *) cvAlloc( count * sizeof( float )); |
| |
| if( vv == NULL ) |
| return CV_OUTOFMEM_ERR; |
| |
| /* alloc memory for the point's counter in the bickets */ |
| bin_counts = (int *) cvAlloc( nbins * sizeof( int )); |
| |
| if( bin_counts == NULL ) |
| { |
| status = CV_OUTOFMEM_ERR; |
| goto M_END; |
| } |
| memset( bin_counts, 0, nbins * sizeof( int )); |
| |
| /* alloc memory for the point's counter in the bickets */ |
| // bin_countsj = (int*) icvAlloc(nbins*sizeof(int)); |
| // if(bin_countsj == NULL) {status = CV_OUTOFMEM_ERR; goto M_END;} |
| // memset(bin_countsj,0,nbins*sizeof(int)); |
| |
| cvStartReadSeq( indexs, &reader, 0 ); |
| |
| /* alloc memory for the temporale point's numbers */ |
| tmp_number = (CvPoint *) cvAlloc( count * sizeof( CvPoint )); |
| if( tmp_number == NULL ) |
| { |
| status = CV_OUTOFMEM_ERR; |
| goto M_END; |
| } |
| |
| /* find min and max point's projection on the line */ |
| vmin = 1000; |
| vmax = -1000; |
| jmin = 1000; |
| jmax = -1000; |
| i_point = 0; |
| for( i = 0; i < count; i++ ) |
| { |
| /* |
| icvSubVector_32f ((IppmVect32f )&points[i], (IppmVect32f )&line[3], sub, 3); |
| |
| icvCrossProduct2L_32f ((IppmVect32f )&line[0], sub, cros); |
| */ |
| |
| sub[0] = points[i].x - line[3]; |
| sub[1] = points[i].y - line[4]; |
| sub[2] = points[i].z - line[5]; |
| |
| // if(fabs(sub[0])<eps||fabs(sub[1])<eps||fabs(sub[2])<eps) continue; |
| |
| a[0] = sub[0] * line[1] - sub[1] * line[0]; |
| a[1] = sub[1] * line[2] - sub[2] * line[1]; |
| a[2] = sub[2] * line[0] - sub[0] * line[2]; |
| |
| v_gor = icvDotProduct_32f( gor, &line[0], 3 ); |
| v_ver = icvDotProduct_32f( ver, &line[0], 3 ); |
| |
| if( v_ver > v_gor ) |
| log = true; |
| else |
| log = false; |
| |
| |
| /* if(IPPI_NORM_L22 ( cros ) < threshold2) */ |
| /* |
| if(fabs(a[0])<eps && fabs(a[1])<eps && fabs(a[2])<eps) |
| { |
| icvDotProduct_32f( sub, &line[0], 3, &value); |
| if(value > vmax) vmax = value; |
| if(value < vmin) vmin = value; |
| |
| vv[i_point] = value; |
| |
| pt = (CvPoint* )icvGetSeqElem ( indexs, i, 0); |
| |
| if(pt->x > jmax) jmax = pt->x; |
| if(pt->x < jmin) jmin = pt->x; |
| |
| tmp_number[i_point] = *pt; |
| i_point++; |
| } |
| else |
| */ |
| { |
| if( _CV_NORM_L32( a ) < threshold2 ) |
| { |
| value = (float)icvDotProduct_32f( sub, &line[0], 3 ); |
| if( value > vmax ) |
| vmax = value; |
| if( value < vmin ) |
| vmin = value; |
| |
| vv[i_point] = value; |
| |
| pt = (CvPoint*)cvGetSeqElem( indexs, i ); |
| |
| if( !log ) |
| { |
| if( pt->x > jmax ) |
| jmax = pt->x; |
| if( pt->x < jmin ) |
| jmin = pt->x; |
| } |
| else |
| { |
| if( pt->y > jmax ) |
| jmax = pt->y; |
| if( pt->y < jmin ) |
| jmin = pt->y; |
| } |
| |
| |
| tmp_number[i_point] = *pt; |
| i_point++; |
| } |
| } |
| } |
| |
| /* compute the length of one bucket along the line */ |
| vl = vmax - vmin; |
| |
| /* examining on the arm's existence */ |
| if( vl < eps ) |
| { |
| *numbers = NULL; |
| status = CV_OK; |
| goto M_END; |
| } |
| |
| bsize = vl / nbins; |
| |
| /* compute the number of points in each bucket along the line */ |
| for( i = 0; i < i_point; i++ ) |
| { |
| l = cvRound( (vv[i] - vmin) / bsize ); |
| bin_counts[l]++; |
| } |
| |
| /* compute the length of one bucket along the X axe */ |
| jl = jmax - jmin; |
| if( jl <= 1 ) |
| { |
| *numbers = NULL; |
| status = CV_OK; |
| goto M_END; |
| } |
| |
| bsizej = (float) (jl / (nbins + 0.)); |
| |
| /* compute the number of points in each bucket along the X axe */ |
| // for(i=0;i<i_point;i++) |
| // { |
| // l = cvRound((tmp_number[i].x - jmin)/bsizej); |
| // bin_countsj[l]++; |
| // } |
| |
| |
| left = right = -1; |
| |
| /* find the leftmost and the rightmost buckets */ |
| for( l = 0; l < nbins; l++ ) |
| { |
| if( bin_counts[l] > low_count && left == -1 ) |
| left = l; |
| else if( bin_counts[l] > low_count && left >= 0 ) |
| right = l; |
| |
| } |
| |
| /* compute center point of the left hand */ |
| if( left == -1 && right == -1 ) |
| { |
| *numbers = NULL; |
| status = CV_OK; |
| goto M_END; |
| } |
| |
| hand_left = vmin + left * bsize; |
| j_left = (int) (jmin + left * bsizej); |
| |
| vcl = hand_left + hand_length2; |
| |
| /* compute center point of the right hand */ |
| hand_right = vmax - (nbins - right - 1) * bsize; |
| vcr = hand_right - hand_length2; |
| |
| j_right = (int) (jmax - (nbins - right - 1) * bsizej); |
| |
| j_left = abs( j_left - jc ); |
| j_right = abs( j_right - jc ); |
| |
| if( j_left <= j_right ) |
| { |
| hand_right = hand_left + hand_length; |
| vc = vcl; |
| } |
| else |
| { |
| hand_left = hand_right - hand_length; |
| vc = vcr; |
| } |
| |
| icvScaleVector_32f( &line[0], sub, 3, vc ); |
| icvAddVector_32f( &line[3], sub, (float *) center, 3 ); |
| |
| /* select hand's points and calculate mean value */ |
| *numbers = cvCreateSeq( CV_SEQ_POINT_SET, sizeof( CvSeq ), sizeof( CvPoint ), storage ); |
| assert( *numbers != 0 ); |
| if( *numbers == NULL ) |
| { |
| status = CV_OUTOFMEM_ERR; |
| goto M_END; |
| } |
| |
| cvStartAppendToSeq( *numbers, &writer ); |
| |
| for( l = 0; l < i_point; l++ ) |
| { |
| if( vv[l] >= hand_left && vv[l] <= hand_right ) |
| { |
| CV_WRITE_SEQ_ELEM( tmp_number[l], writer ); |
| |
| } |
| } |
| |
| cvEndWriteSeq( &writer ); |
| |
| M_END: |
| if( tmp_number != NULL ) |
| cvFree( &tmp_number ); |
| // if(bin_countsj != NULL) cvFree( &bin_countsj ); |
| if( bin_counts != NULL ) |
| cvFree( &bin_counts ); |
| |
| if( vv != NULL ) |
| cvFree( &vv ); |
| |
| if( sub != NULL ) icvDeleteVector (sub); |
| if( cros != NULL ) icvDeleteVector (cros); |
| |
| return status; |
| } |
| |
| |
| /*F/////////////////////////////////////////////////////////////////////////////////////// |
| // Name: cvFindHandRegion |
| // Purpose: finds hand region in range image data |
| // Context: |
| // Parameters: |
| // points - pointer to the input point's set. |
| // count - the number of the input points. |
| // indexs - pointer to the input sequence of the point's indexes |
| // line - pointer to the 3D-line |
| // size - size of the hand in meters |
| // flag - hand direction's flag (0 - left, -1 - right, |
| // otherwise j-index of the initial image center) |
| // center - pointer to the output hand center |
| // storage - pointer to the memory storage |
| // numbers - pointer to the output sequence of the point's indexes inside |
| // hand region |
| // |
| // Notes: |
| //F*/ |
| CV_IMPL void |
| cvFindHandRegion( CvPoint3D32f * points, int count, |
| CvSeq * indexs, |
| float *line, CvSize2D32f size, int flag, |
| CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers ) |
| { |
| CV_FUNCNAME( "cvFindHandRegion" ); |
| __BEGIN__; |
| |
| if(flag == 0 || flag == -1) |
| { |
| IPPI_CALL( icvFindHandRegion( points, count, indexs, line, size, -flag, |
| center, storage, numbers )); |
| } |
| else |
| IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, flag, |
| center, storage, numbers )); |
| |
| __CLEANUP__; |
| __END__; |
| } |
| |
| /*F/////////////////////////////////////////////////////////////////////////////////////// |
| // Name: cvFindHandRegionA |
| // Purpose: finds hand region in range image data |
| // Context: |
| // Parameters: |
| // points - pointer to the input point's set. |
| // count - the number of the input points. |
| // indexs - pointer to the input sequence of the point's indexes |
| // line - pointer to the 3D-line |
| // size - size of the hand in meters |
| // jc - j-index of the initial image center |
| // center - pointer to the output hand center |
| // storage - pointer to the memory storage |
| // numbers - pointer to the output sequence of the point's indexes inside |
| // hand region |
| // |
| // Notes: |
| //F*/ |
| CV_IMPL void |
| cvFindHandRegionA( CvPoint3D32f * points, int count, |
| CvSeq * indexs, |
| float *line, CvSize2D32f size, int jc, |
| CvPoint3D32f * center, CvMemStorage * storage, CvSeq ** numbers ) |
| { |
| CV_FUNCNAME( "cvFindHandRegionA" ); |
| __BEGIN__; |
| |
| IPPI_CALL( icvFindHandRegionA( points, count, indexs, line, size, jc, |
| center, storage, numbers )); |
| __CLEANUP__; |
| __END__; |
| } |
| |
