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
45 static void
intersect(CvPoint2D32f pt,CvSize win_size,CvSize imgSize,CvPoint * min_pt,CvPoint * max_pt)46 intersect( 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
icvMinimalPyramidSize(CvSize imgSize)67 static int icvMinimalPyramidSize( CvSize imgSize )
68 {
69 return cvAlign(imgSize.width,8) * imgSize.height / 3;
70 }
71
72
73 static void
icvInitPyramidalAlgorithm(const CvMat * imgA,const CvMat * imgB,CvMat * pyrA,CvMat * pyrB,int level,CvTermCriteria * criteria,int max_iters,int flags,uchar *** imgI,uchar *** imgJ,int ** step,CvSize ** size,double ** scale,uchar ** buffer)74 icvInitPyramidalAlgorithm( 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 */
225 static void
icvCalcIxIy_32f(const float * src,int src_step,float * dstX,float * dstY,int dst_step,CvSize src_size,const float * smooth_k,float * buffer0)226 icvCalcIxIy_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
258 icvOpticalFlowPyrLKInitAlloc_8u_C1R_t icvOpticalFlowPyrLKInitAlloc_8u_C1R_p = 0;
259 icvOpticalFlowPyrLKFree_8u_C1R_t icvOpticalFlowPyrLKFree_8u_C1R_p = 0;
260 icvOpticalFlowPyrLK_8u_C1R_t icvOpticalFlowPyrLK_8u_C1R_p = 0;
261
262
263 CV_IMPL void
cvCalcOpticalFlowPyrLK(const void * arrA,const void * arrB,void * pyrarrA,void * pyrarrB,const CvPoint2D32f * featuresA,CvPoint2D32f * featuresB,int count,CvSize winSize,int level,char * status,float * error,CvTermCriteria criteria,int flags)264 cvCalcOpticalFlowPyrLK( 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
654 CV_IMPL void
cvCalcAffineFlowPyrLK(const void * arrA,const void * arrB,void * pyrarrA,void * pyrarrB,const CvPoint2D32f * featuresA,CvPoint2D32f * featuresB,float * matrices,int count,CvSize winSize,int level,char * status,float * error,CvTermCriteria criteria,int flags)655 cvCalcAffineFlowPyrLK( 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
1034 static void
icvGetRTMatrix(const CvPoint2D32f * a,const CvPoint2D32f * b,int count,CvMat * M,int full_affine)1035 icvGetRTMatrix( 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
1133 CV_IMPL int
cvEstimateRigidTransform(const CvArr * _A,const CvArr * _B,CvMat * _M,int full_affine)1134 cvEstimateRigidTransform( 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