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 53static CvStatus CV_STDCALL 54icvFindHandRegion( 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 268static CvStatus CV_STDCALL 269icvFindHandRegionA( 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*/ 591CV_IMPL void 592cvFindHandRegion( 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*/ 631CV_IMPL void 632cvFindHandRegionA( 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