SLAM十四讲-设计前端-0.4-代码解读

最新推荐文章于 2022-10-06 19:12:49 发布

Timmy_Handsome_Cheng

最新推荐文章于 2022-10-06 19:12:49 发布

阅读量1.1k

点赞数 2

分类专栏： SLAM

本文链接：https://blog.csdn.net/Timmy_attack/article/details/88061341

版权

SLAM 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

本博文的创作目的是对高翔博士的《视觉SLAM十四讲》第九讲——实践：设计前端，0.4部分的代码进行解读，以作为阅读的笔记，若有错误之处，欢迎留言讨论。

基本类

基本类的关系示意图
上图是工程中所用到的基本的类关系图。
Cmara类可以看做是相机的模型，用来存储相机的内参，并提供完成相机坐标系、像素坐标系和世界坐标系之间的坐标转换的方法。

相机内参：一般在数学上用内参数矩阵K表示 $f_x \ \ 0 \ \ c_x; \ 0 \ \ f_y \ \ c_y; \ 0 \ \ 0 \ \ 1]$ ，相机内参可以根据张正友经典标定法来确定。
相机外参：相机的位姿 $R, t$ 又称为相机的外参数。

Frame类是基本的数据单元，用来存储图像帧，每获取一张图片就会有一个Frame实例对象，所以在类中要用标号去区分每一个Frame对象。该类用于存储每帧图像的时间戳、外参、像素矩阵（彩色图像和深度图像）、关键像素点等。

彩色图用RGB分量，是一个 $c o l s * r o w s * 3$ 的三维矩阵
深度图只有对应像素的深度信息，是一个 $c o l s * r o w s$ 的二维矩阵

MapPoint类用来存储每帧图像的关键像素点的描述子，以及该像素点的世界坐标等信息。可以存着多个MapPoint对象对应一个Frame对象。

Map类是将所有的MapPoint对象管理起来，形成一个地图用于优化，并提供加入关键帧和插入MapPoint的方法。

Config类提供文件操作的方法，用于读取配置文件，并且在程序的任意地方都可随时提供参数的值，所以要把Config类写成单件模式，即将Config类的公共构造函数声明为私有，防止该类对象在别处建立。

单件模式是一种用于确保整个应用程序中只有一个类实例且这个实例所占资源在整个应用程序中是共享时的程序设计方法

VisualOdometry类将视觉里程计的状态，Map指针、Frame指针，关键点、描述子、匹配信息、配置文件内容等参数管理起来，并提供关键点提取匹配、优化等方法。

关键点：是指图片的特征点，这些特征点在相似的两张图片中容易被识别出来，使两张图片能够根据这些特征点匹配。在这里用到的关键点是FAST关键点，它是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。
描述子：是对提取出来的特征点的周围像素区进行描述。BRIEF描述子是一种二进制描述子，其描述向量可以表述不同的关键点。

程序流程

程序流程图
整个里程计的工作流程就是从数据集中读取一帧图像，然后根据上一帧图像与当前帧图像来估计相机位姿，然后根据整个地图来优化估计所得的位姿。

配置文件

读取配置文件用到的是Config类，它在setparameterFile时构造，而设计的构造对象是Config的智能指针：static shared_ptr <Config> config

namespace myslam 
{
class Config
{
private:
    static std::shared_ptr<Config> config_; 
    cv::FileStorage file_;
    
    Config () {} // private constructor makes a singleton
public:
    ~Config();  // close the file when deconstructing 
    
    // set a new config file 
    static void setParameterFile( const std::string& filename ); 
    
    // access the parameter values
    template< typename T >
    static T get( const std::string& key )
    {
        return T( Config::config_->file_[key] );
    }
};
}

我们要读取的配置文件是config文件夹里的default.yaml文件，用get函数即可获取文件中的对应关键字的参数内容。

%YAML:1.0
# data
# the tum dataset directory, change it to yours! 
dataset_dir: /home/xiang/dataset/rgbd_dataset_freiburg1_desk

# camera intrinsics
# fr1
camera.fx: 517.3
camera.fy: 516.5
camera.cx: 325.1
camera.cy: 249.7

# fr2
#camera.fx: 520.9
#camera.fy: 521.0
#camera.cx: 325.1
#camera.cy: 249.7

camera.depth_scale: 5000

# VO Parameters
number_of_features: 500
scale_factor: 1.2
level_pyramid: 4
match_ratio: 2.0
max_num_lost: 10
min_inliers: 30
keyframe_rotation: 0.1
keyframe_translation: 0.1
map_point_erase_ratio: 0.1

viz

