VIO学习笔记（六）—— 前端 Frontend

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。
关于前端的内容可以参考高翔博士的《视觉SLAM十四讲》。

前端的工作

SLAM 的框架

通常的 SLAM 框架由前后端共同构成

一些现实的问题

1. 前端的步骤?初始化、正常追踪、丢失处理
2. 相机和路标的建模和参数化
   $$x_{camera}=[R,t],x_{landmark}=?$$
   可能的路标:绝对坐标 xyz,逆深度 ρ,灰度值(灰度 Pattern),等等
3. 关键帧?
   需不需要关键帧?
   怎么选关键帧?
   控制关键帧数量?
   仅在关键帧中补充新特征点?还是对所有帧提取新特征点?
   何时进行三角化?

前后端在各自领域处理效果上的差异

实际上, 前端非常能 体现一个 SLAM 的追踪效果。
在实现中,尽管 后端存在明显的理论 差异,但 很难直观体验在最终精度上。
问:假设给定相同的 Pose 和 Landmark,给定噪声范围,各种方法是否有明显差异?还是仅仅有理论性质上的差异?
            

尽管存在上述问题但是还可以问：

1. 实际情况离理论假设有多远?(高斯噪声)
2. 拿到的数据究竟有多好?

在很多实际场合,很难回答某种后端算法是否明确优于另一种。

例如:ORB-SLAM2 使用 Covisibility-Graph,DSO 使用带边缘化的滑动窗口,Tango 使用 MSCKF,等等。

实验效果:ORB-SLAM2 具有较好的全局精度,但无回环时 DSO具有漂移较少。Tango 计算量明显少于其他算法。

算法的结果和数据集关系很大。例如:Kitti 属于比较简单的(视野开阔,动态物体少,标定准确),EUROC 一般(人工设定场景,纹理丰富,但曝光有变化),TUM-Mono 比较难(场景多样,主要为真实场景)

相比来说,前端的差异就比较明显:

1. 追踪算法是否很容易丢失?
2. 算法对干扰的鲁棒性如何(光照、遮挡、动态物体)?

前端更多是范式(Paradigm)之间的比较,而非范式之内的比较。

1. 好的前端
   追踪效果好,不容易丢
   计算速度快
   累计误差小
2. 不好的前端
   追踪效果差
   计算耗时
   容易出现累计误差

VO 方法的定性比较:

几个要点

1. 光流法最早,最成熟,缺点也明显(抗光照干扰弱,依赖角点)
2. FAST+ 光流是比较实用的快速算法/GFTT+ 光流效果更好,也具备实时性
3. 特征匹配需要提的特征具有旋转平移缩放不变性,SIFT/SURF 是最好的一类,BRISK 等次之,ORB 算比较差的
4. 特征匹配和光流都非常依赖角点,日常生活场景中角点不明显的很多
5. 直接法不依赖角点,但实现效果根据选点数量变化较大.
   

特征点提取、匹配和光流

特征点提取

我们后文以传统光流为主来展开前端的介绍。
一个传统的双目光流前端流程(正常追踪流程):
  

特征点提取

在光流中,我们通常选择角点来追踪。
            

为什么需要角点?

角点的梯度在两个方向都有分布:
              

FAST/GFTT 角点:

FAST:仅含像素亮度、不含计算的快速角点提取方式;
GFTT:在 Harris 基础改进:Shi-tomasi 分数,增加固定选点数,等等
                

光流

光流可以追踪一个时刻的角点在下个时刻的图像位置

灰度不变假设

灰度不变假设:
            
一阶 Taylor 展开:

得到:
                

带 Warp function 的光流:

      
其中 W 为 Warp Function,通常取仿射变换
 
其中 $p_1-p_6$ 为 W 的参数,需要在线估计。

金字塔式光流

      
Coarse-to-Fine:从顶层最模糊的图像开始计算光流,一直往下迭代到最高分辨率

光流在 SLAM 中的运用

1. 光流可以追踪上一帧的角点
2. 可以一直追踪该角点,直到超出图像范围或被遮挡
3. 在单目 SLAM 中,新提出的角点没有 3D 信息,因此可通过追踪角点在各图像位置,进行三角化

光流的局限性

