OpenCV: внедрить мою модель RANSAC

Не так далеко, насколько я знаю, но в коде есть реализация RANSAC для estimateRigidTransform Вот —

https://github.com/opencv/opencv/blob/master/modules/video/src/lkpyramid.cpp

// RANSAC stuff:
// 1. find the consensus
for( k = 0; k < RANSAC_MAX_ITERS; k++ )
{
int idx[RANSAC_SIZE0];
Point2f a[RANSAC_SIZE0];
Point2f b[RANSAC_SIZE0];
// choose random 3 non-complanar points from A & B
for( i = 0; i < RANSAC_SIZE0; i++ )
{
for( k1 = 0; k1 < RANSAC_MAX_ITERS; k1++ )
{
idx[i] = rng.uniform(0, count);
for( j = 0; j < i; j++ )
{
if( idx[j] == idx[i] )
break;
// check that the points are not very close one each other
if( fabs(pA[idx[i]].x - pA[idx[j]].x) +
fabs(pA[idx[i]].y - pA[idx[j]].y) < FLT_EPSILON )
break;
if( fabs(pB[idx[i]].x - pB[idx[j]].x) +
fabs(pB[idx[i]].y - pB[idx[j]].y) < FLT_EPSILON )
break;
}
if( j < i )
continue;
if( i+1 == RANSAC_SIZE0 )
{
// additional check for non-complanar vectors
a[0] = pA[idx[0]];
a[1] = pA[idx[1]];
a[2] = pA[idx[2]];
b[0] = pB[idx[0]];
b[1] = pB[idx[1]];
b[2] = pB[idx[2]];
double dax1 = a[1].x - a[0].x, day1 = a[1].y - a[0].y;
double dax2 = a[2].x - a[0].x, day2 = a[2].y - a[0].y;
double dbx1 = b[1].x - b[0].x, dby1 = b[1].y - b[0].y;
double dbx2 = b[2].x - b[0].x, dby2 = b[2].y - b[0].y;
const double eps = 0.01;
if( fabs(dax1*day2 - day1*dax2) < eps*std::sqrt(dax1*dax1+day1*day1)*std::sqrt(dax2*dax2+day2*day2) ||
fabs(dbx1*dby2 - dby1*dbx2) < eps*std::sqrt(dbx1*dbx1+dby1*dby1)*std::sqrt(dbx2*dbx2+dby2*dby2) )
continue;
}
break;
}
if( k1 >= RANSAC_MAX_ITERS )
break;
}
if( i < RANSAC_SIZE0 )
continue;
// estimate the transformation using 3 points
getRTMatrix( a, b, 3, M, fullAffine );
const double* m = M.ptr<double>();
for( i = 0, good_count = 0; i < count; i++ )
{
if( std::abs( m[0]*pA[i].x + m[1]*pA[i].y + m[2] - pB[i].x ) +
std::abs( m[3]*pA[i].x + m[4]*pA[i].y + m[5] - pB[i].y ) < std::max(brect.width,brect.height)*0.05 )
good_idx[good_count++] = i;
}
if( good_count >= count*RANSAC_GOOD_RATIO )
break;
}
if( k >= RANSAC_MAX_ITERS )
return Mat();