OpenCV的viz模块可用于3D显示，它需要先实例化化一个viz的3d视图对象，再设置各个坐标轴的参数。

    // visualization
    cv::viz::Viz3d vis ( "Visual Odometry" );
    cv::viz::WCoordinateSystem world_coor ( 1.0 ), camera_coor ( 0.5 );
    cv::Point3d cam_pos ( 0, -1.0, -1.0 ), cam_focal_point ( 0,0,0 ), cam_y_dir ( 0,1,0 );
    cv::Affine3d cam_pose = cv::viz::makeCameraPose ( cam_pos, cam_focal_point, cam_y_dir );
    vis.setViewerPose ( cam_pose );

    world_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 2.0 );
    camera_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 1.0 );
    vis.showWidget ( "World", world_coor );
    vis.showWidget ( "Camera", camera_coor );

数学原理

0.4用到的求解相机运动的方法是PnP，所以在本博文中只简介PnP方法，其他方法可以参考《视觉SLAM十四讲》第7讲。PnP（Perspective-n-Point）是求解3D到2D点对运动的方法。在RGB-D的视觉里程计中，特征点的3D位置可以由RGB-D相机的深度图确定。当两张图片的其中一张特征点的3D位置已知，那么最少只需3个点对就可以估计相机运动。
假设空间中的某一点 $P$ ，其世界坐标为 $P=(X,Y,Z)^T$ ，该点在图像 $I_1$ 中，投影到特征点 $x_1=(u_1,v_1,1)^T$ （以归一化平面齐次坐标表示的像素坐标）。由于我们需要求解的是 $p_{uv}=RP_w+t$ 中的旋转信息 $R$ 和平移信息 $t$ ，所以我们定义增广矩阵 $[R ∣ t]$ 为一个 $\times 4$ 的矩阵 $T=[t_1,t_2,t_3]^T$ ，其中 $t_1=[a_1,a_2,a_3,a_4]^T,t_2=[a_5,a_6,a_7,a_8]^T,t_3=[a_9,a_{10},a_{11},a_{12}]^T$ 。我们将 $p_{uv}=RP_w+t$ 展开成下式：
$\begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} = \begin{bmatrix} a_1 & a_2 & a_3 &a_4 \\ a_5 & a_6 & a_7 &a_8 \\ a_9 & a_{10} & a_{11} &a_{12} \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix} \tag{1}$
将其展开得到（此处 $P_w$ 是三维向量， $t$ 是四维向量，故存在非齐次先齐次转换的过程被省略，读者在计算时只需在 $P_w$ 最后加上1使其成为齐次坐标即可，如式(1)中右侧）：
$su_1={t_1}^TP_w \\ sv_1={t_2}^TP_w \\ s={t_3}^TP_w \tag{2}$
将式(2)中最后一行代入前两行把 $s$ 消去，便得到两个约束：
${t_1}^TP_w - {t_3}^TP_wu_1 = 0 \\ {t_2}^TP_w - {t_3}^TP_wv_1 = 0 \tag{3}$
每个特征点提供了式(3)的两个关于 $t$ 的线性约束
由于 $t$ 一共有12维，因此，最少通过6对匹配点即可实现矩阵 $T$ 的线性求解，这种方法成为直接线性变换（Direct Linear Transform，DLT）。但DLT求解中，我们直接求出将 $T$ 矩阵看成12个未知数，忽略了旋转矩阵 $\in SO(3)$ 的关系，所以用DLT求出来的解不一定满足 $\in SO(3)$ ，而只是个一般矩阵。平移向量 $t$ 属于向量空间，但对于旋转矩阵 $R$ ，我们必须针对 $T$ 右边的 $\times 3$ 矩阵块用一个最好的旋转矩阵对它进行类似，这部分可由QR分解完成。
当匹配点大于6对时，也可以使用SVD等方法对超定方程求最小二乘解。
在OpenCV中可以直接通过cv::solvePnPRansac来完成。

计算里程计

里程计计算流程
计算里程计的过程有三种状态，INITIALIZING/OK/LOST。在实例化VisualOdometry类时就将status_置为INITIALIZING。
处理第一帧图像时不需要与其他图像进行匹配，只需计算描述子，并作为KeyFrame加入地图。将status_状态置为OK。
在接下来的图像处理工作都会在status_为OK的流程中进行（上图中间）。在计算出描述子后就可以估计位姿并优化，接着对估计的位姿继续评估，若为好的位姿则插入地图，若为坏的位姿则统计丢失数+1，继续用下一帧图像计算。知道丢失数统计值大于设定值时将status_设为LOST，并退出里程计计算。
在整个里程计计算的过程中位姿估计和优化是较为重要的部分，本博文接下来回对这两部分的代码进行解释。

位姿估计

