/* Find fundamental matrix */
void cvFindFundamentalMatrix( int* points1, int* points2, int numpoints, int, float* matrix )
{
CvMat* pointsMat1;
CvMat* pointsMat2;
CvMat fundMatr = cvMat(3,3,CV_32F,matrix);
int i, curr = 0;
pointsMat1 = cvCreateMat(3,numpoints,CV_64F);
pointsMat2 = cvCreateMat(3,numpoints,CV_64F);
for( i = 0; i < numpoints; i++ )
{
cvmSet(pointsMat1,0,i,points1[curr]);//x
cvmSet(pointsMat1,1,i,points1[curr+1]);//y
cvmSet(pointsMat1,2,i,1.0);
cvmSet(pointsMat2,0,i,points2[curr]);//x
cvmSet(pointsMat2,1,i,points2[curr+1]);//y
cvmSet(pointsMat2,2,i,1.0);
curr += 2;
}
cvFindFundamentalMat(pointsMat1,pointsMat2,&fundMatr,CV_FM_RANSAC,1,0.99,0);
cvReleaseMat(&pointsMat1);
cvReleaseMat(&pointsMat2);
}
Mat GetFundamentalMat(const vector<TrackedPoint> & trackedPoints,
vector<TrackedPoint>* inliers,
double ransac_max_dist, double ransac_p) {
//vector<TrackedPoint> trackedPoints;
//vector<matf> H_out;
//NormalizePoints2(trackedPoints_, trackedPoints, H_out);
const int method = FM_RANSAC;
//const int method = FM_LMEDS;
const double param1 = ransac_max_dist;
const double param2 = ransac_p;
#ifdef OPENCV_2_1
matf pts1(trackedPoints.size(), 2), pts2(trackedPoints.size(), 2);
for (size_t i = 0; i < trackedPoints.size(); ++i) {
pts1(i, 0) = trackedPoints[i].x1;
pts2(i, 1) = trackedPoints[i].y1;
pts2(i, 0) = trackedPoints[i].x2;
pts2(i, 1) = trackedPoints[i].y2;
}
CvMat pts1cv = pts1;
CvMat pts2cv = pts2;
matf mat(3, 3);
CvMat matcv = mat;
Mat_<uchar> status(1, trackedPoints.size());
CvMat statuscv = status;
cvFindFundamentalMat(&pts1cv, &pts2cv, &matcv, method, param1, param2, &statuscv);
#else
Mat_<Vec2f> pts1(trackedPoints.size(), 1), pts2(trackedPoints.size(), 1);
for (size_t i = 0; i < trackedPoints.size(); ++i) {
pts1(i, 0) = Vec2f(trackedPoints[i].x1, trackedPoints[i].y1);
pts2(i, 0) = Vec2f(trackedPoints[i].x2, trackedPoints[i].y2);
}
vector<unsigned char> status;
Mat mat = findFundamentalMat(pts1, pts2, method, param1, param2, status);
#endif
if (inliers) {
inliers->clear();
for (size_t i = 0; i < trackedPoints.size(); ++i)
#ifdef OPENCV_2_1
if (status(0, i))
#else
if (status[i])
#endif
inliers->push_back(trackedPoints[i]);
}
//return H_out[1].inv().t() * (matf)mat * H_out[0].inv();
return mat;
}
static Mat _findFundamentalMat( const Mat& points1, const Mat& points2,
int method, double param1, double param2,
vector<uchar>* mask )
{
CV_Assert(points1.checkVector(2) >= 0 && points2.checkVector(2) >= 0 &&
(points1.depth() == CV_32F || points1.depth() == CV_32S) &&
points1.depth() == points2.depth());
Mat F(3, 3, CV_64F);
CvMat _pt1 = Mat(points1), _pt2 = Mat(points2);
CvMat matF = F, _mask, *pmask = 0;
if( mask )
{
mask->resize(points1.cols*points1.rows*points1.channels()/2);
pmask = &(_mask = cvMat(1, (int)mask->size(), CV_8U, (void*)&(*mask)[0]));
}
int n = cvFindFundamentalMat( &_pt1, &_pt2, &matF, method, param1, param2, pmask );
if( n <= 0 )
F = Scalar(0);
return F;
}
/* Returns number of corresponding points */
int icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */
IplImage *image2,/* Image 2 */
CvMat *points1,
CvMat *pntStatus1,
CvMat *points2,
CvMat *pntStatus2,
int useFilter,/*Use fundamental matrix to filter points */
double threshold)/* Threshold for good points in filter */
{
int resNumCorrPoints = 0;
CvPoint2D32f* cornerPoints1 = 0;
CvPoint2D32f* cornerPoints2 = 0;
char* status = 0;
float* errors = 0;
CvMat* tmpPoints1 = 0;
CvMat* tmpPoints2 = 0;
CvMat* pStatus = 0;
IplImage *grayImage1 = 0;
IplImage *grayImage2 = 0;
IplImage *pyrImage1 = 0;
IplImage *pyrImage2 = 0;
CV_FUNCNAME( "icvFindCorrForGivenPoints" );
__BEGIN__;
/* Test input data for errors */
/* Test for null pointers */
if( image1 == 0 || image2 == 0 ||
points1 == 0 || points2 == 0 ||
pntStatus1 == 0 || pntStatus2 == 0)
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
/* Test image size */
int w,h;
w = image1->width;
h = image1->height;
if( w <= 0 || h <= 0)
{
CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" );
}
if( image2->width != w || image2->height != h )
{
CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" );
}
/* Test for matrices */
if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) ||
!CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" );
}
/* Test type of status matrices */
if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
}
/* Test number of points */
int numPoints;
numPoints = points1->cols;
if( numPoints <= 0 )
{
CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" );
}
if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" );
}
if( points1->rows != 2 || points2->rows != 2 )
{
CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" );
}
if( pntStatus1->rows != 1 || pntStatus2->rows != 1 )
{
CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" );
}
/* ----- End test ----- */
/* Compute number of visible points on image1 */
int numVisPoints;
numVisPoints = cvCountNonZero(pntStatus1);
if( numVisPoints > 0 )
{
/* Create temporary images */
/* We must use iplImage againts hughgui images */
/*
CvvImage grayImage1;
CvvImage grayImage2;
CvvImage pyrImage1;
CvvImage pyrImage2;
*/
/* Create Ipl images */
CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( pyrImage1 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( pyrImage2 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) );
CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) );
int i;
for( i = 0; i < numVisPoints; i++ )
{
status[i] = 1;
}
/* !!! Need test creation errors */
/*
if( !grayImage1.Create(w,h,8)) EXIT;
if( !grayImage2.Create(w,h,8)) EXIT;
if( !pyrImage1. Create(w,h,8)) EXIT;
if( !pyrImage2. Create(w,h,8)) EXIT;
*/
cvCvtColor(image1,grayImage1,CV_BGR2GRAY);
cvCvtColor(image2,grayImage2,CV_BGR2GRAY);
/*
grayImage1.CopyOf(image1,0);
grayImage2.CopyOf(image2,0);
*/
/* Copy points good points from input data */
uchar *stat1 = pntStatus1->data.ptr;
uchar *stat2 = pntStatus2->data.ptr;
int curr = 0;
for( i = 0; i < numPoints; i++ )
{
if( stat1[i] )
{
cornerPoints1[curr].x = (float)cvmGet(points1,0,i);
cornerPoints1[curr].y = (float)cvmGet(points1,1,i);
curr++;
}
}
/* Define number of levels of pyramid */
cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
pyrImage1, pyrImage2,
cornerPoints1, cornerPoints2,
numVisPoints, cvSize(10,10), 3,
status, errors,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
0/*CV_LKFLOW_PYR_A_READY*/ );
memset(stat2,0,sizeof(uchar)*numPoints);
int currVis = 0;
int totalCorns = 0;
/* Copy new points and set status */
/* stat1 may not be the same as stat2 */
for( i = 0; i < numPoints; i++ )
{
if( stat1[i] )
{
if( status[currVis] && errors[currVis] < 1000 )
{
stat2[i] = 1;
cvmSet(points2,0,i,cornerPoints2[currVis].x);
cvmSet(points2,1,i,cornerPoints2[currVis].y);
totalCorns++;
}
currVis++;
}
}
resNumCorrPoints = totalCorns;
/* Filter points using RANSAC */
if( useFilter )
{
resNumCorrPoints = 0;
/* Use RANSAC filter for found points */
if( totalCorns > 7 )
{
/* Create array with good points only */
CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) );
CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) );
/* Copy just good points */
int currPoint = 0;
for( i = 0; i < numPoints; i++ )
{
if( stat2[i] )
{
cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i));
cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i));
cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i));
cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i));
currPoint++;
}
}
/* Compute fundamental matrix */
CvMat fundMatr;
double fundMatr_dat[9];
fundMatr = cvMat(3,3,CV_64F,fundMatr_dat);
CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) );
int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus);
if( num > 0 )
{
int curr = 0;
/* Set final status for points2 */
for( i = 0; i < numPoints; i++ )
{
if( stat2[i] )
{
if( cvmGet(pStatus,0,curr) == 0 )
{
stat2[i] = 0;
}
curr++;
}
}
resNumCorrPoints = curr;
}
}
}
}
__END__;
/* Free allocated memory */
cvFree(&cornerPoints1);
cvFree(&cornerPoints2);
cvFree(&status);
cvFree(&errors);
cvFree(&tmpPoints1);
cvFree(&tmpPoints2);
cvReleaseMat( &pStatus );
cvReleaseImage( &grayImage1 );
cvReleaseImage( &grayImage2 );
cvReleaseImage( &pyrImage1 );
cvReleaseImage( &pyrImage2 );
return resNumCorrPoints;
}
