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

上回咱们读完了pose_estimation_2d2d.cpp这个文件，基本上明白了通过对极几何计算相机位姿变换的过程，简单地说就是：你给我两帧图像，我给你算个R、t。

我们按部就班，跟着小绿来看一下接下来要读的程序——pose_estimation_3d2d。

这里小绿简单的拿两张图来看一下2d-2d与3d-2d在本质上的区别：

↑两张平面图

↑一张平面图+一张深度图 与一张平面图

这个程序，顾名思义，便是已知一帧图像中特征点的3d位置信息，以及另一帧图像中特征点的2d位置信息，进行相机的位姿变换计算。其中，3d位置信息是指该特征点所对应的真实物体点，在当前相机坐标系下的坐标；2d位置信息则是特征点的像素坐标。这里3d位置信息是由RGB-D相机提供的深度信息进行计算得到的。

我们来看一下子函数声明和主函数：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

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

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

void bundleAdjustment (
   const vector<Point3f> points_3d,
   const vector<Point2f> points_2d,
   const Mat& K,
   Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
   if ( argc != 5 )
   {
       cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<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, keypoints_1, keypoints_2, matches );
   cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

// 建立3D点
   Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
   Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
   vector<Point3f> pts_3d;
   vector<Point2f> pts_2d;
   for ( DMatch m:matches )
   {
       ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
       if ( d == 0 )   // bad depth
           continue;
       float dd = d/5000.0;
       Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
       pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
       pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
   }

cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

Mat r, t;
   solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
   Mat R;
   cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

cout<<"R="<<endl<<R<<endl;
   cout<<"t="<<endl<<t<<endl;

cout<<"calling bundle adjustment"<<endl;

bundleAdjustment ( pts_3d, pts_2d, K, R, t );
   return 0;
}

可以看到一共使用了三个子函数：find_feature_matches、pixel2cam和bundleAdjustment。这里引用白白的一张图：

图中的BA指的就是Bundle Adjustment（光束平差法，即最小化重投影误差），在这里针对本程序则封装为一个函数进行调用。在这三个子函数中，find_feature_matches即特征点匹配，用来匹配两帧图像中的特征点；pixel2cam即像素坐标到归一化平面坐标变换，用来转换坐标：这两杆数是我们研读过的，在此不做赘述。只有第三个子函数bundleAdjustment是个新面孔，的确，他是3d-2d位姿估计中的重点。bundleAdjustment函数无返回值，形参为存储Point3f类对象的容器points_3d、存储Point2f类对象的容器points_2d、Mat类矩阵K、R和t。通过const限定符可以推算该函数是要修改引用调用的R和t，即通过一组点的3d坐标、一组点的2d坐标求取相机位姿变换。我们先来看看主函数，最后再对bundleAdjustment进行梳理。

主函数

if ( argc != 5 )

{
      cout<<"usage:pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
       return 1;
}

这里表示需要额外传入四个参数：img1、img2、depth1和depth2。其实在3d-2d匹配过程中，我们只需要前一帧的深度信息，因此可以将argc的判断改为4，不再传入depth2也是可以的。

Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );

读入了argv[3]对应的参数，即depth_1.png，前一帧的深度信息图，调用cv::imread存入一个480*640的Mat类变量d1。

// 建立3D点
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
   for ( DMatch m:matches )
   {
       ushortd = d1.ptr<unsigned short>(int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
       if ( d == 0 )  // bad depth

continue;

float dd = d/5000.0;
       Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
       pts_3d.push_back ( Point3f ( p1.x*dd,p1.y*dd, dd ) );
       pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
   }

首先定义了两个容器，分别用于存储点的3d坐标和2d坐标，故容器内元素类型分别为Point3f和Point2f。注意，这里将容器命名为pts_3d与pts_2d并不是说其坐标值是double类型的，而是3-dimention与2-dimention。

那么在接下来的循环中，我们就是要将matches中每一对点的坐标分别存入3d容器与2d容器中去。这里我们需要使用前一帧图像中特征点的深度信息，因此需要在深度信息矩阵d中提取出来该特征点所对应的深度信息，于是定义了一个无符号整型变量d，并进行了如下初始化：

ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];

我们分离出其原型：

ushort d = d1.ptr<unsignedshort> ( row )[ cloumn ];

其中使用.ptr函数访问Mat类对象d1的第row行首地址，[ column ]表示本行的第column个对象，整体来看就是获取了d1内第row行第column列的元素的值，存储为uchar类型。注意，图像中第m行第n列的数据（即像素坐标为(m,n)）存储在Mat类对象中，其数据将位于第n行第m列，因此比方说我们要看看像素坐标为(0,1)的灰度值，就需要去找一下灰度矩阵第2行第1列的值，即img.ptr<uchar>(1)[0]。在写程序时不要写反行和列，因为编译时不会报错，但得到的结果会出错，甚至产生越界。

其实这里我们也可以用上一期所学的，使用.at()函数来访问Mat类对象内部的值：

ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];

其结果是一样的（注意keypoints内存储的特征点横纵坐标虽然是像素坐标，为整数，但仍是float类型的，需要强转为int类型）。

