// Computing the Fundamental Matrix of an Image Pair.cpp : 定义控制台应用程序的入口点。
//#include "stdafx.h"
/#include <iostream>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include<opencv2/legacy/legacy.hpp>
using namespace std;int main()
{
// Read input images
cv::Mat image1= cv::imread("../church01.jpg",0);
cv::Mat image2= cv::imread("../church03.jpg",0);
if (!image1.data || !image2.data)
return 0;     // Display the images
cv::namedWindow("Right Image");
cv::imshow("Right Image",image1);
cv::namedWindow("Left Image");
cv::imshow("Left Image",image2); // vector of keypoints
std::vector<cv::KeyPoint> keypoints1;
std::vector<cv::KeyPoint> keypoints2; // Construction of the SURF feature detector 
cv::SurfFeatureDetector surf(3000); // Detection of the SURF features
surf.detect(image1,keypoints1);
surf.detect(image2,keypoints2); std::cout << "Number of SURF points (1): " << keypoints1.size() << std::endl;
std::cout << "Number of SURF points (2): " << keypoints2.size() << std::endl;

// Draw the kepoints
cv::Mat imageKP;
cv::drawKeypoints(image1,keypoints1,imageKP,cv::Scalar(255,255,255),cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::namedWindow("Right SURF Features");
cv::imshow("Right SURF Features",imageKP);
cv::drawKeypoints(image2,keypoints2,imageKP,cv::Scalar(255,255,255),cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv::namedWindow("Left SURF Features");
cv::imshow("Left SURF Features",imageKP); // Construction of the SURF descriptor extractor 
cv::SurfDescriptorExtractor surfDesc; // Extraction of the SURF descriptors
cv::Mat descriptors1, descriptors2;
surfDesc.compute(image1,keypoints1,descriptors1);
surfDesc.compute(image2,keypoints2,descriptors2); std::cout << "descriptor matrix size: " << descriptors1.rows << " by " << descriptors1.cols << std::endl; // Construction of the matcher 
cv::BruteForceMatcher<cv::L2<float>> matcher; // Match the two image descriptors
std::vector<cv::DMatch> matches;
matcher.match(descriptors1,descriptors2, matches); std::cout << "Number of matched points: " << matches.size() << std::endl; // Select few Matches  
std::vector<cv::DMatch> selMatches;
/* between church01 and church03 */
selMatches.push_back(matches[14]);  
selMatches.push_back(matches[16]);
selMatches.push_back(matches[141]);  
selMatches.push_back(matches[146]);
selMatches.push_back(matches[235]);
selMatches.push_back(matches[238]);
//selMatches.push_back(matches[274]); // Draw the selected matches
cv::Mat imageMatches;
cv::drawMatches(image1,keypoints1,  // 1st image and its keypoints
            image2,keypoints2,  // 2nd image and its keypoints
// selMatches, // the matches
matches, // the matches
imageMatches, // the image produced
cv::Scalar(255,255,255)); // color of the lines
cv::namedWindow("Matches");
cv::imshow("Matches",imageMatches); // Convert 1 vector of keypoints into
// 2 vectors of Point2f
std::vector<int> pointIndexes1;
std::vector<int> pointIndexes2;
for (std::vector<cv::DMatch>::const_iterator it= selMatches.begin();
 it!= selMatches.end(); ++it) 
{  // Get the indexes of the selected matched keypoints
 pointIndexes1.push_back(it->queryIdx);
 pointIndexes2.push_back(it->trainIdx);
}
 
// Convert keypoints into Point2f
std::vector<cv::Point2f> selPoints1, selPoints2;
cv::KeyPoint::convert(keypoints1,selPoints1,pointIndexes1);
cv::KeyPoint::convert(keypoints2,selPoints2,pointIndexes2); // check by drawing the points 
std::vector<cv::Point2f>::const_iterator it= selPoints1.begin();
while (it!=selPoints1.end()) 
{ // draw a circle at each corner location
cv::circle(image1,*it,3,cv::Scalar(255,255,255),2);
++it;
} it= selPoints2.begin();
while (it!=selPoints2.end()) { // draw a circle at each corner location
cv::circle(image2,*it,3,cv::Scalar(255,255,255),2);
++it;
} // Compute F matrix from 7 matches
cv::Mat fundemental= cv::findFundamentalMat(
cv::Mat(selPoints1), // points in first image
cv::Mat(selPoints2), // points in second image
CV_FM_7POINT);       // 7-point method std::cout << "F-Matrix size= " << fundemental.rows << "," << fundemental.cols << std::endl;   // draw the left points corresponding epipolar lines in right image 
std::vector<cv::Vec3f> lines1; 
cv::computeCorrespondEpilines(
cv::Mat(selPoints1), // image points 
1,                   // in image 1 (can also be 2)
fundemental, // F matrix
lines1);     // vector of epipolar lines // for all epipolar lines
for (vector<cv::Vec3f>::const_iterator it= lines1.begin(); it!=lines1.end(); ++it)
{  // draw the epipolar line between first and last column
 cv::line(image2,cv::Point(0,-(*it)[2]/(*it)[1]),
             cv::Point(image2.cols,-((*it)[2]+(*it)[0]*image2.cols)/(*it)[1]),
 cv::Scalar(255,255,255));
}

// draw the left points corresponding epipolar lines in left image 
std::vector<cv::Vec3f> lines2;
cv::computeCorrespondEpilines(cv::Mat(selPoints2),2,fundemental,lines2);
for (vector<cv::Vec3f>::const_iterator it= lines2.begin();it!=lines2.end(); ++it)
{  // draw the epipolar line between first and last column
 cv::line(image1,cv::Point(0,-(*it)[2]/(*it)[1]),
             cv::Point(image1.cols,-((*it)[2]+(*it)[0]*image1.cols)/(*it)[1]),
 cv::Scalar(255,255,255));
}

    // Display the images with points and epipolar lines
cv::namedWindow("Right Image Epilines");
cv::imshow("Right Image Epilines",image1);
cv::namedWindow("Left Image Epilines");
cv::imshow("Left Image Epilines",image2); /*
std::nth_element(matches.begin(),    // initial position
             matches.begin()+matches.size()/2, // 50%
 matches.end());     // end position
// remove all elements after the 
matches.erase(matches.begin()+matches.size()/2, matches.end()); 
*/
// Convert keypoints into Point2f
std::vector<cv::Point2f> points1, points2;
for (std::vector<cv::DMatch>::const_iterator it= matches.begin();
 it!= matches.end(); ++it) 
{  // Get the position of left keypoints
 float x= keypoints1[it->queryIdx].pt.x;
 float y= keypoints1[it->queryIdx].pt.y;
 points1.push_back(cv::Point2f(x,y));
 // Get the position of right keypoints
 x= keypoints2[it->trainIdx].pt.x;
 y= keypoints2[it->trainIdx].pt.y;
 points2.push_back(cv::Point2f(x,y));
} std::cout << points1.size() << " " << points2.size() << std::endl;  // Compute F matrix using RANSAC
std::vector<uchar> inliers(points1.size(),0);
fundemental= cv::findFundamentalMat(
cv::Mat(points1),cv::Mat(points2), // matching points
inliers,      // match status (inlier ou outlier)  
CV_FM_RANSAC, // RANSAC method
1,            // distance to epipolar line
0.98);        // confidence probability // Read input images
image1= cv::imread("../church01.jpg",0);
image2= cv::imread("../church03.jpg",0); // Draw the epipolar line of few points
cv::computeCorrespondEpilines(cv::Mat(selPoints1),1,fundemental,lines1);
for (vector<cv::Vec3f>::const_iterator it= lines1.begin();
 it!=lines1.end(); ++it)
{  cv::line(image2,cv::Point(0,-(*it)[2]/(*it)[1]),
             cv::Point(image2.cols,-((*it)[2]+(*it)[0]*image2.cols)/(*it)[1]),
 cv::Scalar(255,255,255));
} cv::computeCorrespondEpilines(cv::Mat(selPoints2),2,fundemental,lines2);
for (vector<cv::Vec3f>::const_iterator it= lines2.begin();
 it!=lines2.end(); ++it) {  cv::line(image1,cv::Point(0,-(*it)[2]/(*it)[1]),
             cv::Point(image1.cols,-((*it)[2]+(*it)[0]*image1.cols)/(*it)[1]),
 cv::Scalar(255,255,255));
} // Draw the inlier points
std::vector<cv::Point2f> points1In, points2In;
std::vector<cv::Point2f>::const_iterator itPts= points1.begin();
std::vector<uchar>::const_iterator itIn= inliers.begin();
while (itPts!=points1.end())
{ // draw a circle at each inlier location
if (*itIn)
{
  cv::circle(image1,*itPts,3,cv::Scalar(255,255,255),2);
points1In.push_back(*itPts);
}
++itPts;
++itIn;
} itPts= points2.begin();
itIn= inliers.begin();
while (itPts!=points2.end())
{ // draw a circle at each inlier location
if (*itIn) {
cv::circle(image2,*itPts,3,cv::Scalar(255,255,255),2);
points2In.push_back(*itPts);
}
++itPts;
++itIn;
}    // Display the images with points
cv::namedWindow("Right Image Epilines (RANSAC)");
cv::imshow("Right Image Epilines (RANSAC)",image1);
cv::namedWindow("Left Image Epilines (RANSAC)");
cv::imshow("Left Image Epilines (RANSAC)",image2); cv::findHomography(cv::Mat(points1In),cv::Mat(points2In),inliers,CV_RANSAC,1.); // Read input images
image1= cv::imread("../church01.jpg",0);
image2= cv::imread("../church03.jpg",0); // Draw the inlier points
itPts= points1In.begin();
itIn= inliers.begin();
while (itPts!=points1In.end()) { // draw a circle at each inlier location
if (*itIn) 
  cv::circle(image1,*itPts,3,cv::Scalar(255,255,255),2);

++itPts;
++itIn;
} itPts= points2In.begin();
itIn= inliers.begin();
while (itPts!=points2In.end())
{ // draw a circle at each inlier location
if (*itIn) 
cv::circle(image2,*itPts,3,cv::Scalar(255,255,255),2);

++itPts;
++itIn;
}    // Display the images with points
cv::namedWindow("Right Image Homography (RANSAC)");
cv::imshow("Right Image Homography (RANSAC)",image1);
cv::namedWindow("Left Image Homography (RANSAC)");
cv::imshow("Left Image Homography (RANSAC)",image2); cv::waitKey();
return 0;
}