// Compute the essential matrix given a set of
// paired keypoints list
void RobustMatcher::computeEssentialMat(const KeyPointVec &kpts1, const KeyPointVec &kpts2,
const cv::Mat &K, cv::Mat &essentialMat, cv::Mat &inliers_mask)
{
std::vector<cv::Point2f> kpts1_pts, kpts2_pts;
// put keypoints inside vector
for (int i = 0; i < kpts1.size(); ++i)
{
kpts1_pts.push_back(kpts1[i].pt);
kpts2_pts.push_back(kpts2[i].pt);
}
// default params
double f = 55; // focal length in mm
double sx = 22.3, sy = 14.9; // sensor size
double width = 640, height = 480; // image size
double focal = K.at<double>(0,0);
cv::Point2d pp = cv::Point2d(K.at<double>(0,2),K.at<double>(1,2));
int method = cv::RANSAC;
double prob = 0.999;
// compute opencv essential mat
essentialMat =
findEssentialMat(kpts1_pts, kpts2_pts, focal, pp, method, prob, ransac_thresh_, inliers_mask);
}
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;
}
void add_Points( cv::Mat R[],cv::Mat t[],const std::vector< cv::Point2f > points_1[],
const std::vector< cv::Point2f > points_2[],cv::Mat& points3D,const int add,std::vector< cv::Point2f > mask3D[],cv::Mat& img_matches){
// obtain mask on new matches by two-view constraint
cv::Mat mask;
findEssentialMat(points_1[add-1], points_2[add-1], camIntrinsic, cv::RANSAC, 0.999, 0.25, mask);
std::vector< cv::Point2f > pointsx,pointsComparex;
for(int i=0;i<points_1[add-1].size();i++){
if (mask.at<uchar>(i,0) != 0){
pointsx.push_back(points_1[add-1][i]);
pointsComparex.push_back(points_2[add-1][i]);
}
}
std::cout<<"mask result: "<<points_1[add-1].size()<<" "<<pointsx.size()<<std::endl;
// draw red circles on new points, blue circles on points masked out
for (int j=0; j<points_1[add-1].size();j++){
if (mask.at<uchar>(j,0) != 0)
circle(img_matches, points_1[add-1][j], 10, cv::Scalar(0,0,255), 3);
else
circle(img_matches, points_1[add-1][j], 10, cv::Scalar(255,0,0), 3);
}
cv::namedWindow("Matches", CV_WINDOW_NORMAL);
cv::imshow("Matches", img_matches);
cv::waitKey();
// form projection matrix of the 2nd last camera
cv::Mat projMatr1(3,4,CV_32F);
cv::Mat Rt1(3,4,CV_32F); // Rt = [R | t]
R[add-1].convertTo(R[add-1],CV_32F);
t[add-1].convertTo(t[add-1],CV_32F);
hconcat(R[add-1], t[add-1], Rt1); // Rt concatenate
projMatr1 = camIntrinsic * Rt1;
// form projection matrix of the last camera
cv::Mat projMatr2(3,4,CV_32F);
cv::Mat Rt(3,4,CV_32F); // Rt = [R | t]
R[add].convertTo(R[add],CV_32F);
t[add].convertTo(t[add],CV_32F);
hconcat(R[add], t[add], Rt); // Rt concatenate
projMatr2 = camIntrinsic * Rt;
// triangulation on the masked matches
cv::Mat points4Dtemp=cv::Mat_<float>(4,points_1[add-1].size());
cv::triangulatePoints(projMatr1, projMatr2, pointsx, pointsComparex, points4Dtemp);
// fill in new parts of mask3D with 2D points
cv::Point2f x(-1,-1);
for(int i = 0;i<points4Dtemp.cols;i++){
for (int j=0;j<m;j++)
{
if (j == (add-1))
mask3D[j].push_back(pointsx[i]);
else if (j == add)
mask3D[j].push_back(pointsComparex[i]);
else
mask3D[j].push_back(x);
}
}
cv::Mat points3Dtemp(3,pointsx.size(),CV_32F);
for (int i=0; i<pointsx.size(); i++)
{
float x = points4Dtemp.at<float>(3,i);
points3Dtemp.at<float>(0,i) = points4Dtemp.at<float>(0,i) / x;
points3Dtemp.at<float>(1,i) = points4Dtemp.at<float>(1,i) / x;
points3Dtemp.at<float>(2,i) = points4Dtemp.at<float>(2,i) / x;
}
hconcat(points3D,points3Dtemp,points3D);
n = points3D.cols;
}
