slam十四讲第七讲 pose_estimation_2d2d

以下均为ubuntu20.04下完成，报错的地方均以修改，希望小伙伴批评指正。另外，关于特征点匹配的详细代码在另一篇文章中，可自行查阅。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>//该库用于3D信息重建，姿态估计，摄像机标定等。

using namespace std;
using namespace cv;

void find_feature_matches(
	const Mat &img_1, const Mat &img_2,//&img_1 指向img_1的地址
	std::vector<KeyPoint> &keypoints_1,
	std::vector<KeyPoint> &keypoints_2,
	std::vector<DMatch> &matches);
	
void pose_estimation_2d2d(  
 	std::vector<KeyPoint> keypoints_1,//定义容器存储特征点信息
  	std::vector<KeyPoint> keypoints_2,
  	std::vector<DMatch> matches,
  	Mat &R, Mat &t);
  	
Point2d pixel2cam(const Point2d &p, const Mat &k);//像素转相机坐标 只是定义一下，想要转换需要 p 和 K 两个东西。

int main(int argc, char **argv) {
    
    
  if (argc != 3) {
    
                //程序自身加两张图片=3
    cout << "usage: pose_estimation_2d2d img1 img2" << endl;
    return 1;
  }
  
  Mat img_1 = imread(argv[1], 1);//读取图像
  Mat img_2 = imread(argv[2], 1);
  assert(img_1.data && img_2.data && "Can not load images!");//判断传入的是否是图像数据
  
  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches; 
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);//上面声明过用法，把匹配信息传入matches
  cout << "一共有" << matches.size() << "组匹配点" << endl;
  
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
  
  Mat t_x =                                         //就是t的反对称矩阵，看看反对称形式就明白
    (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= " << endl << t_x * R << endl;
  
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//K为相机内参，已知
  for (DMatch m: matches) {
    
    
    Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);//利用p=KPc求出相机坐标，queryIdx与trainIdx为匹配点对应的序号，pixel2cam上边定义过，此处输入 p 和 K
    Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);//将相机坐标变成三维向量，因为是单目无深度信息，故z=1
    Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
    Mat d = y2.t() * t_x * R * y1;//这就是公式，看书  eg:P转至*K=y2
    cout << "epipolar constraint = " << d << endl;
  }
  return 0;
} 
//这部分内容在上一个有明确的说明，大家可以查阅，就是特征点的提取和匹配
void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
    
    
  Mat descriptors_1, descriptors_2;
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);
  vector<DMatch> match;
  matcher->match(descriptors_1, descriptors_2, match);
  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  double min_dist = 10000, max_dist = 0;//定义最大值最小值，不用纠结数字，后面会被换掉
  
  for (int i = 0; i < descriptors_1.rows; i++) {
    
    
  	double dist = match[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);
  
  for (int i = 0; i < descriptors_1.rows; i++) {
    
    
  	if (match[i].distance <= max(2 * min_dist, 30.0)) {
    
       //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  		matches.push_back(match[i]);
  		}
  	}
  }
  
  Point2d pixel2cam(const Point2d &p, const Mat &K) {
    
       //像素点转化相机坐标的公式，自己定义上面在调用，哈哈开始我也以为是自动转的，具体做法就是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> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          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<Point2f> points1;//用来存储像素坐标的
   vector<Point2f> points2;
   
   for (int i = 0; i < (int)matches.size(); i++) {
    
    
   	points1.push_back(keypoints_1[matches[i].queryIdx].pt);//像素信息存入points1
   	points2.push_back(keypoints_2[matches[i].trainIdx].pt);
   	
   }
   
   Mat fundamental_matrix;//计算基础矩阵
   fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT);//系统给你算，输入两个像素点就好。FM_8POINT这个地方改动了，要注意，不同版本的形式可能不同。
   cout << "fundamental_matrix is " << endl <<fundamental_matrix << endl;
   
   Point2d principal_point(325.1, 249.7);//相机光心，TUM dataset标定值
   double focal_length = 521;//相机焦据 f，TUM dataset标定值
   Mat essential_matrix;//系统给你算，输入像素点，相机光心，焦距就好
   essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
   cout << "essential_matrix is " << endl << essential_matrix << endl;
   
   recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);//同理，不说了
   cout << "R is" << endl << R << endl;
   cout << "t is" << endl << t << endl;
  }
  
  
  
  
  
  

以下为运行结果