#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace std;
using namespace cv;

void pose_estimation_2d2d
(std::vector<KeyPoint> keypoints1,
std::vector<KeyPoint> keypoints2,
std::vector<DMatch> matches,
Mat& R,Mat & t);

void find_feature_match
(Mat imag1,Mat imag2,
vector<KeyPoint> &keypoints1,
vector<KeyPoint> &keypoints2,
vector<DMatch>& matches);

Point2d pixel2cam ( const Point2d& p, const Mat& K );

int main(int argc, char **argv)
{
/* if(argc !=3){
cout<<"usage:feature_extraction img1 img2"<<endl;
return 1;
}*/
//讀取影象
Mat imag1=imread("1.png");
Mat imag2=imread("2.png");

std::vector<KeyPoint> keypoints1,keypoints2;
vector<DMatch> matches;
//特徵匹配
find_feature_match(imag1,imag2,keypoints1,keypoints2,matches);
cout<<"一共找到了"<<matches.size() <<"組匹配點"<<endl;
//估計運動
Mat R,t;
pose_estimation_2d2d(keypoints1,keypoints2,matches,R,t);

//驗證E=t^R*Scale
Mat t_x=(Mat_<double>(3,3)<<0,-t.at<double>(2,0),t.at<double>(1,0),
t.at<double>(2,0),0,-t.at<double>(0,0),
-t.at<double>(1,0),t.at<double>(0,0),0);
cout<<"t^R="<<t_x*R<<endl;

//驗證對極幾何
Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
for(DMatch m:matches){
Point2d pt1=pixel2cam(keypoints1[m.queryIdx].pt,K);
Mat y1=(Mat_<double>(3,1)<<pt1.x,pt1.y,1);
Point2d pt2=pixel2cam(keypoints2[m.trainIdx].pt,K);
Mat y2=(Mat_<double>(3,1)<<pt2.x,pt2.y,1);
Mat d=y2.t()*t_x*R*y1;
cout<<"對極約束="<<d<<endl;
}
return 0;
}


Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
return Point2d
(
( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
);
}
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints1,
std::vector<KeyPoint> keypoints2,
std::vector<DMatch> matches,
Mat& R,Mat & t)
{
//相機內參
Mat K=(Mat_<double> (3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
//把匹配點轉換為vector<Points2f>
vector<Point2f> points1;
vector<Point2f> points2;
for(int i=0;i<(int)matches.size();i++){
points1.push_back( keypoints1[matches[i].queryIdx].pt );
points2.push_back( keypoints2[matches[i].trainIdx].pt );
}
//計算基礎矩陣F
Mat F;
F=findFundamentalMat(points1,points2,CV_FM_8POINT);
cout<<"F="<<endl<<F<<endl;
//計算本質矩陣F
Point2d principal_point(325.1,249.7);//光心
int focus_length=521;//焦距
Mat E=findEssentialMat(points1,points2,focus_length,principal_point,RANSAC);
cout<<"E="<<endl<<E<<endl;
//計算單位矩陣H
Mat H;
H=findHomography(points1,points2,RANSAC,3,noArray(),2000,0.99);
cout<<"H="<<endl<<H<<endl;
recoverPose(E,points1,points2,R,t,focus_length,principal_point);
cout<<"R="<<endl<<R<<endl;
cout<<"t="<<endl<<t<<endl;
}
void find_feature_match(Mat imag1,Mat imag2,
vector<KeyPoint> &keypoints1,
vector<KeyPoint> &keypoints2,
vector<DMatch> &matches)
{
Mat descriptors1,descriptors2;
Ptr<ORB> orb=ORB::create();//(500,1.2f,8,31,0,2,ORB::HARRIS_SCORE,31,20);
//第一步：檢測角點位置
orb->detect(imag1,keypoints1);
orb->detect(imag2,keypoints2);

//第二步：根據角點位置計算BRIEF描述子
orb->compute(imag1,keypoints1,descriptors1);
orb->compute(imag2,keypoints2,descriptors2);
/* //畫出關鍵點的位置
Mat outimage1;
drawKeypoints(imag1,keypoints1,outimage1);//,Scalar::all(-1),DrawMatchesFlags::DEFAULT);
// imshow("ORB特徵點",outimage1);*/

//第三步：對兩幅影象中的BRIEF描述子進行匹配，使用Hamming距離
vector<DMatch> match1;
BFMatcher matcher (NORM_HAMMING);
matcher.match(descriptors1,descriptors2,match1);

//第四步：匹配點篩選
double min_dist=10000,max_dist=0;
//找出所有匹配之間的最小距離和最大距離
for(int i=0;i<descriptors1.rows;i++){
double dist=match1[i].distance;
if(dist<min_dist)
min_dist=dist;
if(dist>max_dist)
max_dist=dist;
}
printf("max_dist: %f \n",max_dist);
printf("min_dist: %f\n",min_dist);
//認為當描述子之間的距離大於兩倍最小距離時，為誤匹配
//但有時最小距離會非常小，所以應設定下限(經驗值)
// std::vector<DMatch> good_matches;
for(int i=0;i<descriptors1.rows;i++){
if(match1[i].distance<=max(2*min_dist,30.0)){//這個決定了匹配的點數
// good_matches.push_back(matches[i]);
matches.push_back(match1[i]);
}
}

/* //第五步：繪製匹配結果
Mat imag_match;
Mat imag_goodmatch;
drawMatches(imag1,keypoints1,imag2,keypoints2,matches,imag_match);
drawMatches(imag1,keypoints1,imag2,keypoints2,good_matches,imag_goodmatch);
imshow("所有匹配點對",imag_match);
imshow("篩選過後的點對",imag_goodmatch);*/

}