学习资料是深蓝学院的《从零开始手写VIO》课程,对课程做一些记录,方便自己以后查询,如有错误还请斧正。由于习惯性心算公式,所以为了加深理解,文章公式采用手写的形式。
关于前端的内容可以参考高翔博士的《视觉SLAM十四讲》。
VIO学习笔记(一)—— 概述
VIO学习笔记(二)—— IMU 传感器
VIO学习笔记(三)—— 基于优化的 IMU 与视觉信息融合
VIO学习笔记(四)—— 基于滑动窗口算法的 VIO 系统:可观性和 一致性
VIO学习笔记(五)—— 后端优化实践:逐行手写求解器
前端 Frontend
前端的工作
SLAM 的框架
通常的 SLAM 框架由前后端共同构成
- 前端:提取
特征点
,追踪相机Pos
e,定位
相机 - 后端:提供
全局优化
或滑动窗口
优化
一些现实的问题
- 前端的步骤?初始化、正常追踪、丢失处理
- 相机和路标的建模和参数化
x c a m e r a = [ R , t ] , x l a n d m a r k = ? x _{camera} = [R, t], x_{ landmark} = ? xcamera=[R,t],xlandmark=?
可能的路标:绝对坐标 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
p _1 − p _6
p1−p6 为 W 的参数,需要在线估计。
金字塔式光流
Coarse-to-Fine:从顶层最模糊的图像开始计算光流,一直往下迭代到最高分辨率
光流在 SLAM 中的运用
- 光流可以追踪上一帧的角点
- 可以一直追踪该角点,直到超出图像范围或被遮挡
- 在单目 SLAM 中,新提出的角点没有 3D 信息,因此可通过追踪角点在各图像位置,进行三角化
光流的局限性
- 容易受光照变化影响
- 只适合连续图像中的短距离追踪,不适合更长距离
- 图像外观发生明显变化时不适用(例:远处的角点凑近看之后不为角点了)
- 对角点强依赖,对 Edge 类型点表现较差
- 稀疏光流不约束各点光流的方向统一,可能出现一些 outlier。
关键帧与三角化
关键帧
为什么需要关键帧
- 后端通常实时性较差,不适合处理所有帧;
- 如果相机停止,可能给后端留下无用的优化,甚至导致后端问题退化(尺度问题)
如何选择关键帧
- 关键帧之间不必太近(退化或三角化问题)
- 关键帧之间不能太远(共视点太少)
- VIO 中,定期选择关键帧(假设 b g b ^g bg , b a b^ a ba 在关键帧期间不变)
关键帧策略
对于非关键帧,只执行前端算法,不参与后端优化
因此,对于非关键帧,它的误差会逐渐累积。只有该帧被作为关键帧插入后端,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 y ∈ R^4 y∈R4 ,取齐次坐标。每次观测为 x k = [ u k , v k , 1 ] ⊤ x _k = [u _k , v _k , 1] ^⊤ xk=[uk,vk,1]⊤ ,取归一化平面坐标(这样可以忽略掉内参)。
- 记投影矩阵 P k = [ R k , t k ] ∈ R 3 × 4 P _k = [R _k , t_ k ] ∈ R ^{3×4} Pk=[Rk,tk]∈R3×4 ,为 World 系到 Camera 系
- 投影关系:
∀ k , λ k x k = P k y . ∀k, λ _k x_ k = P_ k y. ∀k,λkxk=Pky.
其中 λ k λ _k λk 为观测点的深度值(未知)
根据上式第三行:
λ k = P k , 3 ⊤ y λ_ k = P ^⊤_{k,3} y λk=Pk,3⊤y
其中 P k , 3 ⊤ P ^⊤_{k,3} Pk,3⊤为 P k P _k Pk 的第 3 行。 - 将上式代入上上式的前两行:
u k P k , 3 ⊤ y = P k , 1 T y u k P k , 3 ⊤ y = P k , 2 T y u_k P^ ⊤_{k,3} y = P_{ k,1}^T y\\ u_k P^ ⊤_{k,3} y = P_{ k,2}^T y ukPk,3⊤y=Pk,1TyukPk,3⊤y=Pk,2Ty - 每次观测将提供两个这样的方程,视 y 为未知量,并将 y 移到等式一侧:
- 于是,y 为 D 零空间中的一个非零元素。
- 由于 D ∈ R 2 n × 4 D ∈ R ^{2n×4} D∈R2n×4 ,在观测次于大于等于两次时,很可能 D 满秩,无零空间。
- 寻找最小二乘解:
min y ∥ D y ∥ 2 2 , s . t . ∥ y ∥ = 1 \min_y ∥Dy∥ ^2_2 , s.t. ∥y∥ = 1 ymin∥Dy∥22,s.t.∥y∥=1
解法:对 D ⊤ D D ^⊤ D D⊤D 进行 SVD(再去研究一下矩阵论的内容):
D ⊤ D = ∑ i = 1 4 σ i 2 u i u j ⊤ D ^⊤ D =∑^4_{i=1}σ_ i^ 2 u_ i u_ j^ ⊤ D⊤D=i=1∑4σi2uiuj⊤
其中 σ i σ _i σi 为奇异值,且由大到小排列, u i u_ i ui , u j u_ j uj 正交。 - 此时,取 y = u 4 y = u _4 y=u4 (为什么?),那么该问题的目标函数值为 σ 4 σ _4 σ4 。
- 判断该解有效性的条件: σ 4 ≪ σ 3 σ _4 ≪ σ_ 3 σ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