// Use RANSAC to compute H without openCV function
Mat Assignment2::computeRANSAC(Assignment2 &m2)
{
vector< DMatch > matches = this->FindMatchesEuclidian(m2); // find matches based on euclidian distances
// Create vectors for storing Points
vector<Point2f> query(matches.size());
vector<Point2f> train(matches.size());
//cout << matches.size() << " " << keypoints.size() << endl;
// Assign points to that declared array
for(unsigned int i=0;i<(matches.size());i++)
{
query[i]=this->keypoints[matches[i].queryIdx].pt;
train[i]=m2.keypoints[matches[i].trainIdx].pt;
}
// define parameters for RANSAC algorithm
int iterations=0;
double inlierThresh=0.90;
double inlierPerc=0;
Mat H;
Mat Hbest;
double inlierbest=0;
// do while loop for 3000 iterations
do
{
int N=matches.size();
int M=4;
vector<Point2f> queryH(M); // take 4 points
vector<Point2f> trainH(M);
vector<int> randVecId=randomGenerate(N,M);
for(int i=0;i<M;i++)
{
queryH[i]=query[randVecId[i]]; // take 4 points randomly
trainH[i]=train[randVecId[i]];
}
H = computeH(queryH, trainH); // Compute H based on the that 4 points
double inliners = computeInliers(train, query, H, 3); // calculate inliners for that H
if(inliners>inlierbest)
{
inlierbest=inliners;
Hbest=H.clone();
}
inlierPerc=inliners/(double)N;
iterations++;
} while(iterations<3000);//&&inlierPerc<inlierThresh);
normalizeH(Hbest); // normalize Hbest
return Hbest;
}
void Initialization::computeHomography(
const std::vector<Vector3d>& f_ref,
const std::vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
std::vector<int>& inliers,
std::vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
std::vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
std::vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
for (size_t i = 0, i_max = f_ref.size(); i < i_max; ++i)
{
uv_ref[i] = project2d(f_ref[i]);
uv_cur[i] = project2d(f_cur[i]);
}
Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
std::vector<int> outliers;
computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1_.rotation_matrix(), Homography.T_c2_from_c1_.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
T_cur_from_ref = Homography.T_c2_from_c1_;
}
