(1) 如果直接对H矩阵求逆来计算增量方程,复杂度为O( n 3 n^3 n3).
(2) 可以利用H的稀疏结构来加速运算:
将H Δx = g 的H矩阵划分为4个块: [ B E E T C ] \begin{bmatrix} B & E~ \\ E^T & C~ \end{bmatrix} \quad [BETE C ] [ Δ x c Δ x p ] \begin{bmatrix} Δxc~ \\ Δxp~ \end{bmatrix} \quad [Δxc Δxp ] = [ v w ] ~\begin{bmatrix} v~ \\ w~ \end{bmatrix} \quad [v w ]
=> [ B − E C − 1 E T 0 E T C ] \begin{bmatrix} B-EC^-1 E^T & 0~ \\ E^T & C~ \end{bmatrix} \quad [B−EC−1ETET0 C ] [ Δ x c Δ x p ] \begin{bmatrix} Δxc~ \\ Δxp~ \end{bmatrix} \quad [Δxc Δxp ] = [ v − E C − 1 w w ] ~\begin{bmatrix} v-EC^-1w~ \\ w~ \end{bmatrix} \quad [v−EC−1w w ]
再利用Marginalization(Schur消元)来分步求解:
首先直接给出AX=B解的情况:
上式的解为 x = A-1B,这里A为可逆或伪逆矩阵.
SLAM中的最小二乘问题就是用来解决第二、三种情况求最优解。当出现第二种情况下(也就是A满秩下,称为满秩最小二乘问题),为了提高效率通常使用QR分解、LTLD分解、Cholesky分解和SVD分解(奇异分解)等。当出现第三种情况时(也就是A秩亏,称为亏秩最小二乘问题),只能使用SVD分解方法,其他方法将失效。
首先说下结论:QR分解比SVD更快,但SVD更加稳定.
回环检测是指:找到与当前帧4相似的关键帧1,这样的话就可以根据1直接估计4的位姿,而不是1–2–3–4。减少了误差传递。对于当前帧4的共视帧6,可以1—4—6来获得,而不是1–2–3–4–6(也就是回环校正,不过在回环校正的时候,还会去校正路标点坐标)。
step2.3 遍历当前检测的关键帧(红色点)的所有连接的共视关键帧,计算当前关键帧与每个共视关键帧的词袋相似度得分,并得到最低的分数(要保证闭环候选关键帧与当前关键帧的相似度得分大于当前关键帧与连接的共视关键帧得分的最低值)
step2.4 在所有关键帧中找到不与当前关键帧连接的闭环候选帧
获得了几个闭环候选帧并不能说明存在闭环,闭环必须是满足连续闭环的。考虑连续性需要将上一次闭环检测的连续组与当前闭环候选帧构建的连续组是否存在连续性(判断条件就是两个连续组中是否有同一个关键帧),判断成功后会记录连续成功的次数,只有当连续成功的次数大于3次才认为这个关键帧是真正的闭环候选帧,放入mvpEnoughconsistentCandidates中
step2.5:在候选帧中检测具有连续性的候选帧(每个候选关键帧与自己相连的关键帧构成一个子候选组,检测子候选组中的关键帧是否也存在与子连续组(上一次回环检测的连续组)中,存在则连续性+1,如果连续性大于3则认为该关键帧是真正的闭环候选帧)
一致性检查
考虑到单目相机的尺度漂移,计算当前帧与候选闭环帧的sim3变换,而不是T。这里的sim3求解是对当前关键帧和闭环候选帧之间匹配的MapPoint进行sim3求解
step3.1 从筛选后的闭环候选帧中取出一帧有效的关键帧,并与当前帧通过词袋进行匹配
step3.2 对匹配点数>20的候选闭环候选帧构建一个SIM3求解器
step3.3 对每个符合要求的候选帧进行sim3迭代匹配,来搜索两个关键帧中更多的匹配点对。并使用新的匹配构建优化器来优化sim3.只要有一个闭环候选帧通过了SIM3的求解和优化,就跳出对其他候选帧的判断。
Sim3相似变换群,有7个自由度,即一个尺度因子s,3个旋转角度,3个位移,即[sR|t]
(1)在上一帧计算当前帧和闭环帧的Sim3位姿变换时,建立了闭环帧及其共视帧的路标点与当前帧的联系,因此先更新共视图。
(2)根据计算的当前帧和闭环帧的Sim3变换,去更新当前帧及其共视帧的位姿,以及路标点的坐标。
(3)因为闭环帧已经经过了多次优化,认为是精确的,因此建立闭环帧及其共视帧的路标点与当前帧及其共视帧的联系,进行路标点的匹配、融合。
(4)优化本质图(只优化位姿)
(5)建立一个全局BA优化线程
回环优化了哪些量:
针对光照明暗变化:
针对于动态场景:
1.ransac方法: 将运动的点视为outlier处理,但是当运动的物体占据图像大部分比例时失效
2.前景背景的分割: 通过像素灰度和目标深度的变化,分辨前景和背景,前景是动的,背景是静止的
3.光流、场景流(三维光流): 通过像素灰度变化,推测出运动物体的运动方向和速度
4.多传感器融合: 将相机与IMU,轮式里程计等的结合,或者得到相机的运动先验信息
5.几何约束: 利用静态特征点的三维空间相对位置不变可以分离静态特征点。还有投影关系也可以用
6.目标识别:识别可能会移动的物体(例如人),将其去除
7.建立运动模型:存在动态特征点就存在多个运动模型,找出一个最好的模型
长廊问题: 机器人在经过长走廊弱纹理场景引起的累积误差问题.
解决方法:
初始化追踪线程
从配置文件中加载相机参数
加载ORB特征点相关的参数,并新建特征点提取器(建立金字塔,计算每层需要提取的特征点个数)
初始化帧 :
step1 对单目图像进行特征点提取
step 1.1 :构建金字塔
step 1.2 :计算图像特征点(Fast)并对特征点使用四叉树均匀化
step 1.3 :对图像进行高斯模糊
step 1.4 :计算高斯模糊后图像的描述子
step 1.5 :对非第0层图像中的特征点的坐标恢复到第0层图像(原图像)的坐标系下
step2 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
Step3 计算去畸变后图像边界,将特征点分配到网格中。
初始化地图
step1 创建单目初始器
step2 如果当前帧特征点数太少(不超过100),则重新构造初始器
Step3 在mInitialFrame与mCurrentFrame中找匹配的特征点对,如果初始化的两帧之间的匹配点太少(<100),重新初始化 (同step2)
step 3.1 :构建旋转直方图
step 3.2 :在F2半径窗口内搜索当前F2所有相对于F1的候选匹配特征点,找到最优和次优
step 3.3 :检查满足阈值,最优/次优比,以及直方图非前三的部分。
Step4 通过H模型或F模型进行单目初始化,得到两帧间相对运动、初始MapPoints。返回是否成功初始化
step 4.1 :在所有匹配特征点对中随机选择8对匹配特征点为一组
step 4.2 :计算fundamental 矩阵 和homography 矩阵,为了加速分别开了线程计算
step 4.3 :计算得分比例来判断选取哪个模型来求位姿R,t float RH = SH/(SH+SF)
Step5 初始化成功后,删除那些无法进行三角化的匹配点
初始化建图线程
初始化回环检测线程
初始化可视化线程
想要采样成功率达到 z z z 时所需的采样次数 M M M
H矩阵使用了齐次坐标系,可以进行任意尺度的缩放(H33 = 1),所以单应矩阵只有8个自由度
PnP(Perspective-n-point):多点透视成像,描述了当知道n个3D空间点及其投影位置时,如何估计相机的位姿.
PnP求解最少需要三个点对:P3P
P3P的缺点:
只有一个点对的自由度是4.两个点对的自由度是2