float dd = d/5000.0;
Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );

在得到深度信息后，我们将其除以一个常数方便计算（取5000也可能是内定的，这个数可以改，成反比例影响t的求取，对R的结果无影响）。进而，将需要计算3d坐标点（前一帧）的特征点的像素坐标转化为归一化平面坐标，并结合深度信息计算相机坐标系下的坐标：

最终存于Point3f类的容器pts_3d中。

  // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
   Mat r, t;
   solvePnP (pts_3d, pts_2d, K, Mat(), r, t, false );
   Mat R;
   cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

这里神奇的事情出现了：我们调用OpenCV提供的solvePnP函数（并结合罗德里格斯变换），直接求出了旋转矩阵R和平移向量t。好了，大功告成，3d-2d位姿求取拿pnp就直接搞定了。然而，只使用pnp解算出的R、t往往具有较大误差，只能作为估计值，实际应用中还需要构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）（高翔Slambook原话）。下面我们来看一下这个最关键的子函数bundleAdjustment。

bundleAdjustment

void bundleAdjustment (
   const vector< Point3f > points_3d,
   const vector< Point2f > points_2d,
   const Mat& K,
   Mat& R, Mat& t )
{
   // 初始化g2o
   typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  
   // pose 维度为 6, landmark 维度为 3
   Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
   // 线性方程求解器
 
   
   
//     Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
   
      Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );
   
   
   g2o::SparseOptimizer optimizer;
   optimizer.setAlgorithm ( solver );

// vertex
   g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
   Eigen::Matrix3d R_mat;
   R_mat <<
         R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
              R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
              R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
   pose->setId ( 0 );
   pose->setEstimate ( g2o::SE3Quat (
                           R_mat,
                           Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                       ) );
   optimizer.addVertex ( pose );

int index = 1;
   for ( const Point3f p:points_3d )   // landmarks
   {
       g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
       point->setId ( index++ );
       point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
       point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
       optimizer.addVertex ( point );
   }

// parameter: camera intrinsics
   g2o::CameraParameters* camera = new g2o::CameraParameters (
       K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
   );
   camera->setId ( 0 );
   optimizer.addParameter ( camera );

// edges
   index = 1;
   for ( const Point2f p:points_2d )
   {
       g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
       edge->setId ( index );
       edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
       edge->setVertex ( 1, pose );
       edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
       edge->setParameterId ( 0,0 );
       edge->setInformation ( Eigen::Matrix2d::Identity() );
       optimizer.addEdge ( edge );
       index++;
   }

chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
   optimizer.setVerbose ( true );
   optimizer.initializeOptimization();
   optimizer.optimize ( 100 );
   chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
   chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
   cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

cout<<endl<<"after optimization:"<<endl;
   cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

这里需要注意，由于g2o版本改动，高翔的代码会报错（我给注释上了），按照网上的方案修改后运行无误。

将节点定义为李代数形式的第二帧相机位姿，与所有特征点的空间位置；将边定义为每个3D点在第二个相机中的投影。位姿使用李代数形式，为6自由度；空间坐标点为3自由度，因而参数为6、3。这里将参数块求解器重命名为Block（也有的程序将其命名为BlockSlover_6_3等等）。

迭代方式选择列文伯格马夸尔特（LM法），将之前使用PnP求解出的R、t进行传入后，在此作为BundleAdjustment的迭代初值。进而设置好图模型中的节点与边，打开求解器，并指定迭代次数上限为100次，开始迭代求解。最后解出优化后的变换矩阵T。

其实关于使用g2o的BA模块，小绿实在难以深入阐述甚至理解其中的很多代码。包括3d-2d中使用的BA模块，以及下一个.cpp中3d-3d的BA模块，小绿认为完全可以在定义好所需要的类后（视情况需要），将BA模块作为一个函数封装进行调用，即输入给定的3d或2d坐标和相机内参（视情况需要），以及迭代初值R、t，就可以按照已经设定好的图模型进行图优化处理。

本程序在传入R、t之后，虽然没有加const限定符并使用引用调用&，但事实上没有修改（优化）R、t的值，而是直接cout了优化后的变换矩阵T。我们来看一下程序的运行结果：

对比T的左上方3×3矩阵，与PnP直接求得的R，我们可以发现差距并不大；对比T右侧3×1矩阵，与PnP直接求得的t，我们发现差距也不大。但毕竟是经过了一个最小二乘优化问题的求解，既然有些微小的变化，我们宁愿相信BA是起着优化R、t求解的作用的。

好了，本次的3d-2d位姿求解程序解读就这样草草收尾（g2o看不懂，小绿强行将其黑箱化）。在实际的SLAM工程中，优化部分占着极大的比重，不仅因为其计算量巨大，更在于定位、建图的精确程度基本由后端优化的精良所决定。使用图优化理论进行非线性优化其实特别重要，因此g2o库的使用、图模型的建立、边与节点的定义、求解器的定义与初始化与具体使用等细节操作...等等等等，这些小绿都需要日后完善。

最后，希望在SLAM道路上的各位敢于攻坚。共勉。

赞 (0)

相关推荐