高翔Slambook第七讲代码解读（2d-2d位姿估计）

前言：小白创建了一个微信公众号，主要面向学习视觉的小伙伴，分享一些学习过程中的感受和一些技术文章。公众号里文章也会更新在CSDN中，不过由于工作量较大，会出现一段时间的延时，感兴趣的小伙伴可以关注公众号第一时间接受消息。

---

作者：小白

公众号：小白学视觉

---

SLAM前端，或者说视觉里程计VO，做的最主要的一件事就是计算或者估计两帧图像之间的位姿变换，也即旋转矩阵R和平移向量t。根据所应用于计算的特征点信息的不同，可分为三类：2d-2d类、3d-2d类、3d-3d类，这取决于我们手头所现有的能拿来做位姿计算的数据形式。倘若我们只知道某个特征点在相机成像平面的位置，也即像素坐标或归一化平面坐标，却不知其深度，那么这将是一个只具备2d信息的特征点；如果我们通过三角化计算，或者通过RGB-D相机直接测得了某点的深度信息，结合其2d信息，我们能够推算其在该帧图像的相机坐标系下的位置，即相机坐标，此时便形成了一个具备3d信息的特征点。

我们不妨首先来看一下2d-2d位姿估计的过程。关于其中所使用的对极几何、对极约束等数学推算，小绿在此不做赘述，而是将重点放在代码上。

先把子函数声明与主函数放在这里：

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

void pose_estimation_2d2d (
  const  std::vector<KeyPoint> & keypoints_1,
   const std::vector<KeyPoint> & keypoints_2,
   const std::vector< DMatch > & matches,
   Mat& R, Mat& t );

void verify_polar_constraint(
 const  std::vector<KeyPoint> & keypoints_1,
 const std::vector<KeyPoint> & keypoints_2,
 const Mat& R, const Mat& t,
 const std::vector<DMatch> & matches );


// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

int main ( int argc, char** argv )
{
   if ( argc != 3 )
   {
       cout<<"usage: pose_estimation_2d2d img1 img2"<<endl;
       return 1;
   }
   //-- 读取图像
   Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
   Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

   vector<KeyPoint> keypoints_1, keypoints_2;
   vector<DMatch> matches;
   find_feature_matches ( img_1, img_2, keyp