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 "_cv.h" 42#include <float.h> 43#include <stdio.h> 44 45static void 46intersect( CvPoint2D32f pt, CvSize win_size, CvSize imgSize, 47 CvPoint* min_pt, CvPoint* max_pt ) 48{ 49 CvPoint ipt; 50 51 ipt.x = cvFloor( pt.x ); 52 ipt.y = cvFloor( pt.y ); 53 54 ipt.x -= win_size.width; 55 ipt.y -= win_size.height; 56 57 win_size.width = win_size.width * 2 + 1; 58 win_size.height = win_size.height * 2 + 1; 59 60 min_pt->x = MAX( 0, -ipt.x ); 61 min_pt->y = MAX( 0, -ipt.y ); 62 max_pt->x = MIN( win_size.width, imgSize.width - ipt.x ); 63 max_pt->y = MIN( win_size.height, imgSize.height - ipt.y ); 64} 65 66 67static int icvMinimalPyramidSize( CvSize imgSize ) 68{ 69 return cvAlign(imgSize.width,8) * imgSize.height / 3; 70} 71 72 73static void 74icvInitPyramidalAlgorithm( const CvMat* imgA, const CvMat* imgB, 75 CvMat* pyrA, CvMat* pyrB, 76 int level, CvTermCriteria * criteria, 77 int max_iters, int flags, 78 uchar *** imgI, uchar *** imgJ, 79 int **step, CvSize** size, 80 double **scale, uchar ** buffer ) 81{ 82 CV_FUNCNAME( "icvInitPyramidalAlgorithm" ); 83 84 __BEGIN__; 85 86 const int ALIGN = 8; 87 int pyrBytes, bufferBytes = 0, elem_size; 88 int level1 = level + 1; 89 90 int i; 91 CvSize imgSize, levelSize; 92 93 *buffer = 0; 94 *imgI = *imgJ = 0; 95 *step = 0; 96 *scale = 0; 97 *size = 0; 98 99 /* check input arguments */ 100 if( ((flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA) || 101 ((flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB) ) 102 CV_ERROR( CV_StsNullPtr, "Some of the precomputed pyramids are missing" ); 103 104 if( level < 0 ) 105 CV_ERROR( CV_StsOutOfRange, "The number of pyramid layers is negative" ); 106 107 switch( criteria->type ) 108 { 109 case CV_TERMCRIT_ITER: 110 criteria->epsilon = 0.f; 111 break; 112 case CV_TERMCRIT_EPS: 113 criteria->max_iter = max_iters; 114 break; 115 case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: 116 break; 117 default: 118 assert( 0 ); 119 CV_ERROR( CV_StsBadArg, "Invalid termination criteria" ); 120 } 121 122 /* compare squared values */ 123 criteria->epsilon *= criteria->epsilon; 124 125 /* set pointers and step for every level */ 126 pyrBytes = 0; 127 128 imgSize = cvGetSize(imgA); 129 elem_size = CV_ELEM_SIZE(imgA->type); 130 levelSize = imgSize; 131 132 for( i = 1; i < level1; i++ ) 133 { 134 levelSize.width = (levelSize.width + 1) >> 1; 135 levelSize.height = (levelSize.height + 1) >> 1; 136 137 int tstep = cvAlign(levelSize.width,ALIGN) * elem_size; 138 pyrBytes += tstep * levelSize.height; 139 } 140 141 assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 ); 142 143 /* buffer_size = <size for patches> + <size for pyramids> */ 144 bufferBytes = (int)((level1 >= 0) * ((pyrA->data.ptr == 0) + 145 (pyrB->data.ptr == 0)) * pyrBytes + 146 (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) + 147 sizeof(size[0][0]) + sizeof(scale[0][0])) * level1); 148 149 CV_CALL( *buffer = (uchar *)cvAlloc( bufferBytes )); 150 151 *imgI = (uchar **) buffer[0]; 152 *imgJ = *imgI + level1; 153 *step = (int *) (*imgJ + level1); 154 *scale = (double *) (*step + level1); 155 *size = (CvSize *)(*scale + level1); 156 157 imgI[0][0] = imgA->data.ptr; 158 imgJ[0][0] = imgB->data.ptr; 159 step[0][0] = imgA->step; 160 scale[0][0] = 1; 161 size[0][0] = imgSize; 162 163 if( level > 0 ) 164 { 165 uchar *bufPtr = (uchar *) (*size + level1); 166 uchar *ptrA = pyrA->data.ptr; 167 uchar *ptrB = pyrB->data.ptr; 168 169 if( !ptrA ) 170 { 171 ptrA = bufPtr; 172 bufPtr += pyrBytes; 173 } 174 175 if( !ptrB ) 176 ptrB = bufPtr; 177 178 levelSize = imgSize; 179 180 /* build pyramids for both frames */ 181 for( i = 1; i <= level; i++ ) 182 { 183 int levelBytes; 184 CvMat prev_level, next_level; 185 186 levelSize.width = (levelSize.width + 1) >> 1; 187 levelSize.height = (levelSize.height + 1) >> 1; 188 189 size[0][i] = levelSize; 190 step[0][i] = cvAlign( levelSize.width, ALIGN ) * elem_size; 191 scale[0][i] = scale[0][i - 1] * 0.5; 192 193 levelBytes = step[0][i] * levelSize.height; 194 imgI[0][i] = (uchar *) ptrA; 195 ptrA += levelBytes; 196 197 if( !(flags & CV_LKFLOW_PYR_A_READY) ) 198 { 199 prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); 200 next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); 201 cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] ); 202 cvSetData( &next_level, imgI[0][i], step[0][i] ); 203 cvPyrDown( &prev_level, &next_level ); 204 } 205 206 imgJ[0][i] = (uchar *) ptrB; 207 ptrB += levelBytes; 208 209 if( !(flags & CV_LKFLOW_PYR_B_READY) ) 210 { 211 prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 ); 212 next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 ); 213 cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] ); 214 cvSetData( &next_level, imgJ[0][i], step[0][i] ); 215 cvPyrDown( &prev_level, &next_level ); 216 } 217 } 218 } 219 220 __END__; 221} 222 223 224/* compute dI/dx and dI/dy */ 225static void 226icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step, 227 CvSize src_size, const float* smooth_k, float* buffer0 ) 228{ 229 int src_width = src_size.width, dst_width = src_size.width-2; 230 int x, height = src_size.height - 2; 231 float* buffer1 = buffer0 + src_width; 232 233 src_step /= sizeof(src[0]); 234 dst_step /= sizeof(dstX[0]); 235 236 for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step ) 237 { 238 const float* src2 = src + src_step; 239 const float* src3 = src + src_step*2; 240 241 for( x = 0; x < src_width; x++ ) 242 { 243 float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1]; 244 float t1 = src3[x] - src[x]; 245 buffer0[x] = t0; buffer1[x] = t1; 246 } 247 248 for( x = 0; x < dst_width; x++ ) 249 { 250 float t0 = buffer0[x+2] - buffer0[x]; 251 float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1]; 252 dstX[x] = t0; dstY[x] = t1; 253 } 254 } 255} 256 257 258icvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0; 259icvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0; 260icvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0; 261 262 263CV_IMPL void 264cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB, 265 void* pyrarrA, void* pyrarrB, 266 const CvPoint2D32f * featuresA, 267 CvPoint2D32f * featuresB, 268 int count, CvSize winSize, int level, 269 char *status, float *error, 270 CvTermCriteria criteria, int flags ) 271{ 272 uchar *pyrBuffer = 0; 273 uchar *buffer = 0; 274 float* _error = 0; 275 char* _status = 0; 276 277 void* ipp_optflow_state = 0; 278 279 CV_FUNCNAME( "cvCalcOpticalFlowPyrLK" ); 280 281 __BEGIN__; 282 283 const int MAX_ITERS = 100; 284 285 CvMat stubA, *imgA = (CvMat*)arrA; 286 CvMat stubB, *imgB = (CvMat*)arrB; 287 CvMat pstubA, *pyrA = (CvMat*)pyrarrA; 288 CvMat pstubB, *pyrB = (CvMat*)pyrarrB; 289 CvSize imgSize; 290 static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ 291 292 int bufferBytes = 0; 293 uchar **imgI = 0; 294 uchar **imgJ = 0; 295 int *step = 0; 296 double *scale = 0; 297 CvSize* size = 0; 298 299 int threadCount = cvGetNumThreads(); 300 float* _patchI[CV_MAX_THREADS]; 301 float* _patchJ[CV_MAX_THREADS]; 302 float* _Ix[CV_MAX_THREADS]; 303 float* _Iy[CV_MAX_THREADS]; 304 305 int i, l; 306 307 CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); 308 int patchLen = patchSize.width * patchSize.height; 309 int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2); 310 311 CV_CALL( imgA = cvGetMat( imgA, &stubA )); 312 CV_CALL( imgB = cvGetMat( imgB, &stubB )); 313 314 if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) 315 CV_ERROR( CV_StsUnsupportedFormat, "" ); 316 317 if( !CV_ARE_TYPES_EQ( imgA, imgB )) 318 CV_ERROR( CV_StsUnmatchedFormats, "" ); 319 320 if( !CV_ARE_SIZES_EQ( imgA, imgB )) 321 CV_ERROR( CV_StsUnmatchedSizes, "" ); 322 323 if( imgA->step != imgB->step ) 324 CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); 325 326 imgSize = cvGetMatSize( imgA ); 327 328 if( pyrA ) 329 { 330 CV_CALL( pyrA = cvGetMat( pyrA, &pstubA )); 331 332 if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) 333 CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" ); 334 } 335 else 336 { 337 pyrA = &pstubA; 338 pyrA->data.ptr = 0; 339 } 340 341 if( pyrB ) 342 { 343 CV_CALL( pyrB = cvGetMat( pyrB, &pstubB )); 344 345 if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) 346 CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" ); 347 } 348 else 349 { 350 pyrB = &pstubB; 351 pyrB->data.ptr = 0; 352 } 353 354 if( count == 0 ) 355 EXIT; 356 357 if( !featuresA || !featuresB ) 358 CV_ERROR( CV_StsNullPtr, "Some of arrays of point coordinates are missing" ); 359 360 if( count < 0 ) 361 CV_ERROR( CV_StsOutOfRange, "The number of tracked points is negative or zero" ); 362 363 if( winSize.width <= 1 || winSize.height <= 1 ) 364 CV_ERROR( CV_StsBadSize, "Invalid search window size" ); 365 366 for( i = 0; i < threadCount; i++ ) 367 _patchI[i] = _patchJ[i] = _Ix[i] = _Iy[i] = 0; 368 369 CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, pyrA, pyrB, 370 level, &criteria, MAX_ITERS, flags, 371 &imgI, &imgJ, &step, &size, &scale, &pyrBuffer )); 372 373 if( !status ) 374 CV_CALL( status = _status = (char*)cvAlloc( count*sizeof(_status[0]) )); 375 376#if 0 377 if( icvOpticalFlowPyrLKInitAlloc_8u_C1R_p && 378 icvOpticalFlowPyrLKFree_8u_C1R_p && 379 icvOpticalFlowPyrLK_8u_C1R_p && 380 winSize.width == winSize.height && 381 icvOpticalFlowPyrLKInitAlloc_8u_C1R_p( &ipp_optflow_state, imgSize, 382 winSize.width*2+1, cvAlgHintAccurate ) >= 0 ) 383 { 384 CvPyramid ipp_pyrA, ipp_pyrB; 385 static const double rate[] = { 1, 0.5, 0.25, 0.125, 0.0625, 0.03125, 0.015625, 0.0078125, 386 0.00390625, 0.001953125, 0.0009765625, 0.00048828125, 0.000244140625, 387 0.0001220703125 }; 388 // initialize pyramid structures 389 assert( level < 14 ); 390 ipp_pyrA.ptr = imgI; 391 ipp_pyrB.ptr = imgJ; 392 ipp_pyrA.sz = ipp_pyrB.sz = size; 393 ipp_pyrA.rate = ipp_pyrB.rate = (double*)rate; 394 ipp_pyrA.step = ipp_pyrB.step = step; 395 ipp_pyrA.state = ipp_pyrB.state = 0; 396 ipp_pyrA.level = ipp_pyrB.level = level; 397 398 if( !error ) 399 CV_CALL( error = _error = (float*)cvAlloc( count*sizeof(_error[0]) )); 400 401 for( i = 0; i < count; i++ ) 402 featuresB[i] = featuresA[i]; 403 404 if( icvOpticalFlowPyrLK_8u_C1R_p( &ipp_pyrA, &ipp_pyrB, 405 (const float*)featuresA, (float*)featuresB, status, error, count, 406 winSize.width*2 + 1, level, criteria.max_iter, 407 (float)criteria.epsilon, ipp_optflow_state ) >= 0 ) 408 { 409 for( i = 0; i < count; i++ ) 410 status[i] = status[i] == 0; 411 EXIT; 412 } 413 } 414#endif 415 416 /* buffer_size = <size for patches> + <size for pyramids> */ 417 bufferBytes = (srcPatchLen + patchLen * 3) * sizeof( _patchI[0][0] ) * threadCount; 418 CV_CALL( buffer = (uchar*)cvAlloc( bufferBytes )); 419 420 for( i = 0; i < threadCount; i++ ) 421 { 422 _patchI[i] = i == 0 ? (float*)buffer : _Iy[i-1] + patchLen; 423 _patchJ[i] = _patchI[i] + srcPatchLen; 424 _Ix[i] = _patchJ[i] + patchLen; 425 _Iy[i] = _Ix[i] + patchLen; 426 } 427 428 memset( status, 1, count ); 429 if( error ) 430 memset( error, 0, count*sizeof(error[0]) ); 431 432 if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) 433 memcpy( featuresB, featuresA, count*sizeof(featuresA[0])); 434 435 /* do processing from top pyramid level (smallest image) 436 to the bottom (original image) */ 437 for( l = level; l >= 0; l-- ) 438 { 439 CvSize levelSize = size[l]; 440 int levelStep = step[l]; 441 442 { 443#ifdef _OPENMP 444 #pragma omp parallel for num_threads(threadCount) schedule(dynamic) 445#endif // _OPENMP 446 /* find flow for each given point */ 447 for( i = 0; i < count; i++ ) 448 { 449 CvPoint2D32f v; 450 CvPoint minI, maxI, minJ, maxJ; 451 CvSize isz, jsz; 452 int pt_status; 453 CvPoint2D32f u; 454 CvPoint prev_minJ = { -1, -1 }, prev_maxJ = { -1, -1 }; 455 double Gxx = 0, Gxy = 0, Gyy = 0, D = 0, minEig = 0; 456 float prev_mx = 0, prev_my = 0; 457 int j, x, y; 458 int threadIdx = cvGetThreadNum(); 459 float* patchI = _patchI[threadIdx]; 460 float* patchJ = _patchJ[threadIdx]; 461 float* Ix = _Ix[threadIdx]; 462 float* Iy = _Iy[threadIdx]; 463 464 v.x = featuresB[i].x; 465 v.y = featuresB[i].y; 466 if( l < level ) 467 { 468 v.x += v.x; 469 v.y += v.y; 470 } 471 else 472 { 473 v.x = (float)(v.x * scale[l]); 474 v.y = (float)(v.y * scale[l]); 475 } 476 477 pt_status = status[i]; 478 if( !pt_status ) 479 continue; 480 481 minI = maxI = minJ = maxJ = cvPoint( 0, 0 ); 482 483 u.x = (float) (featuresA[i].x * scale[l]); 484 u.y = (float) (featuresA[i].y * scale[l]); 485 486 intersect( u, winSize, levelSize, &minI, &maxI ); 487 isz = jsz = cvSize(maxI.x - minI.x + 2, maxI.y - minI.y + 2); 488 u.x += (minI.x - (patchSize.width - maxI.x + 1))*0.5f; 489 u.y += (minI.y - (patchSize.height - maxI.y + 1))*0.5f; 490 491 if( isz.width < 3 || isz.height < 3 || 492 icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, levelSize, 493 patchI, isz.width*sizeof(patchI[0]), isz, u ) < 0 ) 494 { 495 /* point is outside the image. take the next */ 496 status[i] = 0; 497 continue; 498 } 499 500 icvCalcIxIy_32f( patchI, isz.width*sizeof(patchI[0]), Ix, Iy, 501 (isz.width-2)*sizeof(patchI[0]), isz, smoothKernel, patchJ ); 502 503 for( j = 0; j < criteria.max_iter; j++ ) 504 { 505 double bx = 0, by = 0; 506 float mx, my; 507 CvPoint2D32f _v; 508 509 intersect( v, winSize, levelSize, &minJ, &maxJ ); 510 511 minJ.x = MAX( minJ.x, minI.x ); 512 minJ.y = MAX( minJ.y, minI.y ); 513 514 maxJ.x = MIN( maxJ.x, maxI.x ); 515 maxJ.y = MIN( maxJ.y, maxI.y ); 516 517 jsz = cvSize(maxJ.x - minJ.x, maxJ.y - minJ.y); 518 519 _v.x = v.x + (minJ.x - (patchSize.width - maxJ.x + 1))*0.5f; 520 _v.y = v.y + (minJ.y - (patchSize.height - maxJ.y + 1))*0.5f; 521 522 if( jsz.width < 1 || jsz.height < 1 || 523 icvGetRectSubPix_8u32f_C1R( imgJ[l], levelStep, levelSize, patchJ, 524 jsz.width*sizeof(patchJ[0]), jsz, _v ) < 0 ) 525 { 526 /* point is outside image. take the next */ 527 pt_status = 0; 528 break; 529 } 530 531 if( maxJ.x == prev_maxJ.x && maxJ.y == prev_maxJ.y && 532 minJ.x == prev_minJ.x && minJ.y == prev_minJ.y ) 533 { 534 for( y = 0; y < jsz.height; y++ ) 535 { 536 const float* pi = patchI + 537 (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; 538 const float* pj = patchJ + y*jsz.width; 539 const float* ix = Ix + 540 (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x; 541 const float* iy = Iy + (ix - Ix); 542 543 for( x = 0; x < jsz.width; x++ ) 544 { 545 double t0 = pi[x] - pj[x]; 546 bx += t0 * ix[x]; 547 by += t0 * iy[x]; 548 } 549 } 550 } 551 else 552 { 553 Gxx = Gyy = Gxy = 0; 554 for( y = 0; y < jsz.height; y++ ) 555 { 556 const float* pi = patchI + 557 (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; 558 const float* pj = patchJ + y*jsz.width; 559 const float* ix = Ix + 560 (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x; 561 const float* iy = Iy + (ix - Ix); 562 563 for( x = 0; x < jsz.width; x++ ) 564 { 565 double t = pi[x] - pj[x]; 566 bx += (double) (t * ix[x]); 567 by += (double) (t * iy[x]); 568 Gxx += ix[x] * ix[x]; 569 Gxy += ix[x] * iy[x]; 570 Gyy += iy[x] * iy[x]; 571 } 572 } 573 574 D = Gxx * Gyy - Gxy * Gxy; 575 if( D < DBL_EPSILON ) 576 { 577 pt_status = 0; 578 break; 579 } 580 581 // Adi Shavit - 2008.05 582 if( flags & CV_LKFLOW_GET_MIN_EIGENVALS ) 583 minEig = (Gyy + Gxx - sqrt((Gxx-Gyy)*(Gxx-Gyy) + 4.*Gxy*Gxy))/(2*jsz.height*jsz.width); 584 585 D = 1. / D; 586 587 prev_minJ = minJ; 588 prev_maxJ = maxJ; 589 } 590 591 mx = (float) ((Gyy * bx - Gxy * by) * D); 592 my = (float) ((Gxx * by - Gxy * bx) * D); 593 594 v.x += mx; 595 v.y += my; 596 597 if( mx * mx + my * my < criteria.epsilon ) 598 break; 599 600 if( j > 0 && fabs(mx + prev_mx) < 0.01 && fabs(my + prev_my) < 0.01 ) 601 { 602 v.x -= mx*0.5f; 603 v.y -= my*0.5f; 604 break; 605 } 606 prev_mx = mx; 607 prev_my = my; 608 } 609 610 featuresB[i] = v; 611 status[i] = (char)pt_status; 612 if( l == 0 && error && pt_status ) 613 { 614 /* calc error */ 615 double err = 0; 616 if( flags & CV_LKFLOW_GET_MIN_EIGENVALS ) 617 err = minEig; 618 else 619 { 620 for( y = 0; y < jsz.height; y++ ) 621 { 622 const float* pi = patchI + 623 (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1; 624 const float* pj = patchJ + y*jsz.width; 625 626 for( x = 0; x < jsz.width; x++ ) 627 { 628 double t = pi[x] - pj[x]; 629 err += t * t; 630 } 631 } 632 err = sqrt(err); 633 } 634 error[i] = (float)err; 635 } 636 } // end of point processing loop (i) 637 } 638 } // end of pyramid levels loop (l) 639 640 __END__; 641 642 if( ipp_optflow_state ) 643 icvOpticalFlowPyrLKFree_8u_C1R_p( ipp_optflow_state ); 644 645 cvFree( &pyrBuffer ); 646 cvFree( &buffer ); 647 cvFree( &_error ); 648 cvFree( &_status ); 649} 650 651 652/* Affine tracking algorithm */ 653 654CV_IMPL void 655cvCalcAffineFlowPyrLK( const void* arrA, const void* arrB, 656 void* pyrarrA, void* pyrarrB, 657 const CvPoint2D32f * featuresA, 658 CvPoint2D32f * featuresB, 659 float *matrices, int count, 660 CvSize winSize, int level, 661 char *status, float *error, 662 CvTermCriteria criteria, int flags ) 663{ 664 const int MAX_ITERS = 100; 665 666 char* _status = 0; 667 uchar *buffer = 0; 668 uchar *pyr_buffer = 0; 669 670 CV_FUNCNAME( "cvCalcAffineFlowPyrLK" ); 671 672 __BEGIN__; 673 674 CvMat stubA, *imgA = (CvMat*)arrA; 675 CvMat stubB, *imgB = (CvMat*)arrB; 676 CvMat pstubA, *pyrA = (CvMat*)pyrarrA; 677 CvMat pstubB, *pyrB = (CvMat*)pyrarrB; 678 679 static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */ 680 681 int bufferBytes = 0; 682 683 uchar **imgI = 0; 684 uchar **imgJ = 0; 685 int *step = 0; 686 double *scale = 0; 687 CvSize* size = 0; 688 689 float *patchI; 690 float *patchJ; 691 float *Ix; 692 float *Iy; 693 694 int i, j, k, l; 695 696 CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 ); 697 int patchLen = patchSize.width * patchSize.height; 698 int patchStep = patchSize.width * sizeof( patchI[0] ); 699 700 CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 ); 701 int srcPatchLen = srcPatchSize.width * srcPatchSize.height; 702 int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] ); 703 CvSize imgSize; 704 float eps = (float)MIN(winSize.width, winSize.height); 705 706 CV_CALL( imgA = cvGetMat( imgA, &stubA )); 707 CV_CALL( imgB = cvGetMat( imgB, &stubB )); 708 709 if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 ) 710 CV_ERROR( CV_StsUnsupportedFormat, "" ); 711 712 if( !CV_ARE_TYPES_EQ( imgA, imgB )) 713 CV_ERROR( CV_StsUnmatchedFormats, "" ); 714 715 if( !CV_ARE_SIZES_EQ( imgA, imgB )) 716 CV_ERROR( CV_StsUnmatchedSizes, "" ); 717 718 if( imgA->step != imgB->step ) 719 CV_ERROR( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" ); 720 721 if( !matrices ) 722 CV_ERROR( CV_StsNullPtr, "" ); 723 724 imgSize = cvGetMatSize( imgA ); 725 726 if( pyrA ) 727 { 728 CV_CALL( pyrA = cvGetMat( pyrA, &pstubA )); 729 730 if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) ) 731 CV_ERROR( CV_StsBadArg, "pyramid A has insufficient size" ); 732 } 733 else 734 { 735 pyrA = &pstubA; 736 pyrA->data.ptr = 0; 737 } 738 739 if( pyrB ) 740 { 741 CV_CALL( pyrB = cvGetMat( pyrB, &pstubB )); 742 743 if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) ) 744 CV_ERROR( CV_StsBadArg, "pyramid B has insufficient size" ); 745 } 746 else 747 { 748 pyrB = &pstubB; 749 pyrB->data.ptr = 0; 750 } 751 752 if( count == 0 ) 753 EXIT; 754 755 /* check input arguments */ 756 if( !featuresA || !featuresB || !matrices ) 757 CV_ERROR( CV_StsNullPtr, "" ); 758 759 if( winSize.width <= 1 || winSize.height <= 1 ) 760 CV_ERROR( CV_StsOutOfRange, "the search window is too small" ); 761 762 if( count < 0 ) 763 CV_ERROR( CV_StsOutOfRange, "" ); 764 765 CV_CALL( icvInitPyramidalAlgorithm( imgA, imgB, 766 pyrA, pyrB, level, &criteria, MAX_ITERS, flags, 767 &imgI, &imgJ, &step, &size, &scale, &pyr_buffer )); 768 769 /* buffer_size = <size for patches> + <size for pyramids> */ 770 bufferBytes = (srcPatchLen + patchLen*3)*sizeof(patchI[0]) + (36*2 + 6)*sizeof(double); 771 772 CV_CALL( buffer = (uchar*)cvAlloc(bufferBytes)); 773 774 if( !status ) 775 CV_CALL( status = _status = (char*)cvAlloc(count) ); 776 777 patchI = (float *) buffer; 778 patchJ = patchI + srcPatchLen; 779 Ix = patchJ + patchLen; 780 Iy = Ix + patchLen; 781 782 if( status ) 783 memset( status, 1, count ); 784 785 if( !(flags & CV_LKFLOW_INITIAL_GUESSES) ) 786 { 787 memcpy( featuresB, featuresA, count * sizeof( featuresA[0] )); 788 for( i = 0; i < count * 4; i += 4 ) 789 { 790 matrices[i] = matrices[i + 3] = 1.f; 791 matrices[i + 1] = matrices[i + 2] = 0.f; 792 } 793 } 794 795 for( i = 0; i < count; i++ ) 796 { 797 featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5); 798 featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5); 799 } 800 801 /* do processing from top pyramid level (smallest image) 802 to the bottom (original image) */ 803 for( l = level; l >= 0; l-- ) 804 { 805 CvSize levelSize = size[l]; 806 int levelStep = step[l]; 807 808 /* find flow for each given point at the particular level */ 809 for( i = 0; i < count; i++ ) 810 { 811 CvPoint2D32f u; 812 float Av[6]; 813 double G[36]; 814 double meanI = 0, meanJ = 0; 815 int x, y; 816 int pt_status = status[i]; 817 CvMat mat; 818 819 if( !pt_status ) 820 continue; 821 822 Av[0] = matrices[i*4]; 823 Av[1] = matrices[i*4+1]; 824 Av[3] = matrices[i*4+2]; 825 Av[4] = matrices[i*4+3]; 826 827 Av[2] = featuresB[i].x += featuresB[i].x; 828 Av[5] = featuresB[i].y += featuresB[i].y; 829 830 u.x = (float) (featuresA[i].x * scale[l]); 831 u.y = (float) (featuresA[i].y * scale[l]); 832 833 if( u.x < -eps || u.x >= levelSize.width+eps || 834 u.y < -eps || u.y >= levelSize.height+eps || 835 icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep, 836 levelSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 ) 837 { 838 /* point is outside the image. take the next */ 839 if( l == 0 ) 840 status[i] = 0; 841 continue; 842 } 843 844 icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy, 845 (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize, 846 smoothKernel, patchJ ); 847 848 /* repack patchI (remove borders) */ 849 for( k = 0; k < patchSize.height; k++ ) 850 memcpy( patchI + k * patchSize.width, 851 patchI + (k + 1) * srcPatchSize.width + 1, patchStep ); 852 853 memset( G, 0, sizeof( G )); 854 855 /* calculate G matrix */ 856 for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) 857 { 858 for( x = -winSize.width; x <= winSize.width; x++, k++ ) 859 { 860 double ixix = ((double) Ix[k]) * Ix[k]; 861 double ixiy = ((double) Ix[k]) * Iy[k]; 862 double iyiy = ((double) Iy[k]) * Iy[k]; 863 864 double xx, xy, yy; 865 866 G[0] += ixix; 867 G[1] += ixiy; 868 G[2] += x * ixix; 869 G[3] += y * ixix; 870 G[4] += x * ixiy; 871 G[5] += y * ixiy; 872 873 // G[6] == G[1] 874 G[7] += iyiy; 875 // G[8] == G[4] 876 // G[9] == G[5] 877 G[10] += x * iyiy; 878 G[11] += y * iyiy; 879 880 xx = x * x; 881 xy = x * y; 882 yy = y * y; 883 884 // G[12] == G[2] 885 // G[13] == G[8] == G[4] 886 G[14] += xx * ixix; 887 G[15] += xy * ixix; 888 G[16] += xx * ixiy; 889 G[17] += xy * ixiy; 890 891 // G[18] == G[3] 892 // G[19] == G[9] 893 // G[20] == G[15] 894 G[21] += yy * ixix; 895 // G[22] == G[17] 896 G[23] += yy * ixiy; 897 898 // G[24] == G[4] 899 // G[25] == G[10] 900 // G[26] == G[16] 901 // G[27] == G[22] 902 G[28] += xx * iyiy; 903 G[29] += xy * iyiy; 904 905 // G[30] == G[5] 906 // G[31] == G[11] 907 // G[32] == G[17] 908 // G[33] == G[23] 909 // G[34] == G[29] 910 G[35] += yy * iyiy; 911 912 meanI += patchI[k]; 913 } 914 } 915 916 meanI /= patchSize.width*patchSize.height; 917 918 G[8] = G[4]; 919 G[9] = G[5]; 920 G[22] = G[17]; 921 922 // fill part of G below its diagonal 923 for( y = 1; y < 6; y++ ) 924 for( x = 0; x < y; x++ ) 925 G[y * 6 + x] = G[x * 6 + y]; 926 927 cvInitMatHeader( &mat, 6, 6, CV_64FC1, G ); 928 929 if( cvInvert( &mat, &mat, CV_SVD ) < 1e-4 ) 930 { 931 /* bad matrix. take the next point */ 932 if( l == 0 ) 933 status[i] = 0; 934 continue; 935 } 936 937 for( j = 0; j < criteria.max_iter; j++ ) 938 { 939 double b[6] = {0,0,0,0,0,0}, eta[6]; 940 double t0, t1, s = 0; 941 942 if( Av[2] < -eps || Av[2] >= levelSize.width+eps || 943 Av[5] < -eps || Av[5] >= levelSize.height+eps || 944 icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], levelStep, 945 levelSize, patchJ, patchStep, patchSize, Av ) < 0 ) 946 { 947 pt_status = 0; 948 break; 949 } 950 951 for( y = -winSize.height, k = 0, meanJ = 0; y <= winSize.height; y++ ) 952 for( x = -winSize.width; x <= winSize.width; x++, k++ ) 953 meanJ += patchJ[k]; 954 955 meanJ = meanJ / (patchSize.width * patchSize.height) - meanI; 956 957 for( y = -winSize.height, k = 0; y <= winSize.height; y++ ) 958 { 959 for( x = -winSize.width; x <= winSize.width; x++, k++ ) 960 { 961 double t = patchI[k] - patchJ[k] + meanJ; 962 double ixt = Ix[k] * t; 963 double iyt = Iy[k] * t; 964 965 s += t; 966 967 b[0] += ixt; 968 b[1] += iyt; 969 b[2] += x * ixt; 970 b[3] += y * ixt; 971 b[4] += x * iyt; 972 b[5] += y * iyt; 973 } 974 } 975 976 icvTransformVector_64d( G, b, eta, 6, 6 ); 977 978 Av[2] = (float)(Av[2] + Av[0] * eta[0] + Av[1] * eta[1]); 979 Av[5] = (float)(Av[5] + Av[3] * eta[0] + Av[4] * eta[1]); 980 981 t0 = Av[0] * (1 + eta[2]) + Av[1] * eta[4]; 982 t1 = Av[0] * eta[3] + Av[1] * (1 + eta[5]); 983 Av[0] = (float)t0; 984 Av[1] = (float)t1; 985 986 t0 = Av[3] * (1 + eta[2]) + Av[4] * eta[4]; 987 t1 = Av[3] * eta[3] + Av[4] * (1 + eta[5]); 988 Av[3] = (float)t0; 989 Av[4] = (float)t1; 990 991 if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon ) 992 break; 993 } 994 995 if( pt_status != 0 || l == 0 ) 996 { 997 status[i] = (char)pt_status; 998 featuresB[i].x = Av[2]; 999 featuresB[i].y = Av[5]; 1000 1001 matrices[i*4] = Av[0]; 1002 matrices[i*4+1] = Av[1]; 1003 matrices[i*4+2] = Av[3]; 1004 matrices[i*4+3] = Av[4]; 1005 } 1006 1007 if( pt_status && l == 0 && error ) 1008 { 1009 /* calc error */ 1010 double err = 0; 1011 1012 for( y = 0, k = 0; y < patchSize.height; y++ ) 1013 { 1014 for( x = 0; x < patchSize.width; x++, k++ ) 1015 { 1016 double t = patchI[k] - patchJ[k] + meanJ; 1017 err += t * t; 1018 } 1019 } 1020 error[i] = (float)sqrt(err); 1021 } 1022 } 1023 } 1024 1025 __END__; 1026 1027 cvFree( &pyr_buffer ); 1028 cvFree( &buffer ); 1029 cvFree( &_status ); 1030} 1031 1032 1033 1034static void 1035icvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b, 1036 int count, CvMat* M, int full_affine ) 1037{ 1038 if( full_affine ) 1039 { 1040 double sa[36], sb[6]; 1041 CvMat A = cvMat( 6, 6, CV_64F, sa ), B = cvMat( 6, 1, CV_64F, sb ); 1042 CvMat MM = cvMat( 6, 1, CV_64F, M->data.db ); 1043 1044 int i; 1045 1046 memset( sa, 0, sizeof(sa) ); 1047 memset( sb, 0, sizeof(sb) ); 1048 1049 for( i = 0; i < count; i++ ) 1050 { 1051 sa[0] += a[i].x*a[i].x; 1052 sa[1] += a[i].y*a[i].x; 1053 sa[2] += a[i].x; 1054 1055 sa[6] += a[i].x*a[i].y; 1056 sa[7] += a[i].y*a[i].y; 1057 sa[8] += a[i].y; 1058 1059 sa[12] += a[i].x; 1060 sa[13] += a[i].y; 1061 sa[14] += 1; 1062 1063 sb[0] += a[i].x*b[i].x; 1064 sb[1] += a[i].y*b[i].x; 1065 sb[2] += b[i].x; 1066 sb[3] += a[i].x*b[i].y; 1067 sb[4] += a[i].y*b[i].y; 1068 sb[5] += b[i].y; 1069 } 1070 1071 sa[21] = sa[0]; 1072 sa[22] = sa[1]; 1073 sa[23] = sa[2]; 1074 sa[27] = sa[6]; 1075 sa[28] = sa[7]; 1076 sa[29] = sa[8]; 1077 sa[33] = sa[12]; 1078 sa[34] = sa[13]; 1079 sa[35] = sa[14]; 1080 1081 cvSolve( &A, &B, &MM, CV_SVD ); 1082 } 1083 else 1084 { 1085 double sa[16], sb[4], m[4], *om = M->data.db; 1086 CvMat A = cvMat( 4, 4, CV_64F, sa ), B = cvMat( 4, 1, CV_64F, sb ); 1087 CvMat MM = cvMat( 4, 1, CV_64F, m ); 1088 1089 int i; 1090 1091 memset( sa, 0, sizeof(sa) ); 1092 memset( sb, 0, sizeof(sb) ); 1093 1094 for( i = 0; i < count; i++ ) 1095 { 1096 sa[0] += a[i].x*a[i].x + a[i].y*a[i].y; 1097 sa[1] += 0; 1098 sa[2] += a[i].x; 1099 sa[3] += a[i].y; 1100 1101 sa[4] += 0; 1102 sa[5] += a[i].x*a[i].x + a[i].y*a[i].y; 1103 sa[6] += -a[i].y; 1104 sa[7] += a[i].x; 1105 1106 sa[8] += a[i].x; 1107 sa[9] += -a[i].y; 1108 sa[10] += 1; 1109 sa[11] += 0; 1110 1111 sa[12] += a[i].y; 1112 sa[13] += a[i].x; 1113 sa[14] += 0; 1114 sa[15] += 1; 1115 1116 sb[0] += a[i].x*b[i].x + a[i].y*b[i].y; 1117 sb[1] += a[i].x*b[i].y - a[i].y*b[i].x; 1118 sb[2] += b[i].x; 1119 sb[3] += b[i].y; 1120 } 1121 1122 cvSolve( &A, &B, &MM, CV_SVD ); 1123 1124 om[0] = om[4] = m[0]; 1125 om[1] = -m[1]; 1126 om[3] = m[1]; 1127 om[2] = m[2]; 1128 om[5] = m[3]; 1129 } 1130} 1131 1132 1133CV_IMPL int 1134cvEstimateRigidTransform( const CvArr* _A, const CvArr* _B, CvMat* _M, int full_affine ) 1135{ 1136 int result = 0; 1137 1138 const int COUNT = 15; 1139 const int WIDTH = 160, HEIGHT = 120; 1140 const int RANSAC_MAX_ITERS = 100; 1141 const int RANSAC_SIZE0 = 3; 1142 const double MIN_TRIANGLE_SIDE = 20; 1143 const double RANSAC_GOOD_RATIO = 0.5; 1144 1145 int allocated = 1; 1146 CvMat *sA = 0, *sB = 0; 1147 CvPoint2D32f *pA = 0, *pB = 0; 1148 int* good_idx = 0; 1149 char *status = 0; 1150 CvMat* gray = 0; 1151 1152 CV_FUNCNAME( "cvEstimateRigidTransform" ); 1153 1154 __BEGIN__; 1155 1156 CvMat stubA, *A; 1157 CvMat stubB, *B; 1158 CvSize sz0, sz1; 1159 int cn, equal_sizes; 1160 int i, j, k, k1; 1161 int count_x, count_y, count; 1162 double scale = 1; 1163 CvRNG rng = cvRNG(-1); 1164 double m[6]={0}; 1165 CvMat M = cvMat( 2, 3, CV_64F, m ); 1166 int good_count = 0; 1167 1168 CV_CALL( A = cvGetMat( _A, &stubA )); 1169 CV_CALL( B = cvGetMat( _B, &stubB )); 1170 1171 if( !CV_IS_MAT(_M) ) 1172 CV_ERROR( _M ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" ); 1173 1174 if( !CV_ARE_SIZES_EQ( A, B ) ) 1175 CV_ERROR( CV_StsUnmatchedSizes, "Both input images must have the same size" ); 1176 1177 if( !CV_ARE_TYPES_EQ( A, B ) ) 1178 CV_ERROR( CV_StsUnmatchedFormats, "Both input images must have the same data type" ); 1179 1180 if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 ) 1181 { 1182 cn = CV_MAT_CN(A->type); 1183 sz0 = cvGetSize(A); 1184 sz1 = cvSize(WIDTH, HEIGHT); 1185 1186 scale = MAX( (double)sz1.width/sz0.width, (double)sz1.height/sz0.height ); 1187 scale = MIN( scale, 1. ); 1188 sz1.width = cvRound( sz0.width * scale ); 1189 sz1.height = cvRound( sz0.height * scale ); 1190 1191 equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height; 1192 1193 if( !equal_sizes || cn != 1 ) 1194 { 1195 CV_CALL( sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 )); 1196 CV_CALL( sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 )); 1197 1198 if( !equal_sizes && cn != 1 ) 1199 CV_CALL( gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 )); 1200 1201 if( gray ) 1202 { 1203 cvCvtColor( A, gray, CV_BGR2GRAY ); 1204 cvResize( gray, sA, CV_INTER_AREA ); 1205 cvCvtColor( B, gray, CV_BGR2GRAY ); 1206 cvResize( gray, sB, CV_INTER_AREA ); 1207 } 1208 else if( cn == 1 ) 1209 { 1210 cvResize( gray, sA, CV_INTER_AREA ); 1211 cvResize( gray, sB, CV_INTER_AREA ); 1212 } 1213 else 1214 { 1215 cvCvtColor( A, gray, CV_BGR2GRAY ); 1216 cvResize( gray, sA, CV_INTER_AREA ); 1217 cvCvtColor( B, gray, CV_BGR2GRAY ); 1218 } 1219 1220 cvReleaseMat( &gray ); 1221 A = sA; 1222 B = sB; 1223 } 1224 1225 count_y = COUNT; 1226 count_x = cvRound((double)COUNT*sz1.width/sz1.height); 1227 count = count_x * count_y; 1228 1229 CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) )); 1230 CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) )); 1231 CV_CALL( status = (char*)cvAlloc( count*sizeof(status[0]) )); 1232 1233 for( i = 0, k = 0; i < count_y; i++ ) 1234 for( j = 0; j < count_x; j++, k++ ) 1235 { 1236 pA[k].x = (j+0.5f)*sz1.width/count_x; 1237 pA[k].y = (i+0.5f)*sz1.height/count_y; 1238 } 1239 1240 // find the corresponding points in B 1241 cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3, 1242 status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 ); 1243 1244 // repack the remained points 1245 for( i = 0, k = 0; i < count; i++ ) 1246 if( status[i] ) 1247 { 1248 if( i > k ) 1249 { 1250 pA[k] = pA[i]; 1251 pB[k] = pB[i]; 1252 } 1253 k++; 1254 } 1255 1256 count = k; 1257 } 1258 else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 ) 1259 { 1260 count = A->cols*A->rows; 1261 1262 if( CV_IS_MAT_CONT(A->type & B->type) && CV_MAT_TYPE(A->type) == CV_32FC2 ) 1263 { 1264 pA = (CvPoint2D32f*)A->data.ptr; 1265 pB = (CvPoint2D32f*)B->data.ptr; 1266 allocated = 0; 1267 } 1268 else 1269 { 1270 CvMat _pA, _pB; 1271 1272 CV_CALL( pA = (CvPoint2D32f*)cvAlloc( count*sizeof(pA[0]) )); 1273 CV_CALL( pB = (CvPoint2D32f*)cvAlloc( count*sizeof(pB[0]) )); 1274 _pA = cvMat( A->rows, A->cols, CV_32FC2, pA ); 1275 _pB = cvMat( B->rows, B->cols, CV_32FC2, pB ); 1276 cvConvert( A, &_pA ); 1277 cvConvert( B, &_pB ); 1278 } 1279 } 1280 else 1281 CV_ERROR( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" ); 1282 1283 CV_CALL( good_idx = (int*)cvAlloc( count*sizeof(good_idx[0]) )); 1284 1285 if( count < RANSAC_SIZE0 ) 1286 EXIT; 1287 1288 // RANSAC stuff: 1289 // 1. find the consensus 1290 for( k = 0; k < RANSAC_MAX_ITERS; k++ ) 1291 { 1292 int idx[RANSAC_SIZE0]; 1293 CvPoint2D32f a[3]; 1294 CvPoint2D32f b[3]; 1295 1296 memset( a, 0, sizeof(a) ); 1297 memset( b, 0, sizeof(b) ); 1298 1299 // choose random 3 non-complanar points from A & B 1300 for( i = 0; i < RANSAC_SIZE0; i++ ) 1301 { 1302 for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ ) 1303 { 1304 idx[i] = cvRandInt(&rng) % count; 1305 1306 for( j = 0; j < i; j++ ) 1307 { 1308 if( idx[j] == idx[i] ) 1309 break; 1310 // check that the points are not very close one each other 1311 if( fabs(pA[idx[i]].x - pA[idx[j]].x) + 1312 fabs(pA[idx[i]].y - pA[idx[j]].y) < MIN_TRIANGLE_SIDE ) 1313 break; 1314 if( fabs(pB[idx[i]].x - pB[idx[j]].x) + 1315 fabs(pB[idx[i]].y - pB[idx[j]].y) < MIN_TRIANGLE_SIDE ) 1316 break; 1317 } 1318 1319 if( j < i ) 1320 continue; 1321 1322 if( i+1 == RANSAC_SIZE0 ) 1323 { 1324 // additional check for non-complanar vectors 1325 a[0] = pA[idx[0]]; 1326 a[1] = pA[idx[1]]; 1327 a[2] = pA[idx[2]]; 1328 1329 b[0] = pB[idx[0]]; 1330 b[1] = pB[idx[1]]; 1331 b[2] = pB[idx[2]]; 1332 1333 if( fabs((a[1].x - a[0].x)*(a[2].y - a[0].y) - (a[1].y - a[0].y)*(a[2].x - a[0].x)) < 1 || 1334 fabs((b[1].x - b[0].x)*(b[2].y - b[0].y) - (b[1].y - b[0].y)*(b[2].x - b[0].x)) < 1 ) 1335 continue; 1336 } 1337 break; 1338 } 1339 1340 if( k1 >= RANSAC_MAX_ITERS ) 1341 break; 1342 } 1343 1344 if( i < RANSAC_SIZE0 ) 1345 continue; 1346 1347 // estimate the transformation using 3 points 1348 icvGetRTMatrix( a, b, 3, &M, full_affine ); 1349 1350 for( i = 0, good_count = 0; i < count; i++ ) 1351 { 1352 if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) + 1353 fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < 8 ) 1354 good_idx[good_count++] = i; 1355 } 1356 1357 if( good_count >= count*RANSAC_GOOD_RATIO ) 1358 break; 1359 } 1360 1361 if( k >= RANSAC_MAX_ITERS ) 1362 EXIT; 1363 1364 if( good_count < count ) 1365 { 1366 for( i = 0; i < good_count; i++ ) 1367 { 1368 j = good_idx[i]; 1369 pA[i] = pA[j]; 1370 pB[i] = pB[j]; 1371 } 1372 } 1373 1374 icvGetRTMatrix( pA, pB, good_count, &M, full_affine ); 1375 m[2] /= scale; 1376 m[5] /= scale; 1377 CV_CALL( cvConvert( &M, _M )); 1378 result = 1; 1379 1380 __END__; 1381 1382 cvReleaseMat( &sA ); 1383 cvReleaseMat( &sB ); 1384 cvFree( &pA ); 1385 cvFree( &pB ); 1386 cvFree( &status ); 1387 cvFree( &good_idx ); 1388 cvReleaseMat( &gray ); 1389 1390 return result; 1391} 1392 1393 1394/* End of file. */ 1395