int kpmRansacHomograhyEstimation(CorspMap* corspMap, int inlierIndex[], int *num, float h[3][3])
{
if( corspMap->num <= 3 ) return -1;
else if( corspMap->num == 4 ) {
Point2f pt1[4];
Point2f pt2[4];
for( int i = 0; i < 4; i++ ) {
inlierIndex[i] = i;
pt1[i].x = corspMap->mp[i].x1;
pt1[i].y = corspMap->mp[i].y1;
pt2[i].x = corspMap->mp[i].x2;
pt2[i].y = corspMap->mp[i].y2;
}
*num = 4;
return ComputeHomography(pt1, pt2, h);
}
else if( corspMap->num < 10 ) {
return RansacHomograhyEstimationSub2( corspMap, inlierIndex, num, h );
}
else {
return RansacHomograhyEstimationSub1( corspMap, inlierIndex, num, h );
}
}
/******************* TO DO *********************
* leastSquaresFit:
* INPUT:
* f1, f2: source feature sets
* matches: correspondences between f1 and f2
* m: motion model
* inliers: inlier match indices (indexes into 'matches' array)
* M: transformation matrix (output)
* OUTPUT:
* compute the transformation from f1 to f2 using only the inliers
* and return it in M
*/
int leastSquaresFit(const FeatureSet &f1, const FeatureSet &f2,
const vector<FeatureMatch> &matches, MotionModel m,
const vector<int> &inliers, CTransform3x3& M)
{
// This function needs to handle two possible motion models,
// This function needs to handle two possible motion models,
// pure translations and full homographies.
switch (m) {
case eTranslate: {
// for spherically warped images, the transformation is a
// for spherically warped images, the transformation is a
// translation and only has two degrees of freedom
//
// therefore, we simply compute the average translation vector
// between the feature in f1 and its match in f2 for all inliers
double u = 0;
double v = 0;
for (int i=0; i < (int) inliers.size(); i++) {
// BEGIN TODO
// use this loop to compute the average translation vector
// over all inliers
int m1 = matches.at(inliers.at(i)).id1;
int m2 = matches.at(inliers.at(i)).id2;
u += f1[m1].x - f2[m2].x;
v += f1[m1].y - f2[m2].y;
printf("TODO: %s:%d\n", __FILE__, __LINE__);
// END TODO
}
u /= inliers.size();
v /= inliers.size();
M = CTransform3x3::Translation((float) u, (float) v);
break;
}
case eHomography: {
M = CTransform3x3();
// BEGIN TODO
// Compute a homography M using all inliers.
// This should call ComputeHomography.
vector<FeatureMatch> inmatches;
for (int bo = 0; bo < inliers.size(); bo++)
{
FeatureMatch mat;
mat = matches.at(inliers.at(bo));
inmatches.push_back(mat);
}
M = ComputeHomography(f1, f2, inmatches);
break;
}
case eRotate3D: {
cout << "3D Rotation is not supported by this project";
break;
}
}
// END TODO
return 0;
}
/******************* TO DO *********************
* alignPair:
* INPUT:
* f1, f2: source feature sets
* matches: correspondences between f1 and f2
* Each match in 'matches' contains two feature ids of
* Each match in 'matches' contains two feature ids of
* matching features, id1 (in f1) and id2 (in f2).
* m: motion model
* nRANSAC: number of RANSAC iterations
* RANSACthresh: RANSAC distance threshold
* M: transformation matrix (output)
*
* OUTPUT:
* repeat for nRANSAC iterations:
* choose a minimal set of feature matches
* estimate the transformation implied by these matches
* count the number of inliers
* for the transformation with the maximum number of inliers,
* compute the least squares motion estimate using the inliers,
* and store it in M
*/
int alignPair(const FeatureSet &f1, const FeatureSet &f2,
const vector<FeatureMatch> &matches, MotionModel m,
int nRANSAC, double RANSACthresh, CTransform3x3 &M)
{
// BEGIN TODO
// Write this entire method. You need to handle two types of
// motion models, pure translations (m == eTranslation) and
// full homographies (m == eHomography). However, you should
// only have one outer loop to perform the RANSAC code, as
// BEGIN TODO
// Write this entire method. You need to handle two types of
// motion models, pure translations (m == eTranslation) and
// full homographies (m == eHomography). However, you should
// only have one outer loop to perform the RANSAC code, as
// the use of RANSAC is almost identical for both cases.
//
// Your homography handling code should call ComputeHomography.
// This function should also call countInliers and, at the end,
// leastSquaresFit.
cout << "alignalignalignalign";
cout << f1.size();
cout << '\n';
cout << f2.size();
cout << '\n';
cout << matches.size();
cout << '\n';
int maxInliers = -1;
int sz = matches.size();
for (int i = 0; i < nRANSAC; i++) {
int n = rand() % sz;
FeatureMatch randomMatch = matches.at(n);
CTransform3x3 trans;
switch (m) {
case eTranslate: {
Feature first = f1[randomMatch.id1];
Feature second = f2[randomMatch.id2];
float xTranslation = (float)(second.x - first.x);
float yTranslation = (float)(second.y - first.y);
cout << "Translation: X "; cout << xTranslation;
cout << ", Y ";
cout << yTranslation; cout << '\n';
trans = CTransform3x3::Translation(xTranslation, yTranslation);
break;
}
case eHomography: {
int indices[] = {0, 0, 0, 0};
for (int chooseRand = 0; chooseRand < 4; chooseRand++)
{
indices[chooseRand] = rand() % sz;
for (int p = 0; p < chooseRand; p++)
{
if (indices[p] = indices[chooseRand]);
{
indices[chooseRand] = rand() % sz;
p--;
}
}
}
vector<FeatureMatch> selectedMatches;
selectedMatches.resize(4);
for (int c = 0; c < 4; c++)
{
int idx = indices[c];
FeatureMatch fm;
fm.id1 = matches.at(idx).id1;
fm.id2 = matches.at(idx).id2;
selectedMatches.push_back(fm);
}
trans = ComputeHomography(f1, f2, selectedMatches);
break;
}
}
vector<int> inliers;
countInliers(f1,f2,matches,m,trans,RANSACthresh,inliers);
cout << "Inliers: "; cout << inliers.size(); cout << '\n';
if (inliers.size() > maxInliers) {
maxInliers = inliers.size();
M = trans;
}
}
// END TODO
return 0;
}
static int RansacHomograhyEstimationSub1(CorspMap* corspMap, int inlierIndex[], int *num, float h[3][3])
{
static int *tempIndex;
static int tempIndexMax = 0;
int tempNum;
int maxLoopCount = 200;
int loopCount = 0;
if( corspMap->num <= 4 ) return -1;
if( tempIndexMax < corspMap->num ) {
tempIndexMax = (corspMap->num/100 + 1)*100;
tempIndex = (int*)malloc(sizeof(int)*tempIndexMax);
}
*num = 0;
while( loopCount < maxLoopCount ) {
Point2f pt1[4];
Point2f pt2[4];
int sample[4];
for(int i = 0 ; i < 4 ; i++ ) {
static int s = 0;
int j;
if( s++ == 0 ) srand((unsigned int)time(NULL));
if( s == 128 ) s = 0;
int pos = (int)((float )corspMap->num * rand() / (RAND_MAX + 1.0F));
for(j = 0; j < i; j++ ) {
if( pos == sample[j] ) break;
}
if( j < i ) {
i--;
continue;
}
else {
sample[i] = pos;
}
pt1[i].x = (float)corspMap->mp[pos].x1;
pt1[i].y = (float)corspMap->mp[pos].y1;
pt2[i].x = (float)corspMap->mp[pos].x2;
pt2[i].y = (float)corspMap->mp[pos].y2;
}
if( (!IsGoodSample(pt1)) || (!IsGoodSample(pt2)) ) {loopCount++; continue;}
float htemp[3][3];
if( ComputeHomography(pt1, pt2, htemp) < 0 ) {loopCount++; continue;}
tempNum = 0;
for(int i = 0; i < corspMap->num; i++ ) {
float xx2, yy2, ww2, err;
xx2 = htemp[0][0] * corspMap->mp[i].x2 + htemp[0][1] * corspMap->mp[i].y2 + htemp[0][2];
yy2 = htemp[1][0] * corspMap->mp[i].x2 + htemp[1][1] * corspMap->mp[i].y2 + htemp[1][2];
ww2 = htemp[2][0] * corspMap->mp[i].x2 + htemp[2][1] * corspMap->mp[i].y2 + htemp[2][2];
xx2 /= ww2;
yy2 /= ww2;
err = (xx2 - corspMap->mp[i].x1)*(xx2 - corspMap->mp[i].x1) + (yy2 - corspMap->mp[i].y1)*(yy2 - corspMap->mp[i].y1);
if( err < INLIER_THRESH ) {
tempIndex[tempNum] = i;
tempNum++;
}
}
if( tempNum > *num ) {
*num = tempNum;
for(int j=0;j<3;j++) for(int i=0;i<3;i++) h[j][i] = htemp[j][i];
for(int i=0; i<*num; i++) inlierIndex[i] = tempIndex[i];
static const float p = log(1.0F-0.99F);
float e;
int N;
e = (float)tempNum / (float)corspMap->num;
N = (int)(p / log(1 - e*e*e*e));
if( N < maxLoopCount ) maxLoopCount = N;
}
loopCount++;
}
//printf("sampleCount= %d, outlierProb=%f\n", loopCount, 1.0F-(float)(*num)/(float)corspMap->num);
return 0;
}
static int RansacHomograhyEstimationSub2(CorspMap* corspMap, int inlierIndex[], int *num, float h[3][3])
{
Point2f pt1[4];
Point2f pt2[4];
int tempIndex[10];
int tempNum;
int loopCount = 0;
if( corspMap->num <= 4 ) return -1;
if( corspMap->num >= 10 ) return -1;
*num = 0;
for( int i1 = 0; i1 < corspMap->num; i1++ ) {
pt1[0].x = (float)corspMap->mp[i1].x1;
pt1[0].y = (float)corspMap->mp[i1].y1;
pt2[0].x = (float)corspMap->mp[i1].x2;
pt2[0].y = (float)corspMap->mp[i1].y2;
for( int i2 = i1+1; i2 < corspMap->num; i2++ ) {
pt1[1].x = (float)corspMap->mp[i2].x1;
pt1[1].y = (float)corspMap->mp[i2].y1;
pt2[1].x = (float)corspMap->mp[i2].x2;
pt2[1].y = (float)corspMap->mp[i2].y2;
for( int i3 = i2+1; i3 < corspMap->num; i3++ ) {
pt1[2].x = (float)corspMap->mp[i3].x1;
pt1[2].y = (float)corspMap->mp[i3].y1;
pt2[2].x = (float)corspMap->mp[i3].x2;
pt2[2].y = (float)corspMap->mp[i3].y2;
for( int i4 = i3+1; i4 < corspMap->num; i4++ ) {
pt1[3].x = (float)corspMap->mp[i4].x1;
pt1[3].y = (float)corspMap->mp[i4].y1;
pt2[3].x = (float)corspMap->mp[i4].x2;
pt2[3].y = (float)corspMap->mp[i4].y2;
if( (!IsGoodSample(pt1)) || (!IsGoodSample(pt2)) ) {loopCount++; continue;}
float htemp[3][3];
if( ComputeHomography(pt1, pt2, htemp) < 0 ) {loopCount++; continue;}
tempNum = 0;
for(int i = 0; i < corspMap->num; i++ ) {
float xx2, yy2, ww2, err;
xx2 = htemp[0][0] * corspMap->mp[i].x2 + htemp[0][1] * corspMap->mp[i].y2 + htemp[0][2];
yy2 = htemp[1][0] * corspMap->mp[i].x2 + htemp[1][1] * corspMap->mp[i].y2 + htemp[1][2];
ww2 = htemp[2][0] * corspMap->mp[i].x2 + htemp[2][1] * corspMap->mp[i].y2 + htemp[2][2];
xx2 /= ww2;
yy2 /= ww2;
err = (xx2 - corspMap->mp[i].x1)*(xx2 - corspMap->mp[i].x1) + (yy2 - corspMap->mp[i].y1)*(yy2 - corspMap->mp[i].y1);
if( err < INLIER_THRESH ) {
tempIndex[tempNum] = i;
tempNum++;
}
}
if( tempNum > *num ) {
*num = tempNum;
for(int j=0;j<3;j++) for(int i=0;i<3;i++) h[j][i] = htemp[j][i];
for(int i=0; i<*num; i++) inlierIndex[i] = tempIndex[i];
static const float p = log(1.0F-0.99F);
float e;
int N;
e = (float)tempNum / (float)corspMap->num;
N = (int)(p / log(1 - e*e*e*e));
if( N < loopCount ) {
//printf("sampleCount= %d, outlierProb=%f\n", loopCount, 1.0F-(float)(*num)/(float)corspMap->num);
return 0;
}
}
loopCount++;
}
}
}
}
//printf("sampleCount= %d, outlierProb=%f\n", loopCount, 1.0F-(float)(*num)/(float)corspMap->num);
return 0;
}
/*******************************************************************************
Compute homography transformation between images using RANSAC.
matches: set of matching points between images
numMatches: number of matching points
numIterations: number of iterations to run RANSAC
inlierThreshold: maximum distance between points that are considered to be inliers
hom: returned homography transformation (image1 -> image2)
homInv: returned inverse homography transformation (image2 -> image1)
image1Display: first image used to display matches
image2Display: second image used to display matches
*******************************************************************************/
void MainWindow::RANSAC(CMatches *matches, int numMatches, int numIterations, double inlierThreshold,
double hom[3][3], double homInv[3][3], QImage &image1Display, QImage &image2Display)
{
// We'll be comparing groups of 4 points
#define MATCH_GROUP_SIZE 4
// If there are fewer than matchGroupSize matches, this won't work, so return.
if(numMatches < MATCH_GROUP_SIZE)
{
return;
}
CMatches potentialInliers[MATCH_GROUP_SIZE];
int numInliers, maxInliers, randomMatchID;
int usedMatchIDs[MATCH_GROUP_SIZE];
double potentialInlierHom[3][3];
double bestHom[3][3];
for(int i = 0; i < MATCH_GROUP_SIZE; i++)
{
usedMatchIDs[i] = -1;
}
// Initialize random seed
srand( time(NULL) );
maxInliers = -1;
for (int iter = 0; iter < numIterations; iter++)
{
// Randomly select 4 pairs of potentially matching points from matches
for(int i = 0; i < MATCH_GROUP_SIZE; i++)
{
// Make sure that the match selected is unique
bool matchUnique = false;
while(!matchUnique)
{
matchUnique = true;
randomMatchID = rand() % numMatches;
// Compare generated match to every other match,
// and make sure we're not reusing one
for(int j = 0; j < i; j++)
{
// Check for a match with a previously used match
if(randomMatchID == usedMatchIDs[j])
{
matchUnique = false;
continue;
}
}
}
// Once we've reached this point, we know that we have a unique match,
// so let's add the randomly generated match to the array of potential inliers
potentialInliers[i] = matches[randomMatchID];
// Make sure we don't use the same match again
usedMatchIDs[i] = randomMatchID;
}
// Now that we have four unique, randomly selected matches,
// compute the homography relating the four selected matches
ComputeHomography(potentialInliers, MATCH_GROUP_SIZE, potentialInlierHom, true);
// Using the computed homography, compute the number of inliers against all of the matches
numInliers = ComputeInlierCount(potentialInlierHom, matches, numMatches, inlierThreshold);
// If this homography produces the highest number of inliers, store it as the best homography
if(numInliers > maxInliers)
{
maxInliers = numInliers;
for(int i = 0; i < 3; i++)
{
for(int j = 0; j < 3; j++)
{
bestHom[i][j] = potentialInlierHom[i][j];
}
}
}
}
CMatches *inliers = new CMatches[maxInliers];
// Given the highest scoring homography, once again find all the inliers
GetInliers(bestHom, matches, numMatches, inlierThreshold, inliers);
numInliers = ComputeInlierCount(bestHom, matches, numMatches, inlierThreshold);
// Compute a new refined homography using all of the inliers
ComputeHomography(inliers, maxInliers, hom, true);
// Compute an inverse homography as well
ComputeHomography(inliers, maxInliers, homInv, false);
// Display the inlier matches
DrawMatches(inliers, numInliers, image1Display, image2Display);
delete inliers;
}
/*******************************************************************************
Compute homography transformation between images using RANSAC.
matches - set of matching points between images
numMatches - number of matching points
numIterations - number of iterations to run RANSAC
inlierThreshold - maximum distance between points that are considered to be inliers
hom - returned homography transformation (image1 -> image2)
homInv - returned inverse homography transformation (image2 -> image1)
image1Display - image used to display matches
image2Display - image used to display matches
*******************************************************************************/
void MainWindow::RANSAC(CMatches *matches, int numMatches, int numIterations, double inlierThreshold,
double hom[3][3], double homInv[3][3], QImage &image1Display, QImage &image2Display)
{
int i, j;
CMatches randomMatches[4];
double h[3][3];
int max = 0;
for(i = 0; i < numIterations; i++) {
// Randomly select 4 matching pairs
for(j = 0; j < 4; j++) {
int num = rand() % numMatches;
randomMatches[j].m_X1 = matches[num].m_X1;
randomMatches[j].m_X2 = matches[num].m_X2;
randomMatches[j].m_Y1 = matches[num].m_Y1;
randomMatches[j].m_Y2 = matches[num].m_Y2;
}
// Compute Homography for the 4 random matches and inliers count
if(ComputeHomography(randomMatches, 4, h, true)) {
int count = ComputeInlierCount(h, matches, numMatches, inlierThreshold);
// Check if the homography is the best
if(count > max) {
max = count;
int x, y;
for(x = 0; x < 3; x++) {
for(y = 0; y < 3; y++) {
hom[x][y] = h[x][y];
}
}
}
}
}
CMatches *inliers = new CMatches[max];
int numInliers = 0;
for(i = 0; i < numMatches; i++) {
double x2, y2;
Project(matches[i].m_X1, matches[i].m_Y1, x2, y2, hom);
double distance = pow(x2 - matches[i].m_X2, 2.0) + pow(y2 - matches[i].m_Y2, 2.0);
if(distance < inlierThreshold * inlierThreshold) {
inliers[numInliers].m_X1 = matches[i].m_X1;
inliers[numInliers].m_Y1 = matches[i].m_Y1;
inliers[numInliers].m_X2 = matches[i].m_X2;
inliers[numInliers].m_Y2 = matches[i].m_Y2;
numInliers++;
}
}
// Recompute the best homography and inverse homography
ComputeHomography(inliers, numInliers, hom, true);
ComputeHomography(inliers, numInliers, homInv, false);
// After you're done computing the inliers, display the corresponding matches.
DrawMatches(inliers, numInliers, image1Display, image2Display);
// Clean up
delete [] inliers;
}
