2D-2D位姿估计增加三角测量，确定尺度--参考SLAM十四讲7.6

#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 );

void triangulation(const vector<KeyPoint>& keypoints1,
const vector<KeyPoint>& keypoints2,
const std::vector<DMatch>& matches,
const Mat& R,const Mat& t,
vector<Point3d>& points);//三角化函数

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;
}

//三角化
vector<Point3d> points;
triangulation(keypoints1,keypoints2,matches,R,t,points);

//验证三角化与特征点的重投影关系
for(int i=0;i<matches.size();i++){
Point2d pt1_cam=pixel2cam(keypoints1[matches[i].queryIdx].pt,K);//归一化平面上的坐标，但不知深度
Point2d pt1_cam_3d(
points[i].x/points[i].z,
points[i].y/points[i].z
);//points是通过三角测量以后重投影的坐标，知道了深度
cout<<"point in the first camera frame:"<<pt1_cam<<endl;
cout<<"point projected from 3D "<<pt1_cam_3d<<",d="<<points[i].z<<endl;

Point2d pt2_cam=pixel2cam(keypoints2[matches[i].trainIdx].pt,K);
Mat pt2_trans=R*(Mat_<double>(3,1)<<points[i].x,points[i].y,points[i].z)+t;
pt2_trans /=pt2_trans.at<double>(2,0);
cout<<"point in the second camera frame:"<<pt2_cam<<endl;
cout<<"point projected from second frame: "<<pt2_trans.t()<<endl;
cout<<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);*/

}

void triangulation(const vector<KeyPoint>& keypoints1,
const vector<KeyPoint>& keypoints2,
const std::vector<DMatch>& matches,
const Mat& R,const Mat& t,
vector<Point3d>& points)
{
Mat T1=(Mat_<double>(3,4)<<
1,0,0,0,
0,1,0,0,
0,0,1,0);
Mat T2=(Mat_<double>(3,4)<<
R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),t.at<double>(0,0),
R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),t.at<double>(1,0),
R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2),t.at<double>(2,0));
Mat K=(Mat_<double> (3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
vector<Point2d> pts_1,pts_2;
for(DMatch m:matches)
{
//将像素坐标转为相机坐标
pts_1.push_back(pixel2cam(keypoints1[m.queryIdx].pt,K));
pts_2.push_back(pixel2cam(keypoints2[m.trainIdx].pt,K));
}
Mat pts_4d;
cv::triangulatePoints(T1,T2,pts_1,pts_2,pts_4d);
//转成非齐次坐标
for(int i=0;i<pts_4d.cols;i++){
Mat x=pts_4d.col(i);
x/=x.at<double>(3,0);
Point3d p(
x.at<double>(0,0),
x.at<double>(1,0),
x.at<double>(2,0)
);
points.push_back(p);
}
}