1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
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.
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.
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.
48 void calcOpticalFlowPyrLK( const Mat& prevImg, const Mat& nextImg,
49 const vector<Point2f>& prevPts,
50 vector<Point2f>& nextPts,
51 vector<uchar>& status, vector<float>& err,
52 Size winSize, int maxLevel,
53 TermCriteria criteria,
57 derivLambda = std::min(std::max(derivLambda, 0.), 1.);
58 double lambda1 = 1. - derivLambda, lambda2 = derivLambda;
59 const int derivKernelSize = 3;
60 const float deriv1Scale = 0.5f/4.f;
61 const float deriv2Scale = 0.25f/4.f;
62 const int derivDepth = CV_32F;
63 Point2f halfWin((winSize.width-1)*0.5f, (winSize.height-1)*0.5f);
65 CV_Assert( maxLevel >= 0 && winSize.width > 2 && winSize.height > 2 );
66 CV_Assert( prevImg.size() == nextImg.size() &&
67 prevImg.type() == nextImg.type() );
69 size_t npoints = prevPts.size();
70 nextPts.resize(npoints);
71 status.resize(npoints);
72 for( size_t i = 0; i < npoints; i++ )
79 vector<Mat> prevPyr, nextPyr;
81 int cn = prevImg.channels();
82 buildPyramid( prevImg, prevPyr, maxLevel );
83 buildPyramid( nextImg, nextPyr, maxLevel );
84 // I, dI/dx ~ Ix, dI/dy ~ Iy, d2I/dx2 ~ Ixx, d2I/dxdy ~ Ixy, d2I/dy2 ~ Iyy
85 Mat derivIBuf((prevImg.rows + winSize.height*2),
86 (prevImg.cols + winSize.width*2),
87 CV_MAKETYPE(derivDepth, cn*6));
88 // J, dJ/dx ~ Jx, dJ/dy ~ Jy
89 Mat derivJBuf((prevImg.rows + winSize.height*2),
90 (prevImg.cols + winSize.width*2),
91 CV_MAKETYPE(derivDepth, cn*3));
92 Mat tempDerivBuf(prevImg.size(), CV_MAKETYPE(derivIBuf.type(), cn));
93 Mat derivIWinBuf(winSize, derivIBuf.type());
95 if( (criteria.type & TermCriteria::COUNT) == 0 )
96 criteria.maxCount = 30;
98 criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
99 if( (criteria.type & TermCriteria::EPS) == 0 )
100 criteria.epsilon = 0.01;
102 criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
103 criteria.epsilon *= criteria.epsilon;
105 for( int level = maxLevel; level >= 0; level-- )
108 Size imgSize = prevPyr[level].size();
109 Mat tempDeriv( imgSize, tempDerivBuf.type(), tempDerivBuf.data );
110 Mat _derivI( imgSize.height + winSize.height*2,
111 imgSize.width + winSize.width*2,
112 derivIBuf.type(), derivIBuf.data );
113 Mat _derivJ( imgSize.height + winSize.height*2,
114 imgSize.width + winSize.width*2,
115 derivJBuf.type(), derivJBuf.data );
116 Mat derivI(_derivI, Rect(winSize.width, winSize.height, imgSize.width, imgSize.height));
117 Mat derivJ(_derivJ, Rect(winSize.width, winSize.height, imgSize.width, imgSize.height));
118 CvMat cvderivI = _derivI;
120 CvMat cvderivJ = _derivJ;
123 vector<int> fromTo(cn*2);
124 for( k = 0; k < cn; k++ )
127 prevPyr[level].convertTo(tempDeriv, derivDepth);
128 for( k = 0; k < cn; k++ )
130 mixChannels(&tempDeriv, 1, &derivI, 1, &fromTo[0], cn);
132 // compute spatial derivatives and merge them together
133 Sobel(prevPyr[level], tempDeriv, derivDepth, 1, 0, derivKernelSize, deriv1Scale );
134 for( k = 0; k < cn; k++ )
135 fromTo[k*2+1] = k*6 + 1;
136 mixChannels(&tempDeriv, 1, &derivI, 1, &fromTo[0], cn);
138 Sobel(prevPyr[level], tempDeriv, derivDepth, 0, 1, derivKernelSize, deriv1Scale );
139 for( k = 0; k < cn; k++ )
140 fromTo[k*2+1] = k*6 + 2;
141 mixChannels(&tempDeriv, 1, &derivI, 1, &fromTo[0], cn);
143 Sobel(prevPyr[level], tempDeriv, derivDepth, 2, 0, derivKernelSize, deriv2Scale );
144 for( k = 0; k < cn; k++ )
145 fromTo[k*2+1] = k*6 + 3;
146 mixChannels(&tempDeriv, 1, &derivI, 1, &fromTo[0], cn);
148 Sobel(prevPyr[level], tempDeriv, derivDepth, 1, 1, derivKernelSize, deriv2Scale );
149 for( k = 0; k < cn; k++ )
150 fromTo[k*2+1] = k*6 + 4;
151 mixChannels(&tempDeriv, 1, &derivI, 1, &fromTo[0], cn);
153 Sobel(prevPyr[level], tempDeriv, derivDepth, 0, 2, derivKernelSize, deriv2Scale );
154 for( k = 0; k < cn; k++ )
155 fromTo[k*2+1] = k*6 + 5;
156 mixChannels(&tempDeriv, 1, &derivI, 1, &fromTo[0], cn);
158 nextPyr[level].convertTo(tempDeriv, derivDepth);
159 for( k = 0; k < cn; k++ )
161 mixChannels(&tempDeriv, 1, &derivJ, 1, &fromTo[0], cn);
163 Sobel(nextPyr[level], tempDeriv, derivDepth, 1, 0, derivKernelSize, deriv1Scale );
164 for( k = 0; k < cn; k++ )
165 fromTo[k*2+1] = k*3 + 1;
166 mixChannels(&tempDeriv, 1, &derivJ, 1, &fromTo[0], cn);
168 Sobel(nextPyr[level], tempDeriv, derivDepth, 0, 1, derivKernelSize, deriv1Scale );
169 for( k = 0; k < cn; k++ )
170 fromTo[k*2+1] = k*3 + 2;
171 mixChannels(&tempDeriv, 1, &derivJ, 1, &fromTo[0], cn);
173 /*copyMakeBorder( derivI, _derivI, winSize.height, winSize.height,
174 winSize.width, winSize.width, BORDER_CONSTANT );
175 copyMakeBorder( derivJ, _derivJ, winSize.height, winSize.height,
176 winSize.width, winSize.width, BORDER_CONSTANT );*/
178 for( size_t ptidx = 0; ptidx < npoints; ptidx++ )
180 Point2f prevPt = prevPts[ptidx]*(float)(1./(1 << level));
182 if( level == maxLevel )
184 if( flags & OPTFLOW_USE_INITIAL_FLOW )
185 nextPt = nextPts[ptidx]*(float)(1./(1 << level));
190 nextPt = nextPts[ptidx]*2.f;
191 nextPts[ptidx] = nextPt;
193 Point2i iprevPt, inextPt;
195 iprevPt.x = cvFloor(prevPt.x);
196 iprevPt.y = cvFloor(prevPt.y);
198 if( iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
199 iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows )
203 status[ptidx] = false;
204 err[ptidx] = FLT_MAX;
209 float a = prevPt.x - iprevPt.x;
210 float b = prevPt.y - iprevPt.y;
211 float w00 = (1.f - a)*(1.f - b), w01 = a*(1.f - b);
212 float w10 = (1.f - a)*b, w11 = a*b;
213 size_t stepI = derivI.step/derivI.elemSize1();
214 size_t stepJ = derivJ.step/derivJ.elemSize1();
215 int cnI = cn*6, cnJ = cn*3;
216 double A11 = 0, A12 = 0, A22 = 0;
217 double iA11 = 0, iA12 = 0, iA22 = 0;
219 // extract the patch from the first image
221 for( y = 0; y < winSize.height; y++ )
223 const float* src = (const float*)(derivI.data +
224 (y + iprevPt.y)*derivI.step) + iprevPt.x*cnI;
225 float* dst = (float*)(derivIWinBuf.data + y*derivIWinBuf.step);
227 for( x = 0; x < winSize.width*cnI; x += cnI, src += cnI )
229 float I = src[0]*w00 + src[cnI]*w01 + src[stepI]*w10 + src[stepI+cnI]*w11;
232 float Ix = src[1]*w00 + src[cnI+1]*w01 + src[stepI+1]*w10 + src[stepI+cnI+1]*w11;
233 float Iy = src[2]*w00 + src[cnI+2]*w01 + src[stepI+2]*w10 + src[stepI+cnI+2]*w11;
234 dst[x+1] = Ix; dst[x+2] = Iy;
236 float Ixx = src[3]*w00 + src[cnI+3]*w01 + src[stepI+3]*w10 + src[stepI+cnI+3]*w11;
237 float Ixy = src[4]*w00 + src[cnI+4]*w01 + src[stepI+4]*w10 + src[stepI+cnI+4]*w11;
238 float Iyy = src[5]*w00 + src[cnI+5]*w01 + src[stepI+5]*w10 + src[stepI+cnI+5]*w11;
239 dst[x+3] = Ixx; dst[x+4] = Ixy; dst[x+5] = Iyy;
241 iA11 += (double)Ix*Ix;
242 iA12 += (double)Ix*Iy;
243 iA22 += (double)Iy*Iy;
245 A11 += (double)Ixx*Ixx + (double)Ixy*Ixy;
246 A12 += Ixy*((double)Ixx + Iyy);
247 A22 += (double)Ixy*Ixy + (double)Iyy*Iyy;
251 A11 = lambda1*iA11 + lambda2*A11;
252 A12 = lambda1*iA12 + lambda2*A12;
253 A22 = lambda1*iA22 + lambda2*A22;
255 double D = A11*A22 - A12*A12;
256 double minEig = (A22 + A11 - std::sqrt((A11-A22)*(A11-A22) +
257 4.*A12*A12))/(2*winSize.width*winSize.height);
258 err[ptidx] = (float)minEig;
260 if( D < DBL_EPSILON )
263 status[ptidx] = false;
272 for( int j = 0; j < criteria.maxCount; j++ )
274 inextPt.x = cvFloor(nextPt.x);
275 inextPt.y = cvFloor(nextPt.y);
277 if( inextPt.x < -winSize.width || inextPt.x >= derivJ.cols ||
278 inextPt.y < -winSize.height || inextPt.y >= derivJ.rows )
281 status[ptidx] = false;
285 a = nextPt.x - inextPt.x;
286 b = nextPt.y - inextPt.y;
287 w00 = (1.f - a)*(1.f - b); w01 = a*(1.f - b);
288 w10 = (1.f - a)*b; w11 = a*b;
290 double b1 = 0, b2 = 0, ib1 = 0, ib2 = 0;
292 for( y = 0; y < winSize.height; y++ )
294 const float* src = (const float*)(derivJ.data +
295 (y + inextPt.y)*derivJ.step) + inextPt.x*cnJ;
296 const float* Ibuf = (float*)(derivIWinBuf.data + y*derivIWinBuf.step);
298 for( x = 0; x < winSize.width; x++, src += cnJ, Ibuf += cnI )
300 double It = src[0]*w00 + src[cnJ]*w01 + src[stepJ]*w10 +
301 src[stepJ+cnJ]*w11 - Ibuf[0];
302 double Ixt = src[1]*w00 + src[cnJ+1]*w01 + src[stepJ+1]*w10 +
303 src[stepJ+cnJ+1]*w11 - Ibuf[1];
304 double Iyt = src[2]*w00 + src[cnJ+2]*w01 + src[stepJ+2]*w10 +
305 src[stepJ+cnJ+2]*w11 - Ibuf[2];
306 b1 += Ixt*Ibuf[3] + Iyt*Ibuf[4];
307 b2 += Ixt*Ibuf[4] + Iyt*Ibuf[5];
313 b1 = lambda1*ib1 + lambda2*b1;
314 b2 = lambda1*ib2 + lambda2*b2;
315 Point2f delta( (float)((A12*b2 - A22*b1) * D),
316 (float)((A12*b1 - A11*b2) * D));
320 nextPts[ptidx] = nextPt + halfWin;
322 if( delta.ddot(delta) <= criteria.epsilon )
325 if( j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
326 std::abs(delta.y + prevDelta.y) < 0.01 )
328 nextPts[ptidx] -= delta*0.5f;
340 intersect( CvPoint2D32f pt, CvSize win_size, CvSize imgSize,
341 CvPoint* min_pt, CvPoint* max_pt )
345 ipt.x = cvFloor( pt.x );
346 ipt.y = cvFloor( pt.y );
348 ipt.x -= win_size.width;
349 ipt.y -= win_size.height;
351 win_size.width = win_size.width * 2 + 1;
352 win_size.height = win_size.height * 2 + 1;
354 min_pt->x = MAX( 0, -ipt.x );
355 min_pt->y = MAX( 0, -ipt.y );
356 max_pt->x = MIN( win_size.width, imgSize.width - ipt.x );
357 max_pt->y = MIN( win_size.height, imgSize.height - ipt.y );
361 static int icvMinimalPyramidSize( CvSize imgSize )
363 return cvAlign(imgSize.width,8) * imgSize.height / 3;
368 icvInitPyramidalAlgorithm( const CvMat* imgA, const CvMat* imgB,
369 CvMat* pyrA, CvMat* pyrB,
370 int level, CvTermCriteria * criteria,
371 int max_iters, int flags,
372 uchar *** imgI, uchar *** imgJ,
373 int **step, CvSize** size,
374 double **scale, cv::AutoBuffer<uchar>* buffer )
377 int pyrBytes, bufferBytes = 0, elem_size;
378 int level1 = level + 1;
381 CvSize imgSize, levelSize;
388 /* check input arguments */
389 if( ((flags & CV_LKFLOW_PYR_A_READY) != 0 && !pyrA) ||
390 ((flags & CV_LKFLOW_PYR_B_READY) != 0 && !pyrB) )
391 CV_Error( CV_StsNullPtr, "Some of the precomputed pyramids are missing" );
394 CV_Error( CV_StsOutOfRange, "The number of pyramid levels is negative" );
396 switch( criteria->type )
398 case CV_TERMCRIT_ITER:
399 criteria->epsilon = 0.f;
401 case CV_TERMCRIT_EPS:
402 criteria->max_iter = max_iters;
404 case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS:
408 CV_Error( CV_StsBadArg, "Invalid termination criteria" );
411 /* compare squared values */
412 criteria->epsilon *= criteria->epsilon;
414 /* set pointers and step for every level */
417 imgSize = cvGetSize(imgA);
418 elem_size = CV_ELEM_SIZE(imgA->type);
421 for( i = 1; i < level1; i++ )
423 levelSize.width = (levelSize.width + 1) >> 1;
424 levelSize.height = (levelSize.height + 1) >> 1;
426 int tstep = cvAlign(levelSize.width,ALIGN) * elem_size;
427 pyrBytes += tstep * levelSize.height;
430 assert( pyrBytes <= imgSize.width * imgSize.height * elem_size * 4 / 3 );
432 /* buffer_size = <size for patches> + <size for pyramids> */
433 bufferBytes = (int)((level1 >= 0) * ((pyrA->data.ptr == 0) +
434 (pyrB->data.ptr == 0)) * pyrBytes +
435 (sizeof(imgI[0][0]) * 2 + sizeof(step[0][0]) +
436 sizeof(size[0][0]) + sizeof(scale[0][0])) * level1);
438 buffer->allocate( bufferBytes );
440 *imgI = (uchar **) (uchar*)(*buffer);
441 *imgJ = *imgI + level1;
442 *step = (int *) (*imgJ + level1);
443 *scale = (double *) (*step + level1);
444 *size = (CvSize *)(*scale + level1);
446 imgI[0][0] = imgA->data.ptr;
447 imgJ[0][0] = imgB->data.ptr;
448 step[0][0] = imgA->step;
450 size[0][0] = imgSize;
454 uchar *bufPtr = (uchar *) (*size + level1);
455 uchar *ptrA = pyrA->data.ptr;
456 uchar *ptrB = pyrB->data.ptr;
469 /* build pyramids for both frames */
470 for( i = 1; i <= level; i++ )
473 CvMat prev_level, next_level;
475 levelSize.width = (levelSize.width + 1) >> 1;
476 levelSize.height = (levelSize.height + 1) >> 1;
478 size[0][i] = levelSize;
479 step[0][i] = cvAlign( levelSize.width, ALIGN ) * elem_size;
480 scale[0][i] = scale[0][i - 1] * 0.5;
482 levelBytes = step[0][i] * levelSize.height;
483 imgI[0][i] = (uchar *) ptrA;
486 if( !(flags & CV_LKFLOW_PYR_A_READY) )
488 prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 );
489 next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 );
490 cvSetData( &prev_level, imgI[0][i-1], step[0][i-1] );
491 cvSetData( &next_level, imgI[0][i], step[0][i] );
492 cvPyrDown( &prev_level, &next_level );
495 imgJ[0][i] = (uchar *) ptrB;
498 if( !(flags & CV_LKFLOW_PYR_B_READY) )
500 prev_level = cvMat( size[0][i-1].height, size[0][i-1].width, CV_8UC1 );
501 next_level = cvMat( size[0][i].height, size[0][i].width, CV_8UC1 );
502 cvSetData( &prev_level, imgJ[0][i-1], step[0][i-1] );
503 cvSetData( &next_level, imgJ[0][i], step[0][i] );
504 cvPyrDown( &prev_level, &next_level );
511 /* compute dI/dx and dI/dy */
513 icvCalcIxIy_32f( const float* src, int src_step, float* dstX, float* dstY, int dst_step,
514 CvSize src_size, const float* smooth_k, float* buffer0 )
516 int src_width = src_size.width, dst_width = src_size.width-2;
517 int x, height = src_size.height - 2;
518 float* buffer1 = buffer0 + src_width;
520 src_step /= sizeof(src[0]);
521 dst_step /= sizeof(dstX[0]);
523 for( ; height--; src += src_step, dstX += dst_step, dstY += dst_step )
525 const float* src2 = src + src_step;
526 const float* src3 = src + src_step*2;
528 for( x = 0; x < src_width; x++ )
530 float t0 = (src3[x] + src[x])*smooth_k[0] + src2[x]*smooth_k[1];
531 float t1 = src3[x] - src[x];
532 buffer0[x] = t0; buffer1[x] = t1;
535 for( x = 0; x < dst_width; x++ )
537 float t0 = buffer0[x+2] - buffer0[x];
538 float t1 = (buffer1[x] + buffer1[x+2])*smooth_k[0] + buffer1[x+1]*smooth_k[1];
539 dstX[x] = t0; dstY[x] = t1;
548 struct LKTrackerInvoker
550 LKTrackerInvoker( const CvMat* _imgI, const CvMat* _imgJ,
551 const CvPoint2D32f* _featuresA,
552 CvPoint2D32f* _featuresB,
553 char* _status, float* _error,
554 CvTermCriteria _criteria,
555 CvSize _winSize, int _level, int _flags )
559 featuresA = _featuresA;
560 featuresB = _featuresB;
563 criteria = _criteria;
569 void operator()(const BlockedRange& range) const
571 static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; // 3/32, 10/32, 3/32
573 int i, i1 = range.begin(), i2 = range.end();
575 CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
576 int patchLen = patchSize.width * patchSize.height;
577 int srcPatchLen = (patchSize.width + 2)*(patchSize.height + 2);
579 AutoBuffer<float> buf(patchLen*3 + srcPatchLen);
581 float* patchJ = patchI + srcPatchLen;
582 float* Ix = patchJ + patchLen;
583 float* Iy = Ix + patchLen;
584 float scaleL = 1.f/(1 << level);
585 CvSize levelSize = cvGetMatSize(imgI);
587 // find flow for each given point
588 for( i = i1; i < i2; i++ )
591 CvPoint minI, maxI, minJ, maxJ;
595 CvPoint prev_minJ = { -1, -1 }, prev_maxJ = { -1, -1 };
596 double Gxx = 0, Gxy = 0, Gyy = 0, D = 0, minEig = 0;
597 float prev_mx = 0, prev_my = 0;
600 v.x = featuresB[i].x*2;
601 v.y = featuresB[i].y*2;
603 pt_status = status[i];
607 minI = maxI = minJ = maxJ = cvPoint(0, 0);
609 u.x = featuresA[i].x * scaleL;
610 u.y = featuresA[i].y * scaleL;
612 intersect( u, winSize, levelSize, &minI, &maxI );
613 isz = jsz = cvSize(maxI.x - minI.x + 2, maxI.y - minI.y + 2);
614 u.x += (minI.x - (patchSize.width - maxI.x + 1))*0.5f;
615 u.y += (minI.y - (patchSize.height - maxI.y + 1))*0.5f;
617 if( isz.width < 3 || isz.height < 3 ||
618 icvGetRectSubPix_8u32f_C1R( imgI->data.ptr, imgI->step, levelSize,
619 patchI, isz.width*sizeof(patchI[0]), isz, u ) < 0 )
621 // point is outside the first image. take the next
626 icvCalcIxIy_32f( patchI, isz.width*sizeof(patchI[0]), Ix, Iy,
627 (isz.width-2)*sizeof(patchI[0]), isz, smoothKernel, patchJ );
629 for( j = 0; j < criteria.max_iter; j++ )
631 double bx = 0, by = 0;
635 intersect( v, winSize, levelSize, &minJ, &maxJ );
637 minJ.x = MAX( minJ.x, minI.x );
638 minJ.y = MAX( minJ.y, minI.y );
640 maxJ.x = MIN( maxJ.x, maxI.x );
641 maxJ.y = MIN( maxJ.y, maxI.y );
643 jsz = cvSize(maxJ.x - minJ.x, maxJ.y - minJ.y);
645 _v.x = v.x + (minJ.x - (patchSize.width - maxJ.x + 1))*0.5f;
646 _v.y = v.y + (minJ.y - (patchSize.height - maxJ.y + 1))*0.5f;
648 if( jsz.width < 1 || jsz.height < 1 ||
649 icvGetRectSubPix_8u32f_C1R( imgJ->data.ptr, imgJ->step, levelSize, patchJ,
650 jsz.width*sizeof(patchJ[0]), jsz, _v ) < 0 )
652 // point is outside of the second image. take the next
657 if( maxJ.x == prev_maxJ.x && maxJ.y == prev_maxJ.y &&
658 minJ.x == prev_minJ.x && minJ.y == prev_minJ.y )
660 for( y = 0; y < jsz.height; y++ )
662 const float* pi = patchI +
663 (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
664 const float* pj = patchJ + y*jsz.width;
665 const float* ix = Ix +
666 (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x;
667 const float* iy = Iy + (ix - Ix);
669 for( x = 0; x < jsz.width; x++ )
671 double t0 = pi[x] - pj[x];
680 for( y = 0; y < jsz.height; y++ )
682 const float* pi = patchI +
683 (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
684 const float* pj = patchJ + y*jsz.width;
685 const float* ix = Ix +
686 (y + minJ.y - minI.y)*(isz.width-2) + minJ.x - minI.x;
687 const float* iy = Iy + (ix - Ix);
689 for( x = 0; x < jsz.width; x++ )
691 double t = pi[x] - pj[x];
692 bx += (double) (t * ix[x]);
693 by += (double) (t * iy[x]);
694 Gxx += ix[x] * ix[x];
695 Gxy += ix[x] * iy[x];
696 Gyy += iy[x] * iy[x];
700 D = Gxx * Gyy - Gxy * Gxy;
701 if( D < DBL_EPSILON )
707 // Adi Shavit - 2008.05
708 if( flags & CV_LKFLOW_GET_MIN_EIGENVALS )
709 minEig = (Gyy + Gxx - sqrt((Gxx-Gyy)*(Gxx-Gyy) + 4.*Gxy*Gxy))/(2*jsz.height*jsz.width);
717 mx = (float) ((Gyy * bx - Gxy * by) * D);
718 my = (float) ((Gxx * by - Gxy * bx) * D);
723 if( mx * mx + my * my < criteria.epsilon )
726 if( j > 0 && fabs(mx + prev_mx) < 0.01 && fabs(my + prev_my) < 0.01 )
737 status[i] = (char)pt_status;
738 if( level == 0 && error && pt_status )
742 if( flags & CV_LKFLOW_GET_MIN_EIGENVALS )
746 for( y = 0; y < jsz.height; y++ )
748 const float* pi = patchI +
749 (y + minJ.y - minI.y + 1)*isz.width + minJ.x - minI.x + 1;
750 const float* pj = patchJ + y*jsz.width;
752 for( x = 0; x < jsz.width; x++ )
754 double t = pi[x] - pj[x];
760 error[i] = (float)err;
762 } // end of point processing loop (i)
767 const CvPoint2D32f* featuresA;
768 CvPoint2D32f* featuresB;
771 CvTermCriteria criteria;
782 cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB,
783 void* pyrarrA, void* pyrarrB,
784 const CvPoint2D32f * featuresA,
785 CvPoint2D32f * featuresB,
786 int count, CvSize winSize, int level,
787 char *status, float *error,
788 CvTermCriteria criteria, int flags )
790 cv::AutoBuffer<uchar> pyrBuffer;
791 cv::AutoBuffer<uchar> buffer;
792 cv::AutoBuffer<char> _status;
794 const int MAX_ITERS = 100;
796 CvMat stubA, *imgA = (CvMat*)arrA;
797 CvMat stubB, *imgB = (CvMat*)arrB;
798 CvMat pstubA, *pyrA = (CvMat*)pyrarrA;
799 CvMat pstubB, *pyrB = (CvMat*)pyrarrB;
810 imgA = cvGetMat( imgA, &stubA );
811 imgB = cvGetMat( imgB, &stubB );
813 if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 )
814 CV_Error( CV_StsUnsupportedFormat, "" );
816 if( !CV_ARE_TYPES_EQ( imgA, imgB ))
817 CV_Error( CV_StsUnmatchedFormats, "" );
819 if( !CV_ARE_SIZES_EQ( imgA, imgB ))
820 CV_Error( CV_StsUnmatchedSizes, "" );
822 if( imgA->step != imgB->step )
823 CV_Error( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" );
825 imgSize = cvGetMatSize( imgA );
829 pyrA = cvGetMat( pyrA, &pstubA );
831 if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) )
832 CV_Error( CV_StsBadArg, "pyramid A has insufficient size" );
842 pyrB = cvGetMat( pyrB, &pstubB );
844 if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) )
845 CV_Error( CV_StsBadArg, "pyramid B has insufficient size" );
856 if( !featuresA || !featuresB )
857 CV_Error( CV_StsNullPtr, "Some of arrays of point coordinates are missing" );
860 CV_Error( CV_StsOutOfRange, "The number of tracked points is negative or zero" );
862 if( winSize.width <= 1 || winSize.height <= 1 )
863 CV_Error( CV_StsBadSize, "Invalid search window size" );
865 icvInitPyramidalAlgorithm( imgA, imgB, pyrA, pyrB,
866 level, &criteria, MAX_ITERS, flags,
867 &imgI, &imgJ, &step, &size, &scale, &pyrBuffer );
871 _status.allocate(count);
875 memset( status, 1, count );
877 memset( error, 0, count*sizeof(error[0]) );
879 if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
880 memcpy( featuresB, featuresA, count*sizeof(featuresA[0]));
882 for( i = 0; i < count; i++ )
884 featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5);
885 featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5);
888 /* do processing from top pyramid level (smallest image)
889 to the bottom (original image) */
890 for( l = level; l >= 0; l-- )
892 CvMat imgI_l, imgJ_l;
893 cvInitMatHeader(&imgI_l, size[l].height, size[l].width, imgA->type, imgI[l], step[l]);
894 cvInitMatHeader(&imgJ_l, size[l].height, size[l].width, imgB->type, imgJ[l], step[l]);
896 cv::parallel_for(cv::BlockedRange(0, count),
897 cv::LKTrackerInvoker(&imgI_l, &imgJ_l, featuresA,
898 featuresB, status, error,
899 criteria, winSize, l, flags));
900 } // end of pyramid levels loop (l)
904 /* Affine tracking algorithm */
907 cvCalcAffineFlowPyrLK( const void* arrA, const void* arrB,
908 void* pyrarrA, void* pyrarrB,
909 const CvPoint2D32f * featuresA,
910 CvPoint2D32f * featuresB,
911 float *matrices, int count,
912 CvSize winSize, int level,
913 char *status, float *error,
914 CvTermCriteria criteria, int flags )
916 const int MAX_ITERS = 100;
918 cv::AutoBuffer<char> _status;
919 cv::AutoBuffer<uchar> buffer;
920 cv::AutoBuffer<uchar> pyr_buffer;
922 CvMat stubA, *imgA = (CvMat*)arrA;
923 CvMat stubB, *imgB = (CvMat*)arrB;
924 CvMat pstubA, *pyrA = (CvMat*)pyrarrA;
925 CvMat pstubB, *pyrB = (CvMat*)pyrarrB;
927 static const float smoothKernel[] = { 0.09375, 0.3125, 0.09375 }; /* 3/32, 10/32, 3/32 */
944 CvSize patchSize = cvSize( winSize.width * 2 + 1, winSize.height * 2 + 1 );
945 int patchLen = patchSize.width * patchSize.height;
946 int patchStep = patchSize.width * sizeof( patchI[0] );
948 CvSize srcPatchSize = cvSize( patchSize.width + 2, patchSize.height + 2 );
949 int srcPatchLen = srcPatchSize.width * srcPatchSize.height;
950 int srcPatchStep = srcPatchSize.width * sizeof( patchI[0] );
952 float eps = (float)MIN(winSize.width, winSize.height);
954 imgA = cvGetMat( imgA, &stubA );
955 imgB = cvGetMat( imgB, &stubB );
957 if( CV_MAT_TYPE( imgA->type ) != CV_8UC1 )
958 CV_Error( CV_StsUnsupportedFormat, "" );
960 if( !CV_ARE_TYPES_EQ( imgA, imgB ))
961 CV_Error( CV_StsUnmatchedFormats, "" );
963 if( !CV_ARE_SIZES_EQ( imgA, imgB ))
964 CV_Error( CV_StsUnmatchedSizes, "" );
966 if( imgA->step != imgB->step )
967 CV_Error( CV_StsUnmatchedSizes, "imgA and imgB must have equal steps" );
970 CV_Error( CV_StsNullPtr, "" );
972 imgSize = cvGetMatSize( imgA );
976 pyrA = cvGetMat( pyrA, &pstubA );
978 if( pyrA->step*pyrA->height < icvMinimalPyramidSize( imgSize ) )
979 CV_Error( CV_StsBadArg, "pyramid A has insufficient size" );
989 pyrB = cvGetMat( pyrB, &pstubB );
991 if( pyrB->step*pyrB->height < icvMinimalPyramidSize( imgSize ) )
992 CV_Error( CV_StsBadArg, "pyramid B has insufficient size" );
1003 /* check input arguments */
1004 if( !featuresA || !featuresB || !matrices )
1005 CV_Error( CV_StsNullPtr, "" );
1007 if( winSize.width <= 1 || winSize.height <= 1 )
1008 CV_Error( CV_StsOutOfRange, "the search window is too small" );
1011 CV_Error( CV_StsOutOfRange, "" );
1013 icvInitPyramidalAlgorithm( imgA, imgB,
1014 pyrA, pyrB, level, &criteria, MAX_ITERS, flags,
1015 &imgI, &imgJ, &step, &size, &scale, &pyr_buffer );
1017 /* buffer_size = <size for patches> + <size for pyramids> */
1018 bufferBytes = (srcPatchLen + patchLen*3)*sizeof(patchI[0]) + (36*2 + 6)*sizeof(double);
1020 buffer.allocate(bufferBytes);
1024 _status.allocate(count);
1028 patchI = (float *)(uchar*)buffer;
1029 patchJ = patchI + srcPatchLen;
1030 Ix = patchJ + patchLen;
1034 memset( status, 1, count );
1036 if( !(flags & CV_LKFLOW_INITIAL_GUESSES) )
1038 memcpy( featuresB, featuresA, count * sizeof( featuresA[0] ));
1039 for( i = 0; i < count * 4; i += 4 )
1041 matrices[i] = matrices[i + 3] = 1.f;
1042 matrices[i + 1] = matrices[i + 2] = 0.f;
1046 for( i = 0; i < count; i++ )
1048 featuresB[i].x = (float)(featuresB[i].x * scale[level] * 0.5);
1049 featuresB[i].y = (float)(featuresB[i].y * scale[level] * 0.5);
1052 /* do processing from top pyramid level (smallest image)
1053 to the bottom (original image) */
1054 for( l = level; l >= 0; l-- )
1056 CvSize levelSize = size[l];
1057 int levelStep = step[l];
1059 /* find flow for each given point at the particular level */
1060 for( i = 0; i < count; i++ )
1065 double meanI = 0, meanJ = 0;
1067 int pt_status = status[i];
1073 Av[0] = matrices[i*4];
1074 Av[1] = matrices[i*4+1];
1075 Av[3] = matrices[i*4+2];
1076 Av[4] = matrices[i*4+3];
1078 Av[2] = featuresB[i].x += featuresB[i].x;
1079 Av[5] = featuresB[i].y += featuresB[i].y;
1081 u.x = (float) (featuresA[i].x * scale[l]);
1082 u.y = (float) (featuresA[i].y * scale[l]);
1084 if( u.x < -eps || u.x >= levelSize.width+eps ||
1085 u.y < -eps || u.y >= levelSize.height+eps ||
1086 icvGetRectSubPix_8u32f_C1R( imgI[l], levelStep,
1087 levelSize, patchI, srcPatchStep, srcPatchSize, u ) < 0 )
1089 /* point is outside the image. take the next */
1095 icvCalcIxIy_32f( patchI, srcPatchStep, Ix, Iy,
1096 (srcPatchSize.width-2)*sizeof(patchI[0]), srcPatchSize,
1097 smoothKernel, patchJ );
1099 /* repack patchI (remove borders) */
1100 for( k = 0; k < patchSize.height; k++ )
1101 memcpy( patchI + k * patchSize.width,
1102 patchI + (k + 1) * srcPatchSize.width + 1, patchStep );
1104 memset( G, 0, sizeof( G ));
1106 /* calculate G matrix */
1107 for( y = -winSize.height, k = 0; y <= winSize.height; y++ )
1109 for( x = -winSize.width; x <= winSize.width; x++, k++ )
1111 double ixix = ((double) Ix[k]) * Ix[k];
1112 double ixiy = ((double) Ix[k]) * Iy[k];
1113 double iyiy = ((double) Iy[k]) * Iy[k];
1136 // G[13] == G[8] == G[4]
1167 meanI /= patchSize.width*patchSize.height;
1173 // fill part of G below its diagonal
1174 for( y = 1; y < 6; y++ )
1175 for( x = 0; x < y; x++ )
1176 G[y * 6 + x] = G[x * 6 + y];
1178 cvInitMatHeader( &mat, 6, 6, CV_64FC1, G );
1180 if( cvInvert( &mat, &mat, CV_SVD ) < 1e-4 )
1182 /* bad matrix. take the next point */
1188 for( j = 0; j < criteria.max_iter; j++ )
1190 double b[6] = {0,0,0,0,0,0}, eta[6];
1191 double t0, t1, s = 0;
1193 if( Av[2] < -eps || Av[2] >= levelSize.width+eps ||
1194 Av[5] < -eps || Av[5] >= levelSize.height+eps ||
1195 icvGetQuadrangleSubPix_8u32f_C1R( imgJ[l], levelStep,
1196 levelSize, patchJ, patchStep, patchSize, Av ) < 0 )
1202 for( y = -winSize.height, k = 0, meanJ = 0; y <= winSize.height; y++ )
1203 for( x = -winSize.width; x <= winSize.width; x++, k++ )
1206 meanJ = meanJ / (patchSize.width * patchSize.height) - meanI;
1208 for( y = -winSize.height, k = 0; y <= winSize.height; y++ )
1210 for( x = -winSize.width; x <= winSize.width; x++, k++ )
1212 double t = patchI[k] - patchJ[k] + meanJ;
1213 double ixt = Ix[k] * t;
1214 double iyt = Iy[k] * t;
1227 for( k = 0; k < 6; k++ )
1228 eta[k] = G[k*6]*b[0] + G[k*6+1]*b[1] + G[k*6+2]*b[2] +
1229 G[k*6+3]*b[3] + G[k*6+4]*b[4] + G[k*6+5]*b[5];
1231 Av[2] = (float)(Av[2] + Av[0] * eta[0] + Av[1] * eta[1]);
1232 Av[5] = (float)(Av[5] + Av[3] * eta[0] + Av[4] * eta[1]);
1234 t0 = Av[0] * (1 + eta[2]) + Av[1] * eta[4];
1235 t1 = Av[0] * eta[3] + Av[1] * (1 + eta[5]);
1239 t0 = Av[3] * (1 + eta[2]) + Av[4] * eta[4];
1240 t1 = Av[3] * eta[3] + Av[4] * (1 + eta[5]);
1244 if( eta[0] * eta[0] + eta[1] * eta[1] < criteria.epsilon )
1248 if( pt_status != 0 || l == 0 )
1250 status[i] = (char)pt_status;
1251 featuresB[i].x = Av[2];
1252 featuresB[i].y = Av[5];
1254 matrices[i*4] = Av[0];
1255 matrices[i*4+1] = Av[1];
1256 matrices[i*4+2] = Av[3];
1257 matrices[i*4+3] = Av[4];
1260 if( pt_status && l == 0 && error )
1265 for( y = 0, k = 0; y < patchSize.height; y++ )
1267 for( x = 0; x < patchSize.width; x++, k++ )
1269 double t = patchI[k] - patchJ[k] + meanJ;
1273 error[i] = (float)sqrt(err);
1282 icvGetRTMatrix( const CvPoint2D32f* a, const CvPoint2D32f* b,
1283 int count, CvMat* M, int full_affine )
1287 double sa[36], sb[6];
1288 CvMat A = cvMat( 6, 6, CV_64F, sa ), B = cvMat( 6, 1, CV_64F, sb );
1289 CvMat MM = cvMat( 6, 1, CV_64F, M->data.db );
1293 memset( sa, 0, sizeof(sa) );
1294 memset( sb, 0, sizeof(sb) );
1296 for( i = 0; i < count; i++ )
1298 sa[0] += a[i].x*a[i].x;
1299 sa[1] += a[i].y*a[i].x;
1302 sa[6] += a[i].x*a[i].y;
1303 sa[7] += a[i].y*a[i].y;
1310 sb[0] += a[i].x*b[i].x;
1311 sb[1] += a[i].y*b[i].x;
1313 sb[3] += a[i].x*b[i].y;
1314 sb[4] += a[i].y*b[i].y;
1328 cvSolve( &A, &B, &MM, CV_SVD );
1332 double sa[16], sb[4], m[4], *om = M->data.db;
1333 CvMat A = cvMat( 4, 4, CV_64F, sa ), B = cvMat( 4, 1, CV_64F, sb );
1334 CvMat MM = cvMat( 4, 1, CV_64F, m );
1338 memset( sa, 0, sizeof(sa) );
1339 memset( sb, 0, sizeof(sb) );
1341 for( i = 0; i < count; i++ )
1343 sa[0] += a[i].x*a[i].x + a[i].y*a[i].y;
1349 sa[5] += a[i].x*a[i].x + a[i].y*a[i].y;
1363 sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;
1364 sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;
1369 cvSolve( &A, &B, &MM, CV_SVD );
1371 om[0] = om[4] = m[0];
1381 cvEstimateRigidTransform( const CvArr* _A, const CvArr* _B, CvMat* _M, int full_affine )
1383 const int COUNT = 15;
1384 const int WIDTH = 160, HEIGHT = 120;
1385 const int RANSAC_MAX_ITERS = 500;
1386 const int RANSAC_SIZE0 = 3;
1387 const double RANSAC_GOOD_RATIO = 0.5;
1389 cv::Ptr<CvMat> sA, sB;
1390 cv::AutoBuffer<CvPoint2D32f> pA, pB;
1391 cv::AutoBuffer<int> good_idx;
1392 cv::AutoBuffer<char> status;
1393 cv::Ptr<CvMat> gray;
1395 CvMat stubA, *A = cvGetMat( _A, &stubA );
1396 CvMat stubB, *B = cvGetMat( _B, &stubB );
1398 int cn, equal_sizes;
1400 int count_x, count_y, count = 0;
1402 CvRNG rng = cvRNG(-1);
1404 CvMat M = cvMat( 2, 3, CV_64F, m );
1408 if( !CV_IS_MAT(_M) )
1409 CV_Error( _M ? CV_StsBadArg : CV_StsNullPtr, "Output parameter M is not a valid matrix" );
1411 if( !CV_ARE_SIZES_EQ( A, B ) )
1412 CV_Error( CV_StsUnmatchedSizes, "Both input images must have the same size" );
1414 if( !CV_ARE_TYPES_EQ( A, B ) )
1415 CV_Error( CV_StsUnmatchedFormats, "Both input images must have the same data type" );
1417 if( CV_MAT_TYPE(A->type) == CV_8UC1 || CV_MAT_TYPE(A->type) == CV_8UC3 )
1419 cn = CV_MAT_CN(A->type);
1421 sz1 = cvSize(WIDTH, HEIGHT);
1423 scale = MAX( (double)sz1.width/sz0.width, (double)sz1.height/sz0.height );
1424 scale = MIN( scale, 1. );
1425 sz1.width = cvRound( sz0.width * scale );
1426 sz1.height = cvRound( sz0.height * scale );
1428 equal_sizes = sz1.width == sz0.width && sz1.height == sz0.height;
1430 if( !equal_sizes || cn != 1 )
1432 sA = cvCreateMat( sz1.height, sz1.width, CV_8UC1 );
1433 sB = cvCreateMat( sz1.height, sz1.width, CV_8UC1 );
1437 gray = cvCreateMat( sz0.height, sz0.width, CV_8UC1 );
1438 cvCvtColor( A, gray, CV_BGR2GRAY );
1439 cvResize( gray, sA, CV_INTER_AREA );
1440 cvCvtColor( B, gray, CV_BGR2GRAY );
1441 cvResize( gray, sB, CV_INTER_AREA );
1446 cvResize( A, sA, CV_INTER_AREA );
1447 cvResize( B, sB, CV_INTER_AREA );
1455 count_x = cvRound((double)COUNT*sz1.width/sz1.height);
1456 count = count_x * count_y;
1460 status.allocate(count);
1462 for( i = 0, k = 0; i < count_y; i++ )
1463 for( j = 0; j < count_x; j++, k++ )
1465 pA[k].x = (j+0.5f)*sz1.width/count_x;
1466 pA[k].y = (i+0.5f)*sz1.height/count_y;
1469 // find the corresponding points in B
1470 cvCalcOpticalFlowPyrLK( A, B, 0, 0, pA, pB, count, cvSize(10,10), 3,
1471 status, 0, cvTermCriteria(CV_TERMCRIT_ITER,40,0.1), 0 );
1473 // repack the remained points
1474 for( i = 0, k = 0; i < count; i++ )
1487 else if( CV_MAT_TYPE(A->type) == CV_32FC2 || CV_MAT_TYPE(A->type) == CV_32SC2 )
1489 count = A->cols*A->rows;
1493 _pA = cvMat( A->rows, A->cols, CV_32FC2, pA );
1494 _pB = cvMat( B->rows, B->cols, CV_32FC2, pB );
1495 cvConvert( A, &_pA );
1496 cvConvert( B, &_pB );
1499 CV_Error( CV_StsUnsupportedFormat, "Both input images must have either 8uC1 or 8uC3 type" );
1501 good_idx.allocate(count);
1503 if( count < RANSAC_SIZE0 )
1506 CvMat _pB = cvMat(1, count, CV_32FC2, pB);
1507 brect = cvBoundingRect(&_pB, 1);
1510 // 1. find the consensus
1511 for( k = 0; k < RANSAC_MAX_ITERS; k++ )
1513 int idx[RANSAC_SIZE0];
1517 memset( a, 0, sizeof(a) );
1518 memset( b, 0, sizeof(b) );
1520 // choose random 3 non-complanar points from A & B
1521 for( i = 0; i < RANSAC_SIZE0; i++ )
1523 for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ )
1525 idx[i] = cvRandInt(&rng) % count;
1527 for( j = 0; j < i; j++ )
1529 if( idx[j] == idx[i] )
1531 // check that the points are not very close one each other
1532 if( fabs(pA[idx[i]].x - pA[idx[j]].x) +
1533 fabs(pA[idx[i]].y - pA[idx[j]].y) < FLT_EPSILON )
1535 if( fabs(pB[idx[i]].x - pB[idx[j]].x) +
1536 fabs(pB[idx[i]].y - pB[idx[j]].y) < FLT_EPSILON )
1543 if( i+1 == RANSAC_SIZE0 )
1545 // additional check for non-complanar vectors
1554 double dax1 = a[1].x - a[0].x, day1 = a[1].y - a[0].y;
1555 double dax2 = a[2].x - a[0].x, day2 = a[2].y - a[0].y;
1556 double dbx1 = b[1].x - b[0].y, dby1 = b[1].y - b[0].y;
1557 double dbx2 = b[2].x - b[0].x, dby2 = b[2].y - b[0].y;
1558 const double eps = 0.01;
1560 if( fabs(dax1*day2 - day1*dax2) < eps*sqrt(dax1*dax1+day1*day1)*sqrt(dax2*dax2+day2*day2) ||
1561 fabs(dbx1*dby2 - dby1*dbx2) < eps*sqrt(dbx1*dbx1+dby1*dby1)*sqrt(dbx2*dbx2+dby2*dby2) )
1567 if( k1 >= RANSAC_MAX_ITERS )
1571 if( i < RANSAC_SIZE0 )
1574 // estimate the transformation using 3 points
1575 icvGetRTMatrix( a, b, 3, &M, full_affine );
1577 for( i = 0, good_count = 0; i < count; i++ )
1579 if( fabs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) +
1580 fabs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < MAX(brect.width,brect.height)*0.05 )
1581 good_idx[good_count++] = i;
1584 if( good_count >= count*RANSAC_GOOD_RATIO )
1588 if( k >= RANSAC_MAX_ITERS )
1591 if( good_count < count )
1593 for( i = 0; i < good_count; i++ )
1601 icvGetRTMatrix( pA, pB, good_count, &M, full_affine );
1604 cvConvert( &M, _M );
1612 Mat estimateRigidTransform( const Mat& A,
1616 Mat M(2, 3, CV_64F);
1617 CvMat _A = A, _B = B, _M = M;
1618 cvEstimateRigidTransform(&_A, &_B, &_M, fullAffine);