void rectify::combineMatrices()
{
firstHomography = euclideanTrans * projectiveTrans;
secondHomography = euclideanTransDash * projectiveTransDash;
firstHomography.Display();
secondHomography.Display();
std::vector<vector3d>checkVec;
checkVec.clear();
checkVec.reserve(6);
checkVec.push_back(vector3d(0, 0, 1));
checkVec.push_back(vector3d(800, 0, 1));
checkVec.push_back(vector3d(0, 600, 1));
checkVec.push_back(vector3d(400, 300, 1));
checkVec.push_back(vector3d(800, 600, 1));
checkVec.push_back(vector3d(500, 500, 1));
vector3d first, second;
for(int i = 0; i < checkVec.size(); i++)
{
first = firstHomography * checkVec[i];
second = secondHomography * checkVec[i];
first.Display("first");
second.Display("second");
}
int dataID;
for(int i = 0; i < global::inliersRecord.size(); i++)
{
global::secondImagePoints[dataID];
dataID = inliersRecord[i];
first = global::firstImagePoints[dataID];
first = firstHomography * first;
first.change(first.x/first.z, first.y/first.z, 1.0);
first.Display("fiC");
second = global::secondImagePoints[dataID];
second = secondHomography * second;
second.change(second.x/second.z, second.y/second.z, 1.0);
second.Display("secChan");
std::cout<<" disparity "<<fabs(first.x - second.x)<<std::endl;
}
first = firstHomography * global::epipoleA;
first.Display("f");
second = secondHomography * global::epipoleB;
second.Display("s");
vector3d inf = vector3d(1, 0, 0);
Matrix3f infX;
//infX.AsymmetricMatrix(inf);
infX.m[2][1] = 1;
infX.m[1][2] = -1;
(secondHomography.ReturnTranspose() * infX * firstHomography).Display("Fun");
Matrix3f fund = global::fundamentalMatrix;
fund = fund * (1/fund.m[2][1]);
fund.Display("DAm");
}
void inliers::ransac()
{
int totalPointSize = firstImagePoints.size(); //'S' equals total set containing all points
//largest block to contain coefficients, accessed partially
float **coeffMat;
int num_pts = 2 * totalPointSize;
coeffMat = new float *[num_pts];
for(int i = 0; i < num_pts; i++)
coeffMat[i] = new float[9];
float outputMat[9];
Matrix3f homographyMat;
//select random points, but here three diverse points are selected
num_pts = 4;
Matrix3f mat, matD;
sampleInliers.clear();
sampleInliers.reserve(num_pts);
int interval = (totalPointSize / num_pts) - 1;
for(int j = 0; j < 4; j++)
sampleInliers.push_back(interval * j);
int dataID;
vector3d first, second;
bool ransacRepeat = true;
float threshold = 0.0; //'t' equals 3.84 * sigma_squared, maximum deviation to be an inlier
float errorDeviation[totalPointSize]; //deviation of point from mean, which is estimated to be zero
int oldSampleSize;
float oldOmega = 0.5; //increase of omega, which is prob. of inliers, suggests convergence
float omega;
int capitalN;
int ransacItr = 0;
while(ransacRepeat)
{
normalization(sampleInliers, mat, matD);
for(int i = 0; i < num_pts; i++)
{
dataID = sampleInliers[i];
first = global::firstImagePoints[dataID];
second = global::secondImagePoints[dataID];
first = mat * first;
second = matD * second;
coeffMat[2 * i][0] = 0;
coeffMat[2 * i][1] = 0;
coeffMat[2 * i][2] = 0;
coeffMat[2 * i][3] = -second.z * first.x;
coeffMat[2 * i][4] = -second.z * first.y;
coeffMat[2 * i][5] = -second.z * first.z;
coeffMat[2 * i][6] = second.y * first.x;
coeffMat[2 * i][7] = second.y * first.y;
coeffMat[2 * i][8] = second.y * first.z;
coeffMat[2 * i + 1][0] = second.z * first.x;
coeffMat[2 * i + 1][1] = second.z * first.y;
coeffMat[2 * i + 1][2] = second.z * first.z;
coeffMat[2 * i + 1][3] = 0;
coeffMat[2 * i + 1][4] = 0;
coeffMat[2 * i + 1][5] = 0;
coeffMat[2 * i + 1][6] = -second.x * first.x;
coeffMat[2 * i + 1][7] = -second.x * first.y;
coeffMat[2 * i + 1][8] = -second.x * first.z;
}
solutionSVD(coeffMat, num_pts * 2, 9, outputMat);
homographyMat = Matrix3f(outputMat);
matD.Inverse();
homographyMat = matD * homographyMat * mat;
//homographyMat.Display();
ransacItr++; //counter increased
//check accuracy of homographyMat with other points
for(int i = 0; i < totalPointSize; i++)
{
first = homographyMat * firstImagePoints[i];
first.change(first.x/first.z, first.y/first.z, 1.0);
second = secondImagePoints[i];
errorDeviation[i] = second.distancePointsSquared(first);
std::cout<<" index "<<i<<" and S.D. "<<sqrt(errorDeviation[i])<<" ";
//first.Display("f"); second.Display("s");
threshold += errorDeviation[i];
if( i % 2 == 0) std::cout<<std::endl;
}
threshold /= totalPointSize;
threshold = sqrt(threshold) * 0.8;
std::cout<<" thres "<<threshold<<std::endl;
oldSampleSize = sampleInliers.size(); //'s' is the sample Size, or old num_pts
sampleInliers.clear();
sampleInliers.reserve(totalPointSize);
std::cout<<std::endl;
for(int i = 0; i < totalPointSize; i++)
{
if(sqrt(errorDeviation[i]) <= threshold)
{
std::cout<<" Pindex "<<i<<" deviation "<<sqrt(errorDeviation[i])<<" ";
if( i % 2 == 0) std::cout<<std::endl;
sampleInliers.push_back(i); //'Si' is the consensus set
}
}
oldSampleSize = sampleInliers.size() - oldSampleSize;
std::cout<<std::endl<<" sampleInliersPushed "<<oldSampleSize<<" "<<std::endl;
num_pts = sampleInliers.size();
omega = (float)num_pts / totalPointSize; //probability of an inlier
omega = log10(1 - pow(omega, oldSampleSize));
capitalN = (-2)/omega;
std::cout<<std::endl<<" omega "<<omega<<" capitalN "<<capitalN<<std::endl;
if(capitalN <= ransacItr || ransacItr > 50) ransacRepeat = false;
//need to check whether omega was converging
}
std::cout<<std::endl;
//final re-estimation of the model
normalization(sampleInliers, mat, matD);
for(int i = 0; i < num_pts; i++)
{
dataID = sampleInliers[i];
first = global::firstImagePoints[dataID];
second = global::secondImagePoints[dataID];
first = mat * first;
second = matD * second;
coeffMat[2 * i][0] = 0;
coeffMat[2 * i][1] = 0;
coeffMat[2 * i][2] = 0;
coeffMat[2 * i][3] = -second.z * first.x;
coeffMat[2 * i][4] = -second.z * first.y;
coeffMat[2 * i][5] = -second.z * first.z;
coeffMat[2 * i][6] = second.y * first.x;
coeffMat[2 * i][7] = second.y * first.y;
coeffMat[2 * i][8] = second.y * first.z;
coeffMat[2 * i + 1][0] = second.z * first.x;
coeffMat[2 * i + 1][1] = second.z * first.y;
coeffMat[2 * i + 1][2] = second.z * first.z;
coeffMat[2 * i + 1][3] = 0;
coeffMat[2 * i + 1][4] = 0;
coeffMat[2 * i + 1][5] = 0;
coeffMat[2 * i + 1][6] = -second.x * first.x;
coeffMat[2 * i + 1][7] = -second.x * first.y;
coeffMat[2 * i + 1][8] = -second.x * first.z;
}
solutionSVD(coeffMat, num_pts * 2, 9, outputMat);
std::cout<<" answer "<<std::endl;
for(int j = 0; j < 9; j++)
std::cout<<" "<<outputMat[j]<<" ";
homographyMat = Matrix3f(outputMat);
homographyMat.Display("homoSmall");
matD.Inverse();
homographyMat = matD * homographyMat * mat;
homographyMat.Display("homography");
global::inliersRecord.clear();
global::inliersRecord.reserve(sampleInliers.size());
for(int i = 0; i < sampleInliers.size(); i++)
global::inliersRecord.push_back(sampleInliers[i]);
//release memory
delete[] coeffMat;
}