1. 容易受光照变化影响
2. 只适合连续图像中的短距离追踪,不适合更长距离
3. 图像外观发生明显变化时不适用(例:远处的角点凑近看之后不为角点了)
4. 对角点强依赖,对 Edge 类型点表现较差
5. 稀疏光流不约束各点光流的方向统一,可能出现一些 outlier。

关键帧与三角化

关键帧

为什么需要关键帧

1. 后端通常实时性较差,不适合处理所有帧;
2. 如果相机停止,可能给后端留下无用的优化,甚至导致后端问题退化(尺度问题)

如何选择关键帧

1. 关键帧之间不必太近(退化或三角化问题)
2. 关键帧之间不能太远(共视点太少)
3. VIO 中,定期选择关键帧(假设 $b^g$ , $b^a$ 在关键帧期间不变)

关键帧策略

对于非关键帧,只执行前端算法,不参与后端优化
因此,对于非关键帧,它的误差会逐渐累积。只有该帧被作为关键帧插入后端,BA 才会保证窗口内的一致性

总结关键帧选取的策略:在计算量允许范围内,且不引起退化时,应尽可能多地插入关键帧。

ORB-SLAM2

            
ORB-SLAM2 使用非常宽松的关键帧策略(大多数时候只要后端线程Idle 就会插入关键帧),然后在后端剔除冗余的关键帧。

DSO

            
DSO 利用光度误差插入关键帧(插入比较频繁)。然后在后端计算每个关键帧的 Active Landmarks,Marg 对窗口贡献最低的。因此,DSO 的关键帧窗口通常有一个很远的和两三个很近的,其他几个分布在中间。

三角化

在单目 SLAM 中,通常在插入关键帧时计算新路标点的三角化。

1. 有的 SLAM 系统在关键帧时提取新 Feature(DSO, SVO),也有方案对每个帧都提新 Feature(VINS, ORB)。
2. 前者节省计算量(非关键帧无需提点,节省 5-10ms 左右);后者效果好(在单目里需要防止三角化 Landmark 数量不够)。

三角化的数学描述

1. 考虑某路标点 y 在若干个关键帧 k = 1, · · · , n 中看到。
2. $y\in R^4$ ,取齐次坐标。每次观测为 $x_k=[u_k,v_k,1{]}^{\top }$ ,取归一化平面坐标(这样可以忽略掉内参)。
3. 记投影矩阵 $P_k=[R_k,t_k]\in R^{3\times 4}$ ,为 World 系到 Camera 系
4. 投影关系:
   $$\forall k,{\lambda }_k{x}_k=P_{k}y.$$
   其中${\lambda }_k$ 为观测点的深度值(未知)
   根据上式第三行:
   $${\lambda }_k=P_{k,3}^{\top }y$$
   其中 $P_{k,3}^{\top }$为 $P_k$ 的第 3 行。
5. 将上式代入上上式的前两行:
   $$\begin{gathered} u_k{P}_{k,3}^{\top }y=P_{k,1}^{T}y \\ {u}_k{P}_{k,3}^{\top }y=P_{k,2}^{T}y \end{gathered}$$
6. 每次观测将提供两个这样的方程,视 y 为未知量,并将 y 移到等式一侧:
             
7. 于是,y 为 D 零空间中的一个非零元素。
8. 由于 $D\in R^{2n\times 4}$ ,在观测次于大于等于两次时,很可能 D 满秩,无零空间。
9. 寻找最小二乘解:
   $$\underset{y}{\min }\Vert Dy{\Vert }_2^2,s.t.\Vert y\Vert =1$$
   解法:对 $D^{\top }D$ 进行 SVD(再去研究一下矩阵论的内容):
   $$D^{\top }D=\sum _{i=1}^4{\sigma }_i^2{u}_i{u}_j^{\top }$$
   其中 ${\sigma }_i$ 为奇异值,且由大到小排列,$u_i$ , $u_j$ 正交。
10. 此时,取 $y=u_4$ (为什么?),那么该问题的目标函数值为 ${\sigma }_4$ 。
11. 判断该解有效性的条件:${\sigma }_4\ll {\sigma }_3$ 。若该条件成立,认为三角化有效,否则认为三角化无效。
12. Rescale:在某此场合(比如我们使用 UTM 坐标的平移),D 的数值大小差异明显,导致解在数值上不稳定。此时对 D 做一个缩放:
                   
    其中 S 为一个对角阵,取 D 最大元素之逆即可。
