/*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__;
}