ORB-SLAM3无限期学习体会

目录

框架:

Tracking: 

LocalMapping:

LoopClosing:

Relocalization: 

IMU:

Mappoint:


框架:

        在第二代版本基础上, 增加了IMU, 提升了位置识别性能, 并加入了地图集atlas. 另外, 相机模型不局限于pin hole, 也可以使用fisheye甚至只要包括投影,反投影, 雅各比矩阵等参数的任意相机模型. 配合一些代码优化, 成为现在性能最爆炸的vislam算法!. 很多和2代一样

      三板斧概括: 分情况使用运动模型(motion)/关键帧进行track, 然后根据共视关系更新局部地图点, 通过投影匹配等操作更新当前帧的匹配地图点, 根据重投影误差再优化pose一次, 即trackLocalMap. 插入关键帧时, 对localMap进行局部BA和地图点fuse. 同时也根据是否发生回环进行全局BA, 此部分主要是本质图优化, 加少量迭代次数的全局BA.

        主要分为三个框架:

  1. Tracking: 检测特征点, 匹配相邻帧的特征点以此优化当前帧的位姿. 根据局部地图点进一步优化pose.
  2. LocalMapping: 如果当前帧满足一定条件被加入关键帧列表, 则根据共视图, 在局部进行BA,  更新地图局部区域. BA之前, 匹配的的地图点会被fuse, 避免冗余. 分为正向反向两个fuse.
  3. Loop closing: 每当加入新的关键帧,  若检测到回环帧, 进行CorrectLoop以及全局BA. 本质图只优化位姿;而全局/局部BA即优化位姿又优化地图点.

详细讨论双目情况

