bool Epipolar::isValidPair(vector<DMatch>& matches, vector<KeyPoint>& key1, vector<KeyPoint>& key2, Mat& cam, Mat& distor, Mat& ess, Mat& inliersMask, double inlierPercent){
vector<Point2f>pts1, pts2;
inliersMask.deallocate();
Mat rot, trans;
size_t n = matches.size();
Utility:: getPointMatches(key1, key2, matches, pts1, pts2);
undistortPoints(pts1, pts1, cam, distor);
undistortPoints(pts2, pts2, cam, distor);
ess = findEssentialMat(pts1, pts2, 1.0, Point(0, 0), RANSAC, 0.999, 1.25, inliersMask);
int inliers = recoverPose(ess, pts1, pts2, rot, trans, 1.0, Point(0, 0), inliersMask);
return ((double)inliers / n) > inlierPercent;
}
void Epipolar::calcEssMatrix(vector<DMatch>& goodMatches, vector<KeyPoint>& keys1, vector<KeyPoint>& keys2, Mat& cam, Mat& distor, Mat& ess, double inlierPercent){
int inliers = 0;
vector<Point2f>pts1, pts2;
Mat rot, trans, inlierMask;
size_t n = goodMatches.size();
for (size_t j = 0; j < n; j++)
{
pts1.push_back(keys1[goodMatches[j].trainIdx].pt);
pts2.push_back(keys2[goodMatches[j].queryIdx].pt);
}
undistortPoints(pts1, pts1, cam, distor);
undistortPoints(pts2, pts2, cam, distor);
ess = findEssentialMat(pts1, pts2, 1.0, Point(0, 0), RANSAC, 0.999, 1.25, inlierMask);
inliers = recoverPose(ess, pts1, pts2, rot, trans, 1.0, Point(0, 0), inlierMask);
if ((double)inliers / n < inlierPercent){
ess.release();
}
}
bool find_transform(Mat& K, vector<Point2f>& p1, vector<Point2f>& p2, Mat& R, Mat& T, Mat& mask)
{
double focal_length = 0.5*(K.at<double>(0) + K.at<double>(4));
Point2d principle_point(K.at<double>(2), K.at<double>(5));
Mat E = findEssentialMat(p1, p2, focal_length, principle_point, RANSAC, 0.999, 1.0, mask);
if (E.empty()) return false;
double feasible_count = countNonZero(mask);
cout << (int)feasible_count << " -in- " << p1.size() << endl;
if (feasible_count <= 15 || (feasible_count / p1.size()) < 0.6)
return false;
int pass_count = recoverPose(E, p1, p2, R, T, focal_length, principle_point, mask);
if (((double)pass_count) / feasible_count < 0.7)
return false;
return true;
}