13. 实用当中,还需要加上 y 投影正负号的判定作为依据

以上,我们推导了对任意次观测的三角化算法,可作为通用的依据。

三角化测试代码

//
// Created by hyj on 18-11-11.
//
#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

struct Pose
{
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;
    // 这帧图像观测到的特征坐标
    Eigen::Vector2d uv;    
};

int main()
{

    int poseNums = 10;
    double radius = 8;
    double fx = 1.;
    double fy = 1.;
    std::vector<Pose> camera_pose;
    for(int n = 0; n < poseNums; ++n ) {
        double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        camera_pose.push_back(Pose(R,t));
    }

    // 随机数生成 1 个 三维特征点
    std::default_random_engine generator;
    std::uniform_real_distribution<double> xy_rand(-4, 4.0);
    std::uniform_real_distribution<double> z_rand(8., 10.);
    double tx = xy_rand(generator);
    double ty = xy_rand(generator);
    double tz = z_rand(generator);

    Eigen::Vector3d Pw(tx, ty, tz);
    // 这个特征从第三帧相机开始被观测，i=3
    int start_frame_id = 3;
    int end_frame_id = poseNums;
    for (int i = start_frame_id; i < end_frame_id; ++i) {
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);

        double x = Pc.x();
        double y = Pc.y();
        double z = Pc.z();

        camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
    }
    
    /// TODO::homework; 请完成三角化估计深度的代码
    // 遍历所有的观测数据，并三角化
    Eigen::Vector3d P_est;           // 结果保存到这个变量
    P_est.setZero();
    /* your code begin */
    
    //step1:constuct D
    Eigen::Matrix<double, 14, 4> D;
    for (int i = start_frame_id; i < end_frame_id; ++i) {

        Eigen::Matrix<double,3,4> Tcw;
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d tcw = -Rcw*camera_pose[i].twc;//注意是-:Pc=Pc0-tcw
        Tcw<< Rcw, tcw;

        //rows = mat.rows(2);  //  获取第3行
        Eigen::Vector2d uv = camera_pose[i].uv ;

        D.block(2*(i - start_frame_id), 0, 1,4) = uv[0]*Tcw.row(2) - Tcw.row(0);
        D.block(2*(i - start_frame_id)+1, 0, 1,4) = uv[1]*Tcw.row(2) - Tcw.row(1);

    }
    
    std::cout<<"D Matrix is :"<<D<<std::endl;

    //step2:recale D,找到D中最大元素
    Eigen::MatrixXd::Index maxRow, maxCol;
    double max = D.maxCoeff(&maxRow,&maxCol);
    //注意是D的绝对值中最大元素的D.cwiseAbs().maxCoeff()
    std::cout << "Max = \n" << max <<"行:"<<maxRow<<"列"<<maxCol<< std::endl;

    Eigen::MatrixXd DtD((D/max).transpose()*(D/max));

    clock_t time_stt = clock();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(DtD, Eigen::ComputeThinU | Eigen::ComputeThinV );

    clock_t time_stt1 = clock();
    
    std::cout<<"SVD分解，耗时:\n"<<(clock()-time_stt1)/(double) CLOCKS_PER_SEC<<"ms"<<std::endl; 

    // 构建SVD分解结果 Eigen::MatrixXd U = svd_holder.matrixU(); Eigen::MatrixXd V = svd_holder.matrixV(); 
    Eigen::MatrixXd S(svd_holder.singularValues()); 
    std::cout<<"singularValues :\n"<<S<<std::endl;

    //step3:判断分解是否有效,然后求解y
    if(std::abs(svd_holder.singularValues()[3]/svd_holder.singularValues()[2]) < 1e-2){

	Eigen::Vector4d U4 = (max*svd_holder.matrixU().rightCols(1));//最后一列
	P_est = (U4/U4[3]).head(3);

    }
    else{

	std::cout<<"这次求解无效"<<std::endl;
    }
    
    /* your code end */
    
    std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
    std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
    // TODO:: 请如课程讲解中提到的判断三角化结果好坏的方式，绘制奇异值比值变化曲线
    return 0;
}

参考博客：

1. https:///orange_littlegirl/article/details/103512138