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
