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

最新推荐文章于 2022-10-31 18:36:23 发布

梁小憨憨

最新推荐文章于 2022-10-31 18:36:23 发布

阅读量742

点赞数

分类专栏： VIO 文章标签： slam

本文链接：https://blog.csdn.net/qq_41990294/article/details/105374545

版权

VIO 专栏收录该内容

11 篇文章 17 订阅

订阅专栏

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

VIO学习笔记（一）—— 概述
 VIO学习笔记（二）—— IMU 传感器
 VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合
 VIO学习笔记（四）—— 基于滑动窗口算法的 VIO 系统:可观性和一致性
 VIO学习笔记（五）—— 后端优化实践:逐行手写求解器

前端的工作

SLAM 的框架

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

前端:提取 特征点,追踪相机 Pose, 定位相机
后端:提供 全局优化或 滑动窗口优化

在这里插入图片描述

一些现实的问题

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

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

实际上, 前端非常能 体现一个 SLAM 的追踪效果。
在实现中,尽管 后端存在明显的理论 差异,但 很难直观体验在最终精度上。
问:假设给定相同的 Pose 和 Landmark,给定噪声范围,各种方法是否有明显差异?还是仅仅有理论性质上的差异?
在这里插入图片描述
在某些理想情况下,利用仿真数据可以给出一定的结果:

这个实验(双目 Pose/Landmark 估计)中,UKF(SPKF) 给出最接近真值的结果。

所以说：尽管后端存在理论上的差异，但是最终处理的效果并不好从直观的感受上体现出来，但不同的前端，针对不同的数据集会有比较好的效果，但可能并不一定适合所有数据集，或者说在处理实际情况数据时效果可能不理想。

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

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

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

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

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

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

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

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

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

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

VO 方法的定性比较:

在这里插入图片描述

几个要点

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

DSO 在不同关键帧数量/不同选点数量下的表现。曲线越靠左上侧越好。

特征点提取、匹配和光流

特征点提取

我们后文以传统光流为主来展开前端的介绍。
一个传统的双目光流前端流程(正常追踪流程):
在这里插入图片描述
除了正常追踪流程以外,还需要考虑初始化、丢失恢复的情况。
VIO 初始化比常规 VO 多一些步骤(主要为 IMU 参数的估计)

特征点提取

在光流中,我们通常选择角点来追踪。
在这里插入图片描述

为什么需要角点?

角点的梯度在两个方向都有分布:
在这里插入图片描述
利用角点附近块的两个特征值大小,可以判断该区是否为角点。

FAST/GFTT 角点:

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

光流

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

灰度不变假设

灰度不变假设:
在这里插入图片描述
一阶 Taylor 展开:

得到:

带 Warp function 的光流:

在这里插入图片描述

其中 W 为 Warp Function,通常取仿射变换

其中 $p _1 − p _6$ 为 W 的参数,需要在线估计。

金字塔式光流

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

光流在 SLAM 中的运用

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

光流的局限性

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

关键帧与三角化

关键帧

为什么需要关键帧

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

如何选择关键帧

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

关键帧策略

对于非关键帧,只执行前端算法,不参与后端优化
因此,对于非关键帧,它的误差会逐渐累积。只有该帧被作为关键帧插入后端,BA 才会保证窗口内的一致性
在这里插入图片描述
总结关键帧选取的策略:在计算量允许范围内,且不引起退化时,应尽可能多地插入关键帧。

ORB-SLAM2

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

DSO

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

三角化

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

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

三角化的数学描述

考虑某路标点 y 在若干个关键帧 k = 1, · · · , n 中看到。
$y ∈ R^4$ ,取齐次坐标。每次观测为 $x _k = [u _k , v _k , 1] ^⊤$ ,取归一化平面坐标(这样可以忽略掉内参)。
记投影矩阵 $P _k = [R _k , t_ k ] ∈ R ^{3×4}$ ,为 World 系到 Camera 系
投影关系:
$k, λ _k x_ k = P_ k y.$
其中 $λ _k$ 为观测点的深度值(未知)
根据上式第三行:
$λ_ k = P ^⊤_{k,3} y$
其中 $P ^⊤_{k,3}$ 为 $P _k$ 的第 3 行。
将上式代入上上式的前两行:
$u_k P^ ⊤_{k,3} y = P_{ k,1}^T y\\ u_k P^ ⊤_{k,3} y = P_{ k,2}^T y$
每次观测将提供两个这样的方程,视 y 为未知量,并将 y 移到等式一侧:
于是,y 为 D 零空间中的一个非零元素。
由于 $D ∈ R ^{2n×4}$ ,在观测次于大于等于两次时,很可能 D 满秩,无零空间。
寻找最小二乘解:
$min_y ∥Dy∥ ^2_2 , s.t. ∥y∥ = 1$
解法:对 $D ^⊤ D$ 进行 SVD(再去研究一下矩阵论的内容):
$^⊤ D =∑^4_{i=1}σ_ i^ 2 u_ i u_ j^ ⊤$
其中 $σ _i$ 为奇异值,且由大到小排列, $u_ i$ , $u_ j$ 正交。
此时,取 $y = u _4$ (为什么?),那么该问题的目标函数值为 $σ _4$ 。
判断该解有效性的条件: $σ _4 ≪ σ_ 3$ 。若该条件成立,认为三角化有效,否则认为三角化无效。
Rescale:在某此场合(比如我们使用 UTM 坐标的平移),D 的数值大小差异明显,导致解在数值上不稳定。此时对 D 做一个缩放:

其中 S 为一个对角阵,取 D 最大元素之逆即可。
实用当中,还需要加上 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;
}

参考博客：

https://blog.csdn.net/orange_littlegirl/article/details/103512138

梁小憨憨

关注

0
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
VIO学习笔记（六）—— 前端 Frontend

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。关于前端的内容可以参考高博博士的《视觉SLAM十四讲》。VIO学习笔记（一）—— 概述VIO学习笔记（二）—— IMU 传感器VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合VIO学习笔记（四）—— 基于滑动窗口算...
复制链接

扫一扫