/*  max 注释
*   函数功能：用ransac的方式求解PnP问题
*
*   参数：
*   [in]    _opoints                参考点在世界坐标系下的点集；float or double
*   [in]    _ipoints                参考点在相机像平面的坐标；float or double
*   [in]    _cameraMatrix           相机内参
*   [in]    _distCoeffs             相机畸变系数
*   [out]   _rvec                   旋转矩阵
*   [out]   _tvec                   平移向量
*   [in]    useExtrinsicGuess       若果求解PnP使用迭代算法，初始值可以使用猜测的初始值（true），也可以使用解析求解的结果作为初始值（false）。
*   [in]    iterationsCount         Ransac算法的迭代次数，这只是初始值，根据估计外点的概率，可以进一步缩小迭代次数；（此值函数内部是会不断改变的）,所以一开始可以赋一个大的值。
*   [in]    reprojectionErrr        Ransac筛选内点和外点的距离阈值，这个根据估计内点的概率和每个点的均方差（假设误差按照高斯分布）可以计算出此阈值。
*   [in]    confidence              此值与计算采样（迭代）次数有关。此值代表从n个样本中取s个点，N次采样可以使s个点全为内点的概率。
*   [out]   _inliers                返回内点的序列。为矩阵形式
*   [in]    flags                   最小子集的计算模型；
*                                                 SOLVEPNP_ITERATIVE(此方案，最小模型用的EPNP，内点选出之后用了一个迭代)；
*                                                 SOLVE_P3P(P3P只用在最小模型上，内点选出之后用了一个EPNP)      
*                                                 SOLVE_AP3P(AP3P只用在最小模型上，内点选出之后用了一个EPNP)
*                                                 SOLVE_EPnP(最小模型上&内点选出之后都采用了EPNP)
*    返回值：
*         成功返回true，失败返回false；
*/
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
	InputArray _cameraMatrix, InputArray _distCoeffs,
	OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
	int iterationsCount, float reprojectionError, double confidence,
	OutputArray _inliers, int flags)
---------------
作者：xuelangwin 
来源：CSDN 
原文：https://blog.csdn.net/xuelangwin/article/details/80847337 
版权声明：本文为博主原创文章，转载请附上博文链接！

在位姿估计中用到的一个比较重要的函数是solvePnPRansac，它的参数解释如上述所示，其作用是用Ransac的方法来求解PnP问题。Ransac方法是一种迭代求解局部最优化问题的方法，关于Ransac方法可以参考《RANSAC算法讲解》
RANSAC的基本假设是：
（1）数据由“局内点”组成，例如：数据的分布可以用一些模型参数来解释；
（2）“局外点”是不能适应该模型的数据；
（3）除此之外的数据属于噪声。
局外点产生的原因有：噪声的极值；错误的测量方法；对数据的错误假设。
RANSAC也做了以下假设：给定一组（通常很小的）局内点，存在一个可以估计模型参数的过程；而该模型能够解释或者适用于局内点。
在这里插入图片描述
局内点就是上图直线附近的点，可以用于拟合模型；局外点就是偏离模型的点。在视觉里程计中，局内点可以用于估计相机位姿，而局外点则干扰位姿的估计。

优化

0.4的设计前端用的是图优化来对估计的位姿进行调整。

所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。
图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。边是用来连接不同的顶点，可以是有向或者无向，表示顶点之间的一种关系。

相机位姿pose是我们的优化变量，用作为顶点Vertex；误差项用作为边Egde，由类EdgeProjectXYZ2UVPoseOnly提供，误差项则是当前图像关键点的像素坐标与根据估计位姿转换的像素坐标的差值。优化原理不是太懂，这里只大概看懂了代码，后续会专门写一篇相关的博文来说明。

    // using bundle adjustment to optimize the pose
    //初始化图
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* solver_ptr = new Block ( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
	//添加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
        T_c_w_estimated_.rotation_matrix(), T_c_w_estimated_.translation()
    ));
    optimizer.addVertex ( pose );

    // edges，添加边
    for ( int i=0; i<inliers.rows; i++ )
    {
        int index = inliers.at<int> ( i,0 );
        // 3D -> 2D projection
        EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
        edge->setId ( i );
        edge->setVertex ( 0, pose );
        edge->camera_ = curr_->camera_.get();
        edge->point_ = Vector3d ( pts3d[index].x, pts3d[index].y, pts3d[index].z );
        edge->setMeasurement ( Vector2d ( pts2d[index].x, pts2d[index].y ) );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        // set the inlier map points 
        match_3dpts_[index]->matched_times_++;
    }
	//开始优化
    optimizer.initializeOptimization();
    optimizer.optimize ( 10 );
	//输出结果
    T_c_w_estimated_ = SE3 (
        pose->estimate().rotation(),
        pose->estimate().translation()
    );
    
    cout<<"T_c_w_estimated_: "<<endl<<T_c_w_estimated_.matrix()<<endl;

void EdgeProjectXYZ2UVPoseOnly::computeError()
{
    const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
    _error = _measurement - camera_->camera2pixel ( 
        pose->estimate().map(point_) );
}