Tracking: 

        首先根据运动模型/关键帧进行追踪, 获得当前帧位姿初始值. 如果上一帧追踪失败, 但状态为RECENTLY_LOST, 可以根据IMU进行预测, 如果lost, 可以通过重定位抢救.
        上述第一阶段成功搞出来一个位姿了, 则进行局部追踪. 根据一级邻居以及二级邻居, 父子关键帧更新局部关键帧(mvpLocalKeyFrames). 然后根据初始位姿投影这些局部关键帧的地图点到当前帧进行匹配, 利用这些新增的地图点继续进行位姿优化. 并且把匹配上的地图点关联到当前帧. 然后判定是否为关键帧, 若是, 进入局部BA. (这里局部ba时使用的按时序回溯的某些关键帧, 而不是局部追踪那种共视帧). 检测到回环的话, 还要终止局部ba, 进行全局ba.

  1. GrabImageStereo/RGBD/Monocular:
    1. 处理输入的帧, 进行ORB特征提取与匹配
      1. 暴力匹配左右目特征点, 进行三角化(多一个lowe's ratio筛选)
  2. 然后用根据motion model/refKeyframe/relocalization, 给定一个初始位姿:
    1. 追踪定位(最小化地图点投影到当前像素坐标的差). 如果运动模型不可用, 就用目前的参考关键帧追踪, 否则用运动模型追踪. 
    2. 注意, 只要距离开机/重定位过了几帧, 而且恒速模型存在或者imu初始化了, 都优先使用motion model. 此时如果imu初始化了, 直接根据imu预测位姿, 然后return. 否则利用上一帧的运动量(恒速模型)mVelocity进行初始位姿预测, 然后多一个额外的投影匹配地图点进行位姿优化.
    3. TrackReferenceKeyFrame()的话, 首先会使用bow加速特征点匹配, 减少描述子距离的计算,不属于同一node的描述子直接跳过匹配. 属于同一node的才计算描述子距离.
  3. TrackLocalMap(): 更新局部关键帧s, 局部地图点s. 根据投影匹配地图点, 进行重投影误差优化位姿
    1. localkeyframes: 遍历当前帧的地图点,将观测到这些地图点的关键帧(一级共视)和它们的一级关键帧,及其父子关键帧,作为mvpLocalKeyFrames: 即
      1. 能观测到当前帧地图点的关键帧,也称一级共视关键帧

      2.  一级共视关键帧的共视关键帧,称为二级共视关键帧

      3.  一级共视关键帧的子关键帧、父关键帧

    2. localmappoints: 遍历局部关键帧, 它们匹配上的地图点即可得到局部地图点. 

    3. 设定搜索搜索窗口的大小, 取决于视角

    4. 利用未使用且在视野范围内的局部地图点进一步优化当前帧的位姿(将地图点投影到当前帧, 在局部搜索匹配一个点, 然后利用这些匹配即可更新位姿, searchbyprojection). 这些利用了的局部地图点也会更新到当前帧的地图点. 

    5. 优化: 只要imu预积分信息正常可用, 就会加入惯性误差优化, 否则只利用重投影误差优化位姿.

  4. 当前帧的参考关键帧会被更新为: 局部图中共视程度最高的关键帧
  5. 会记录每一帧的refkeyframe以及之间的相对位姿, 这样可以间接优化每一帧的世界位姿, 因为keyframe位姿会得到优化.
  6. 关键帧插入条件: 当前帧的匹配点(包含局部地图新增的)和参考关键帧相比较少, 且有一定量的匹配内点(>15). 当然有其他条件:
    1. 没有正在进行回环闭合
    2. 没有刚刚重定位
    3. 没有刚刚才插(id间隔小于mMinFrames),
    4. 没有超出最大kf数, 
    5. localmapper不繁忙
  7. 每一帧并不是所有特征点对应的mappoints都会加入到世界地图, 但是为了使得相邻帧的追踪可靠(MotionModel), 在追踪前会生成一个临时构建的地图(新增了那些未加入的地图点, 根据像素坐标反推)-updatelastframe(). 当然, 如果上一帧是关键帧, 忽略此步.  因为位姿优化的初值基于上一帧位姿, 如果上一帧位姿不好, 需要考虑更多特征点. 关键帧由于会进行局部map以及回环闭合, 位姿很好, 所以没必要再加特征点了.
  8. 当追踪失败时, 在一定条件下会将当前帧标记为RECENTLY_LOST. 后面再来新的帧, 结合IMU看是否能重新获得位姿,  如果连IMU都不行, 只能重定位. 如果还不行, 就记为丢失状态了. 要新建一个地图了(如果当前地图关键帧很少直接重置). 这里借鉴一下计算机视觉life/SLAM研习社的一个流程图注释

LocalMapping:

        这一部分就属于后端了, 是一个单独的线程. 前端存储的候选关键帧, 逻辑上都会送到这里. 首先是计算添加, 后端可能会用到的一些属性, 譬如BoW向量, 共视帧等等. 然后三角测量产生新的地图点(似乎地图只在这里产生新物理特征/地图点). 为了避免重复建点, 会进行局部地图点融合. 后续接上局部BA以及MU初始化两个优化过程, 并进行关键帧去冗余操作. ORB3这里提出了MAP(一大改进)初始化方式, 后续某两个时刻(run()的某两次循环)还会再进行IMU初始化.

  1. 处理关键帧: 计算BoW、更新观测、描述子、共视图,插入到地图等
  2. 去除当前帧一些不好地图点.
  3. 共视图中最好的nn帧成为邻接图, 或者邻居. 
  4. 三角化, 局部产生新的地图点.
  5. 找到和最新关键帧共视程度最好的那些一级邻居及二级邻居, 然后fuse地图点, 包括正向和反向两个融合.:
    1. 将当前关键帧地图点投影到每一共视关键帧, 进行匹配(正向融合):

      1. 如果地图点能匹配关键帧的特征点,并且该点有对应的地图点,那么选择观测数目多的替换这两个地图点.

      2. .如果地图点能匹配关键帧的特征点,并且该点没有对应的地图点,那么为该点添加该投影地图点

    2. 将共视关键帧地图点投影到当前关键帧, 进行上述同样操作.(反向融合)

  6. 处理完所有等待关键帧之后,如果之前地图完成了初始化, 进行LocalInertialBA. 如果没有初始化或者不是IMU模式, 直接LocalBundleAdjustment. 
  7. 如果地图没初始化, 进行初始化
  8. 局部更新后, 如果某一共视关键帧的地图点, 90%都能被其他帧观测到, 判定为冗余的. 需要删除的是该关键帧和其他所有帧、地图点之间的连接关系.. 给冗余关键帧setbagflag. 此时只考虑了一级共视图.
  9. 进行第二第三次初始化. 初始化都是通过InitializeIMU(, , )函数操作.
  10. LocalBundleAdjustment: 只优化了一级共视图及及它们的地图点. 地图点的观测不在共视图中的话, 固定该观测帧位姿, 只优化地图点. 优化利用的是投影误差.
  11. LocalInertialBA:  由于加入了惯性数据, 限制了关键帧数量. 如果追踪的效果不错(bLarge), 可以增大关键帧数, 减小迭代次数.  注意, 惯性数据需要连续性, 所以这里惯性部分对应的关键帧是按时间回溯过去的一些连续帧--vpOptimizableKFs, 投影误差还是按一级共视处理--lpOptVisKFs. 优化利用的是投影误差以及惯性误差.

      正常slam会局部更新, 定位模式下, 如果发生了重定位, 也需要一次局部更新. 初始化时, 会根据第一帧的特征点反投影到相机坐标(也就是世界坐标), 然后添加地图点. 全局的第一帧是不会优化的. 每当有新的关键帧来临, 且局部地图中的关键帧大于2个的时候, 进行局部地图的BA(一次局部BA只新加一个新的关键帧). 

LoopClosing:

        首先会检测当前KF和回环线程KF库里面每个KF的共视区域, 有重合则跳出. 其运行过程中, 逻辑上首先会进行几何校验, 在之后的回环检测中, 可能会先进行时序校验. 

  1. NewDetectCommonRegions():
    1. DetectNBestCandidates: 遍历KF库, 统计每一KF和当前KF有多少公共单词. 找出最大的共视单词数, 并将其0.8倍作为阈值, 将部分KF以及BOW相似度加入到lScoreAndMatch. 然后遍历lScoreAndMatch, 遍历每个KF的10个共视帧, 将其中出现过公共单词的那些帧的score累积(accScore)并求最大score(bestScore). 然后安装accScore遍历KF, 将其加入LoopCandidate或者mergeCandidate(看是否在当前map).

    2. DetectCommonRegionsFromBoW: 对上述的N个候选pKFi进行遍历分析:

      1. 遍历候选KF的共视帧vpCovKFi, 如果和当前KF共视图有重合, continue继续分析下一候选.

      2. 重新遍历vpCovKFi, 和当前KF进行描述子匹配(只计算同一node).

      3. 继续遍历vpCovKFi, 统计和当前KF匹配上的mp个数numBoWMatches.

      4. 如果numBoWMatches大于阈值, 则ransac迭代计算pKFi和当前KF的位姿. 如果收敛, 则把pKFi的共视帧的地图点投影到当前KF. 如果匹配个数满足阈值, 则可以计算当前KF和pKFi的sim3位姿. 如果内点够多, 严苛条件再投影匹配一次, 如果匹配个数大于阈值, 则可以最终比较当前KF共视图和pKFi的有效性(几何校验). 如果有效个数大于3, 则认为检测到公共区域.

  2. CorrectLoop(): 检测到公共区域, 则开始回环矫正
    1. 根据上述相对位姿, 修改当前KF的共视图的位姿和地图点.
    2. 并且把匹配KF的局部地图点和当前KF地图点进行一些融合.
    3. 然后开启本质图优化, 规模不大时还开启全局BA.
  3. 因为地图点通常几次更新就变化不大了, 所以先进行本质图优化, 再进行少量的全局BA. 有初始化就使用FullInertialBA, 否则使用GlobalBundleAdjustemnt.

    1. GlobalBundleAdjustemnt: 优化了所有的有效kf(!isbad())以及mp, 具体做法就是遍历每个地图点观测, 计算投影误差.

    2. FullInertialBA: 如果地图还处于第一二次初始化阶段, 大家bias都认为一样, 否则每一帧都要优化一个偏置, 通过边EdgeGyroRW, EdgeAccRW让二者尽量靠近,不要跑飞. 

        本质图包括哪些?

  1. spinnig tree: 子关键帧及他们的父关键帧, 没有父关键帧的不在树中. 每添加一个关键帧, 都会选取一个共视程度最高的关键帧作为父关键帧.
  2. 闭环帧
  3. 共视极佳的帧
  4. 上一惯性帧

Relocalization: 

用BoW在当前地图中选出相似且匹配点>=15的关键帧,然后分别都用MLPnP估计当前帧位姿. 如果估计内点数N<10, 直接看下一个候选. 如果N<50, 用投影匹配更多点, 如果N加上这些点大于50, 可以再用投影匹配的点优化一下. 最后内点数>50则判定重定位成功.

IMU:

        物体的短暂运动状态由当前速度, 配合加速度以及角速度来确定. 物体追踪丢失时(没有信号了, 画面被遮挡了, 等等), 可利用这三个参数大差不差地估计物体位置. IMU就是测量这些加速度和角速度的. 角速度也用陀螺仪称呼. IMU测量的加速度和陀螺仪受到外界干扰容易产生漂移, 所以ORB-SLAM3会在优化过程中, 除了速度外, 还优化加速度以及陀螺仪的偏置, 用来修正IMU测量漂移.

各个对象的时间关系如下.

  1. 预积分之前的标定, 采用中值定理估计. a_i是i时刻的加速度(陀螺仪同理分析). 这一步的意思就是每个时间段的数据取平均值, 进行之后的预积分累加. 注意这里x到a_0, a_0到a_1均值一样, 只算了一次, deltaT= t_{a_1} - t_{x}.

  2. 每考虑或者说添加一个小区间的measurement, 就更新预积分的位移,速度, 旋转向量
  3. // 考虑偏置后的加速度、角速度
    Eigen::Vector3f acc, accW;
    acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz;
    accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz;
    
    dP = dP + dV * dt + 0.5f * dR * acc * dt * dt; // 更新位移, 转换到之前的imu坐标系
    dV = dV + dR * acc * dt; // 更新速度, 转换到之前的imu坐标系
    IntegratedRotation dRi(angVel, b, dt); // 根据角速度, 即旋转向量求旋转向量
    dR = NormalizeRotation(dR * dRi.deltaR); // 更新旋转矩阵
  4. 上述更新固定了两帧之间imu测量的偏置, 为了考虑偏置本身的变化, 有如下的预积分公式, 所以我们还要更新三个目标量对偏置的导数.
  5. 偏置的导数公式如下, 是个求和形式, 所以也可以一步一步的累加更新. 当然ORB3对此式做了修改, 方便迭代更新(似乎是右式i到k的量都替换成了i到k-1, 也就是上一时刻的累计导数, 所以有了如下的更新方式)

  6. 最后的预积分量就是固定偏置计算的量和一阶泰勒展开的"求和", 上面的公式44

Mappoint:

        它也有描述子, 就是对应角点的描述子.当然会在所有观测中选择一个最优的, 即和其他描述子距离中值最小的那个描述子.

跑自己的数据还挺容易, 主要是用yaml标定传感器的参数, 改一下dataset路径即可. 

  • 1
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值