content.json

{"pages":[],"posts":[{"title":"DSO代码解析--优化","text":"1.关键帧 如果当前帧被认为是关键帧，则进入函数FullSystem::makeKeyFrame： 和非关键帧一样，利用当前帧对前面关键帧中的未成熟点进行逆深度更新FullSystem::traceNewCoarse； 标记后面需要边缘化（从活动窗口踢出）的帧FullSystem::flagFramesForMarginalization； 将当前帧加入到滑动窗口中frameHessians.push_back(fh)，并计算一下该窗口中其他帧与当前帧之间的一些参数比如相对光度、距离等FullSystem::setPrecalcValues； 将当前帧加入到总的能量函数中EnergyFunctional（ef）； 遍历窗口中之前所有帧的成熟点pointHessians，构建它们和新的关键帧的点帧误差PointFrameResidual，加入到ef中； 激活窗口中之前所有帧中符合条件的未成熟点，将其加入到ef中FullSystem::activatePointsMT； 利用高斯牛顿法对活动窗口中的所有变量进行优化FullSystem::optimize； 去除外点FullSystem::removeOutliers； 边缘化不需要的点EnergyFunctional::marginalizePointsF； 在当前帧中提取未成熟点FullSystem::makeNewTraces； 边缘化不需要的帧FullSystem::marginalizeFrame。 1.1 边缘化决策 主要两点（FullSystem::flagFramesForMarginalization）： 当活跃的帧的数量大于最低要求（５个）时，边缘化一帧中活跃的点少于５%或者和最新的帧相比光度参数变化剧烈( |a1−a2|>0.7)的帧（从最早的帧开始遍历）； 如果过程１没有找到需要边缘化的帧，则从全部帧中找到除最近的两帧外离当前帧最远的一帧。 1.2 点的激活 DSO代码中PointHessian表示可用于追踪和参与局部优化的点，除了初始化的第一帧外，它来源于每次生成关键帧时对未成熟点的提取FullSystem::makeNewTraces，并在后续关键帧生成时进行激活FullSystem::activatePointsMT，成功激活的点就由ImmaturePoint变为PointHessian，激活的基本步骤如下： 根据当前窗口中已有的成熟点的数量ef->nPoints，设置激活阈值currentMinActDist； 将所有的成熟点投影到当前帧，生成距离地图CoarseDistanceMap::makeDistanceMap（比如位置p有一个投影点了，那么位置p的值设为0，周围一圈像素设为１，再外面一圈设为２，以此类推，迭代进行）； 遍历所有的未成熟点，如果满足一些条件比如上一次的投影轨迹长度（极线）小于8，quality（次最小误差比最小误差）大于3等，就将逆深度设为其最大值和最小值的平均，将其投影到当前帧，然后考虑其在距离地图上的值，如果该值足够大（用到了第一步中的激活阈值），可以认为该点附近没有成熟点，所以将其加入待优化序列里，反之，则删除该点； 对待优化序列里的未成熟点进行优化FullSystem::activatePointsMT_Reductor，然后激活； 未成熟点的优化是对其逆深度迭代优化。逆深度求导的过程和DSO初始化中的类似，额外加入了一个和点的梯度有关的系数wpw_pwp​，梯度越大，权重系数越小： Jρ1=∂f(x)∂ρ1=wpwhρ1−1ρ2(∇Ixfx(tx−u2′tz)+∇Iyfy(ty−v2′tz))J_{\\rho_{1}}=\\frac{\\partial f(\\mathbf{x})}{\\partial \\rho_{1}}=w_{p} \\sqrt{w_{h}} \\rho_{1}^{-1} \\rho_{2}\\left(\\nabla I_{x} f_{x}\\left(t_{x}-u_{2}^{\\prime} t_{z}\\right)+\\nabla I_{y} f_{y}\\left(t_{y}-v_{2}^{\\prime} t_{z}\\right)\\right)Jρ1​​=∂ρ1​∂f(x)​=wp​wh​​ρ1−1​ρ2​(∇Ix​fx​(tx​−u2′​tz​)+∇Iy​fy​(ty​−v2′​tz​)) 2 滑动窗口法 求导的参数：相对的光度参数，相对位姿，主帧上点的逆深度，相机内参 对相机位姿求导：对应代码中：Vec6f Jpdxi[2] ∂rk∂δξ21=∂rk∂x2∗∂x2∂ξ21=[dx,dy]∗∂x2∂ξ21\\frac{\\partial r_{k}}{\\partial \\delta \\xi_{21}}=\\frac{\\partial r_k}{\\partial x_2}*\\frac{\\partial x_{2}}{\\partial \\xi_{21}}=[d_x,d_y]*\\frac{\\partial x_{2}}{\\partial \\xi_{21}}∂δξ21​∂rk​​=∂x2​∂rk​​∗∂ξ21​∂x2​​=[dx​,dy​]∗∂ξ21​∂x2​​ ∂x2∂ξ21=[fxρ20−fxρ2u2′−fxu2′v2′fx(1+u2′2)−fxv2′0fyρ2−fyρ2v2′−fy(1+v2′2)fyu2′v2′fyu2′000000]\\frac{\\partial x_{2}}{\\partial \\xi_{21}}=\\left[\\begin{array}{ccccc}f_{x} \\rho_{2} & 0 & -f_{x} \\rho_{2} u_{2}^{\\prime} & -f_{x} u_{2}^{\\prime} v_{2}^{\\prime} & f_{x}\\left(1+u_{2}^{\\prime 2}\\right) & -f_{x} v_{2}^{\\prime} \\\\ 0 & f_{y} \\rho_{2} & -f_{y} \\rho_{2} v_{2}^{\\prime} & -f_{y}\\left(1+v_{2}^{\\prime 2}\\right) & f_{y} u_{2}^{\\prime} v_{2}^{\\prime} & f_{y} u_{2}^{\\prime} \\\\ 0 & 0 & 0 & 0 & 0 & 0\\end{array}\\right]∂ξ21​∂x2​​=⎣⎡​fx​ρ2​00​0fy​ρ2​0​−fx​ρ2​u2′​−fy​ρ2​v2′​0​−fx​u2′​v2′​−fy​(1+v2′2​)0​fx​(1+u2′2​)fy​u2′​v2′​0​−fx​v2′​fy​u2′​0​⎦⎤​ 对逆深度求导：对应代码中：Vec2f Jpdd; [fxρ1−1ρ2(t21x−u2′t21z)fyρ1−1ρ2(t21y−v2′t21z)]\\left[\\begin{array}{l}f_{x} \\rho_{1}^{-1} \\rho_{2}\\left(t_{21}^{x}-u_{2}^{\\prime} t_{21}^{z}\\right) \\\\ f_{y} \\rho_{1}^{-1} \\rho_{2}\\left(t_{21}^{y}-v_{2}^{\\prime} t_{21}^{z}\\right)\\end{array}\\right][fx​ρ1−1​ρ2​(t21x​−u2′​t21z​)fy​ρ1−1​ρ2​(t21y​−v2′​t21z​)​] 对相机内参求导：VecCf Jpdc[2]; ∂x2∂C=[∂u2∂fx∂u2∂fy∂u2∂cx∂u2∂cy∂v2∂fx∂v2∂fy∂v2∂cx∂v2∂cy]\\begin{array}{c}\\frac{\\partial x_{2}}{\\partial C}=\\left[\\begin{array}{cccc}\\frac{\\partial u_{2}}{\\partial f_{x}} & \\frac{\\partial u_{2}}{\\partial f_{y}} & \\frac{\\partial u_{2}}{\\partial c_{x}} & \\frac{\\partial u_{2}}{\\partial c_{y}} \\\\ \\frac{\\partial v_{2}}{\\partial f_{x}} & \\frac{\\partial v_{2}}{\\partial f_{y}} & \\frac{\\partial v_{2}}{\\partial c_{x}} & \\frac{\\partial v_{2}}{\\partial c_{y}}\\end{array}\\right] \\\\ \\end{array}∂C∂x2​​=[∂fx​∂u2​​∂fx​∂v2​​​∂fy​∂u2​​∂fy​∂v2​​​∂cx​∂u2​​∂cx​∂v2​​​∂cy​∂u2​​∂cy​∂v2​​​]​ ∂u2∂fx=u2′+fx∂u2′∂fx=u2′+ρ2ρ1−1(r31u2′−r11)fx−1(u1−cx)∂u2∂fy=fx∂u2′∂fy=fxfy−1ρ2ρ1−1(r32u2′−r12)fy−1(v1−cy)∂u2∂cx=fx∂u2′∂cx+1=ρ2ρ1−1(r31u2′−r11)+1∂u2∂cy=fx∂u2′∂cy=fxfy−1ρ2ρ1−1(r32u2′−r12)∂v2∂fx=fy∂v2′∂fx=fyfx−1ρ2ρ1−1(r31v2′−r21)fx−1(u1−cx)∂v2∂fy=v2′+fy∂v2′∂fy=v2′+ρ2ρ1−1(r32v2′−r22)fy−1(v1−cy)∂v2∂cx=fy∂v2′∂cx=fyfx−1ρ2ρ1−1(r31v2′−r21)∂v2∂cy=fy∂v2′∂cy+1=ρ2ρ1−1(r32v2′−r22)+1\\begin{aligned} \\frac{\\partial u_{2}}{\\partial f_{x}} &=u_{2}^{\\prime}+f_{x} \\frac{\\partial u_{2}^{\\prime}}{\\partial f_{x}} =u_{2}^{\\prime}+\\rho_{2} \\rho_{1}^{-1}\\left(r_{31} u_{2}^{\\prime}-r_{11}\\right) f_{x}^{-1}\\left(u_{1}-c_{x}\\right) \\\\ \\frac{\\partial u_{2}}{\\partial f_{y}} &=f_{x} \\frac{\\partial u_{2}^{\\prime}}{\\partial f_{y}} =f_{x} f_{y}^{-1} \\rho_{2} \\rho_{1}^{-1}\\left(r_{32} u_{2}^{\\prime}-r_{12}\\right) f_{y}^{-1}\\left(v_{1}-c_{y}\\right) \\\\ \\frac{\\partial u_{2}}{\\partial c_{x}} &=f_{x} \\frac{\\partial u_{2}^{\\prime}}{\\partial c_{x}}+1 =\\rho_{2} \\rho_{1}^{-1}\\left(r_{31} u_{2}^{\\prime}-r_{11}\\right)+1 \\\\ \\frac{\\partial u_{2}}{\\partial c_{y}} &=f_{x} \\frac{\\partial u_{2}^{\\prime}}{\\partial c_{y}} =f_{x} f_{y}^{-1} \\rho_{2} \\rho_{1}^{-1}\\left(r_{32} u_{2}^{\\prime}-r_{12}\\right) \\\\ \\frac{\\partial v_{2}}{\\partial f_{x}} &=f_{y} \\frac{\\partial v_{2}^{\\prime}}{\\partial f_{x}} =f_{y} f_{x}^{-1} \\rho_{2} \\rho_{1}^{-1}\\left(r_{31} v_{2}^{\\prime}-r_{21}\\right) f_{x}^{-1}\\left(u_{1}-c_{x}\\right) \\\\ \\frac{\\partial v_{2}}{\\partial f_{y}} &=v_{2}^{\\prime}+f_{y} \\frac{\\partial v_{2}^{\\prime}}{\\partial f_{y}} =v_{2}^{\\prime}+\\rho_{2} \\rho_{1}^{-1}\\left(r_{32} v_{2}^{\\prime}-r_{22}\\right) f_{y}^{-1}\\left(v_{1}-c_{y}\\right) \\\\ \\frac{\\partial v_{2}}{\\partial c_{x}} &=f_{y} \\frac{\\partial v_{2}^{\\prime}}{\\partial c_{x}} =f_{y} f_{x}^{-1} \\rho_{2} \\rho_{1}^{-1}\\left(r_{31} v_{2}^{\\prime}-r_{21}\\right) \\\\ \\frac{\\partial v_{2}}{\\partial c_{y}} &=f_{y} \\frac{\\partial v_{2}^{\\prime}}{\\partial c_{y}}+1 =\\rho_{2} \\rho_{1}^{-1}\\left(r_{32} v_{2}^{\\prime}-r_{22}\\right)+1 \\end{aligned}∂fx​∂u2​​∂fy​∂u2​​∂cx​∂u2​​∂cy​∂u2​​∂fx​∂v2​​∂fy​∂v2​​∂cx​∂v2​​∂cy​∂v2​​​=u2′​+fx​∂fx​∂u2′​​=u2′​+ρ2​ρ1−1​(r31​u2′​−r11​)fx−1​(u1​−cx​)=fx​∂fy​∂u2′​​=fx​fy−1​ρ2​ρ1−1​(r32​u2′​−r12​)fy−1​(v1​−cy​)=fx​∂cx​∂u2′​​+1=ρ2​ρ1−1​(r31​u2′​−r11​)+1=fx​∂cy​∂u2′​​=fx​fy−1​ρ2​ρ1−1​(r32​u2′​−r12​)=fy​∂fx​∂v2′​​=fy​fx−1​ρ2​ρ1−1​(r31​v2′​−r21​)fx−1​(u1​−cx​)=v2′​+fy​∂fy​∂v2′​​=v2′​+ρ2​ρ1−1​(r32​v2′​−r22​)fy−1​(v1​−cy​)=fy​∂cx​∂v2′​​=fy​fx−1​ρ2​ρ1−1​(r31​v2′​−r21​)=fy​∂cy​∂v2′​​+1=ρ2​ρ1−1​(r32​v2′​−r22​)+1​ 对光度参数求导：对应代码VecNRf JabF[2]; δphoto =[−eaji,−bji]Jphoto =∂rk∂δphoto =[∂rk∂−eaji∂rk∂−bji]=[Ii[pi]−bi1]\\begin{aligned} \\delta_{\\text {photo }} &=\\left[-e^{a_{j i}},-b_{j i}\\right] \\\\ \\mathrm{J}_{\\text {photo }} &=\\frac{\\partial r_{k}}{\\partial \\delta_{\\text {photo }}}=\\left[\\frac{\\partial r_{k}}{\\partial-e^{a_{j i}}} \\frac{\\partial r_{k}}{\\partial-b_{j i}}\\right] \\\\ &=\\left[I_{i}\\left[\\mathbf{p}_{i}\\right]-b_{i} \\quad 1\\right] \\end{aligned}δphoto ​Jphoto ​​=[−eaji​,−bji​]=∂δphoto ​∂rk​​=[∂−eaji​∂rk​​∂−bji​∂rk​​]=[Ii​[pi​]−bi​1]​ 3.优化过程中零空间的处理 对于纯视觉SLAM，其零空间有7维：3 rotation+3 translation+1 scale；对于VIO系统而言，充分的加速度计激励可以为系统提供尺度信息，加速度计同时可以提供重力方向，使得pitch和roll可观，因此其不客观的维度为4维：3 translation+1 yaw。 对于位置的不可观可以这样理解，当地图整体移动，整个SLAM的优化问题是不变的。所以SLAM中的零空间其实是整个优化问题的零空间，而不是说是优化中某个节点的零空间。就是说整个优化问题存在不可观的维度，这个不客观的维度会通过优化问题进而影响到某个节点的优化，导致那个节点出现问题，常见的比如说纯视觉SLAM在转弯的时候，表现在地图中可能是地图的尺度突然缩小，相机移动量也对应缩小，同时不会对能量函数造成影响 在数学上： 对于正规方程： HΔx=b\\mathbf{H} \\Delta x=\\mathbf{b}HΔx=b (1) 其中hession矩阵的零空间满足：HΔxns=0\\mathbf{H} \\Delta x_{n s}=0HΔxns​=0 (2) 结合两个式子，一定有：H(Δx+Δxns)=b\\mathbf{H}\\left(\\Delta x+\\Delta x_{n s}\\right)=\\mathbf{b}H(Δx+Δxns​)=b 因此在零空间上的漂移不会违反系统的约束,但是会对结果产生影响 初始化一般进行归一化，相当于是人为设了一个尺度，为什么还会有尺度问题呢？这是因为DSO采用的滑动窗口法进行优化，当前面的关键帧离开滑动窗口后，最多只","link":"/2020/11/24/DSO%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90-%E4%BC%98%E5%8C%96/"},{"title":"DSO代码解析--初始化","text":"DSO 的入口是 FullSystem::addActiveFrame，输入的图像生成 FrameHessian 和 FrameShell 的对象，FrameShell 是 FrameHessian 的成员变量，FrameHessian 保存图像信息，FrameShell 保存帧的位置姿态信息。代码中一般用 fh 指针变量指向当前帧的 FrameHessian。在处理完成当前帧之后，会删除 FrameHessian，而保存 FrameShell 在变量 allFrameHistory 中，作为最后整条轨迹的输出。 对输入图像会做预处理，如果有光度标定,像素值不是灰度值，而是处理后的辐射值，这些辐射值的大小是 [0, 255]，float 型。 数据预处理部分是在 FullSystem::addActiveFrame 中调用的 FrameHessian::makeImages，这个函数为当前帧的图像建立图像金字塔，并且计算每一层图像的梯度。这些计算结果都存储在 FrameHessian 的成员变量中，1. dIp 每一层图像的辐射值、x 方向梯度、y 方向梯度；2. dI 指向 dIp[0] 也就是原始图像的信息；3. absSquaredGrad 存储 xy 方向梯度值的平方和。 1.第一帧 进入 FullSystem::addActiveFrame，首先判断是否完成了初始化，如果没有完成初始化，就将当前帧 fh 输入 CoarseInitializer::setFirst 中。完成之后接着处理下一帧。 1void CoarseInitializer::setFirst( CalibHessian*HCalib, FrameHessian* newFrameHessian) CoarseInitializer::setFirst，计算图像的每一层内参, 再针对不同层数选择大梯度像素, 第0层比较复杂1d, 2d, 4d大小block来选择3个层次的像素选取点，其它层则选出goodpoints, 作为后续第二帧匹配生成 pointHessians 和 immaturePoints 的候选点，这些点存储在 CoarseInitializer::points 中。每一层点之间都有联系，在 CoarseInitializer::makeNN 中计算每个点最邻近的10个点 neighbours，在上一层的最邻近点 parent。 pointHessians 是成熟点，具有逆深度信息的点，能够在其他影像追踪到的点。immaturePoints 是未成熟点，需要使用非关键帧的影像对它的逆深度进行优化，在使用关键帧将它转换成 pointHessians，并且加入到窗口优化。 2. 第2-7帧 初始化最少需要有七帧，如果第二帧CoarseInitializer::trackFrame 处理完成之后，位移足够,则再优化到满足位移的后5帧返回true. 在 FullSystem::initializerFromInitializer 中为第一帧生成 pointHessians，一共2000个左右。随后将第7帧作为 KeyFrame 输入到 FullSystem::deliverTrackedFrame，最终流入 FullSystem::makeKeyFrame。（FullSystem::deliverTrackedFrame 的作用就是实现多线程的数据输入。） 2.1 CoarseInitializer::trackFrame CoarseInitializer::trackFrame 中将所有 points （第一帧上的点）的逆深度初始化为1。从金字塔最高层到最底层依次匹配，每一层的匹配都是高斯牛顿优化过程，在 CoarseIntializer::calcResAndGS 中计算Hessian矩阵等信息，计算出来的结果在 CoarseInitializer::trackFrame 中更新相对位姿（存储在局部变量中，现在还没有确定要不要接受这一步优化），在 CoarseInitializer::trackFrame 中调用 CoarseInitializer::doStep 中更新点的逆深度信息。随后再调用一次 CoarseIntializer::calcResAndGS，计算新的能量，如果新能量更低，那么就接受这一步优化，在 CoarseInitializer::applyStep 中生效前面保存的优化结果。 一些加速优化过程的操作：1. 每一层匹配开始的时候，调用一次 CoarseInitializer::propagateDown，将当前层所有点的逆深度设置为的它们 parent （上一层）的逆深度；2. 在每次接受优化结果，更新每个点的逆深度，调用一次 CoarseInitializer::optReg 将所有点的 iR 设置为其 neighbour 逆深度的中位数，其实这个函数在 CoarseInitializer::propagateDown 和 CoarseInitializer::propagateUp 中都有调用，iR 变量相当于是逆深度的真值，在优化的过程中，使用这个值计算逆深度误差，效果是幅面中的逆深度平滑。 优化过程中的 lambda 和点的逆深度有关系，起一个加权的作用，也不是很明白对 lambda 增减的操作。在完成所有层的优化之后，进行 CoarseInitializer::propagateUp 操作，使用低一层点的逆深度更新其高一层点 parent 的逆深度，这个更新是基于 iR 的，使得逆深度平滑。高层的点逆深度，在后续的操作中，没有使用到，所以这一步操作我认为是无用的。 2.2 FullSystem::initializeFromInitializer FullSystem::initializeFromInitializer，第一帧是 firstFrame，第七帧是 newFrame，从 CoarseInitializer 中抽取出 2000 个点作为 firstFrame 的 pointHessians。设置的逆深度有 CoarseIntiailzier::trackFrame 中计算出来的 iR 和 idepth，而这里使用了 rescaleFactor 这个局部变量，保证所有 iR 的均值为 1。iR 设置的是 PointHessian 的 idepth，而 idepth 设置的是 PointHessian 的 idepth_zero(缩放了scale倍的固定线性化点逆深度)，idepth_zero 相当于估计的真值，用于计算误差。","link":"/2020/11/10/DSO%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90-%E5%88%9D%E5%A7%8B%E5%8C%96/"},{"title":"DSO代码解析--跟踪","text":"这部分主要代码在函数FullSystem::trackNewCoarse中。首先，DSO设置了一系列的候选位姿lastF_2_fh_tries，作为前一关键帧到当前帧的相对位姿的初值。这里主要参考前两帧和前一关键帧的位姿，就静止、恒定速度等猜想设了一些初值，另外主要针对旋转设置了许多微小的初始值。然后开始不断地尝试，从图像金字塔顶层开始就这些初值进行追踪CoarseTracker::trackNewestCoarse，如果找到一个合适的初值，就跳出循环。先来看一下追踪部分是如何实现的。函数CoarseTracker::trackNewestCoarse，传入参数有当前帧newFrameHessian，预测的相对位姿lastToNew_out，预测的相对光度aff_g2l_out（初始化为0），金字塔层数coarsestLvl，用来判断是否合适的误差minResForAbort，返回一个表明是否成功的bool值。该函数从输入的金字塔层级开始，由粗到精地计算最佳位姿。 2.1 误差计算 先是计算当前误差的大小CoarseTracker::calcRes。这一步里还没有改变位姿的大小，仅仅将host帧(参考帧,前一关键帧)的点, 通过反投影变换 以及投影变换 变换到Target帧（最新帧）的图像平面，然后将误差累计起来返回，并保存了后续计算雅克比矩阵需要的变量。这里和初始化时不同的是，追踪时已经有了一定的点，因此只考虑位姿加光度共８个参数。 将host帧上的点x1x_1x1​通过反投影,投影变换到target帧的图像平面上 X1=ρ1−1K−1x1x2=Kρ2(R21ρ1−1K−1x1+t21)=Kx2′\\begin{aligned} X_{1} &=\\rho_{1}^{-1} K^{-1} x_{1} \\\\ x_{2} &=K \\rho_{2}\\left(R_{21} \\rho_{1}^{-1} K^{-1} x_{1}+t_{21}\\right) \\\\ &=K x_{2}^{\\prime} \\end{aligned}X1​x2​​=ρ1−1​K−1x1​=Kρ2​(R21​ρ1−1​K−1x1​+t21​)=Kx2′​​ 上式中的 x2′x_2^{\\prime}x2′​ 就是归一化平面坐标，可以写作x2′=[u2′,v2′,1]Tx_{2}^{\\prime}=\\left[u_{2}^{\\prime}, v_{2}^{\\prime}, 1\\right]^{T}x2′​=[u2′​,v2′​,1]T 设误差函数 r21=(I2(p2)−b2)−(exp(a21)I1(p1)−b1)r_{21}=(I_{2}\\left(\\mathbf{p}_{2}\\right)-b_2)-\\left(exp (a_{21})I_{1}(\\mathbf{p}_{1}\\right)-b_{1})r21​=(I2​(p2​)−b2​)−(exp(a21​)I1​(p1​)−b1​) 对相对光度参数求导： ∂r21∂a21=−exp⁡(a21)I1(p1)\\frac{\\partial r_{21}}{\\partial a_{21}}=-\\exp (a_{21})I_{1}\\left(\\mathbf{p}_{1}\\right)∂a21​∂r21​​=−exp(a21​)I1​(p1​) ∂r21∂b21=−1\\frac{\\partial r_{21}}{\\partial b_{21}}=-1∂b21​∂r21​​=−1 对位姿增量求导 ∂r21∂ξ21=∂r21∂x2∂x2∂ξ21\\frac{\\partial r_{21}}{\\partial \\xi_{21}}=\\frac{\\partial r_{21}}{\\partial x_{2}} \\frac{\\partial x_{2}}{\\partial \\xi_{21}}∂ξ21​∂r21​​=∂x2​∂r21​​∂ξ21​∂x2​​ 其中,∂r21∂x2=∂I2[x2]∂x2=[dxdy]\\frac{\\partial r_{21}}{\\partial x_{2}}= \\frac{\\partial I_{2}\\left[x_{2}\\right]}{\\partial x_{2}}=\\left[\\begin{array}{l}d_{x} \\\\ d_{y}\\end{array}\\right]∂x2​∂r21​​=∂x2​∂I2​[x2​]​=[dx​dy​​] 通过链式法则 ∂x2∂ξ21=∂x2∂x2′∂x2′∂ξ21\\frac{\\partial x_{2}}{\\partial \\xi_{21}}=\\frac{\\partial x_{2}}{\\partial x_{2}^{\\prime}} \\frac{\\partial x_{2}^{\\prime}}{\\partial \\xi_{21}}∂ξ21​∂x2​​=∂x2′​∂x2​​∂ξ21​∂x2′​​ ∂x2∂x2′=[fx000fy0000]\\begin{aligned} \\frac{\\partial x_{2}}{\\partial x_{2}^{\\prime}} &=\\left[\\begin{array}{ccc}f_{x} & 0 & 0 \\\\ 0 & f_{y} & 0 \\\\ 0 & 0 & 0\\end{array}\\right] \\end{aligned}∂x2′​∂x2​​​=⎣⎡​fx​00​0fy​0​000​⎦⎤​​ x2′=ρ2(R21X1+t21)=ρ2X2x_{2}^{\\prime}=\\rho_{2}\\left(R_{21} X_{1}+t_{21}\\right)=\\rho_{2} X_{2}x2′​=ρ2​(R21​X1​+t21​)=ρ2​X2​,ρ2\\rho_2ρ2​是点的深度 ∂X2∂ξ21=[∂X2∂ξ21∂Y2∂ξ21∂Z2∂ξ21]=[1000Z2−Y2010−Z20X2001Y2−X20]∂x2′∂ξ21=[∂ρ2∂ξ21X2∂ρ2∂ξ21Y2∂ρ2∂ξ21Z2]+ρ2∂X2∂ξ21\\begin{aligned} \\frac{\\partial X_{2}}{\\partial \\xi_{21}} &=\\left[\\begin{array}{lllll}\\frac{\\partial X_{2}}{\\partial \\xi_{21}} \\\\ \\frac{\\partial Y_{2}}{\\partial \\xi_{21}} \\\\ \\frac{\\partial Z_{2}}{\\partial \\xi_{21}}\\end{array}\\right] \\\\ &=\\left[\\begin{array}{llll}1 & 0 & 0 & 0 & Z_{2} & -Y_{2} \\\\ 0 & 1 & 0 & -Z_{2} & 0 & X_{2} \\\\ 0 & 0 & 1 & Y_{2} & -X_{2} & 0\\end{array}\\right] \\\\ & \\frac{\\partial x_{2}^{\\prime}}{\\partial \\xi_{21}}=\\left[\\begin{array}{cc}\\frac{\\partial \\rho_{2}}{\\partial \\xi_{21}} X_{2} \\\\ \\frac{\\partial \\rho_{2}}{\\partial \\xi_{21}} Y_{2} \\\\ \\frac{\\partial \\rho_{2}}{\\partial \\xi_{21}} Z_{2}\\end{array}\\right]+\\rho_{2} \\frac{\\partial X_{2}}{\\partial \\xi_{21}} \\end{aligned}∂ξ21​∂X2​​​=⎣⎢⎡​∂ξ21​∂X2​​∂ξ21​∂Y2​​∂ξ21​∂Z2​​​⎦⎥⎤​=⎣⎡​100​010​001​0−Z2​Y2​​Z2​0−X2​​−Y2​X2​0​⎦⎤​∂ξ21​∂x2′​​=⎣⎢⎡​∂ξ21​∂ρ2​​X2​∂ξ21​∂ρ2​​Y2​∂ξ21​∂ρ2​​Z2​​⎦⎥⎤​+ρ2​∂ξ21​∂X2​​​ 最终可得: ∂r21∂δξ21=[dx,dy,0][fxρ20−fxρ2u2′−fxu2′v2′fx(1+u2′2)−fxv2′0fyρ2−fyρ2v2′−fy(1+v2′2)fyu2′v2′fyu2′000000]\\frac{\\partial r_{21}}{\\partial \\delta \\xi_{21}}=[d_x,d_y,0]\\left[\\begin{array}{cccccc}f_{x} \\rho_{2} & 0 & -f_{x} \\rho_{2} u_{2}^{\\prime} & -f_{x} u_{2}^{\\prime} v_{2}^{\\prime} & f_{x}\\left(1+u_{2}^{\\prime 2}\\right) & -f_{x} v_{2}^{\\prime} \\\\ 0 & f_{y} \\rho_{2} & -f_{y} \\rho_{2} v_{2}^{\\prime} & -f_{y}\\left(1+v_{2}^{\\prime 2}\\right) & f_{y} u_{2}^{\\prime} v_{2}^{\\prime} & f_{y} u_{2}^{\\prime} \\\\ 0 & 0 & 0 & 0 & 0 & 0\\end{array}\\right]∂δξ21​∂r21​​=[dx​,dy​,0]⎣⎡​fx​ρ2​00​0fy​ρ2​0​−fx​ρ2​u2′​−fy​ρ2​v2′​0​−fx​u2′​v2′​−fy​(1+v2′2​)0​fx​(1+u2′2​)fy​u2′​v2′​0​−fx​v2′​fy​u2′​0​⎦⎤​ 逆深度的雅可比 ∂r21∂ρ1=∂r21∂x2∂x2∂ρ1=∂I2[x2]∂x2∂x2∂ρ1=[dxdy0][fxρ1−1ρ2(t21x−u2′t21z)fyρ1−1ρ2(t21y−v2′t21z)]=(dxfxρ1−1ρ2(t21x−u2′t21z)+dyfyρ1−1ρ2(t21y−v2′t21z))\\begin{aligned} \\frac{\\partial r_{21}}{\\partial \\rho_{1}} &=\\frac{\\partial r_{21}}{\\partial x_{2}} \\frac{\\partial x_{2}}{\\partial \\rho_{1}} \\\\ &=\\frac{\\partial I_{2}\\left[x_{2}\\right]}{\\partial x_{2}} \\frac{\\partial x_{2}}{\\partial \\rho_{1}} \\\\ &=\\left[d_{x} \\quad d_{y} \\quad 0\\right]\\left[\\begin{array}{cc}f_{x} \\rho_{1}^{-1} \\rho_{2}\\left(t_{21}^{x}-u_{2}^{\\prime} t_{21}^{z}\\right) \\\\ f_{y} \\rho_{1}^{-1} \\rho_{2}\\left(t_{21}^{y}-v_{2}^{\\prime} t_{21}^{z}\\right)\\end{array}\\right] \\\\ &=\\left(d_{x} f_{x} \\rho_{1}^{-1} \\rho_{2}\\left(t_{21}^{x}-u_{2}^{\\prime} t_{21}^{z}\\right)+d_{y} f_{y} \\rho_{1}^{-1} \\rho_{2}\\left(t_{21}^{y}-v_{2}^{\\prime} t_{21}^{z}\\right)\\right) \\end{aligned}∂ρ1​∂r21​​​=∂x2​∂r21​​∂ρ1​∂x2​​=∂x2​∂I2​[x2​]​∂ρ1​∂x2​​=[dx​dy​0][fx​ρ1−1​ρ2​(t21x​−u2′​t21z​)fy​ρ1−1​ρ2​(t21y​−v2′​t21z​)​]=(dx​fx​ρ1−1​ρ2​(t21x​−u2′​t21z​)+dy​fy​ρ1−1​ρ2​(t21y​−v2′​t21z​))​ 2.2 计算增量方程 接下来在函数CoarseTracker::calcGSSSE中计算增量方程中的H和b。上一节已经求得了对应变量的导数，它们在代码中的表示为 1234567891011121314_mm_mul_ps(id,dx),// 对位移x导数 _mm_mul_ps(id,dy), // 对位移y导数 _mm_sub_ps(zero, _mm_mul_ps(id,_mm_add_ps(_mm_mul_ps(u,dx), _mm_mul_ps(v,dy)))),// 对位移z导数 _mm_sub_ps(zero, _mm_add_ps( _mm_mul_ps(_mm_mul_ps(u,v),dx), _mm_mul_ps(dy,_mm_add_ps(one, _mm_mul_ps(v,v))))),// 对旋转xi_1求导 _mm_add_ps( _mm_mul_ps(_mm_mul_ps(u,v),dy), _mm_mul_ps(dx,_mm_add_ps(one, _mm_mul_ps(u,u)))),// 对旋转xi_2求导 _mm_sub_ps(_mm_mul_ps(u,dy), _mm_mul_ps(v,dx)),// 对旋转xi_3求导 _mm_mul_ps(a,_mm_sub_ps(b0, _mm_load_ps(buf_warped_refColor+i))),// 对a_21求导 minusOne,// 对目标帧b_21求导 _mm_load_ps(buf_warped_residual+i),//残差 _mm_load_ps(buf_warped_weight+i));//huber权重 可以得到: H=JTWJ\\mathbf{H}=\\mathbf{J}^{T} \\mathbf{W} \\mathbf{J}H=JTWJ $\\mathbf{b}=-\\mathbf{J}^{T} \\mathbf{W} f(\\mathbf{x}) $ 初始的lambda设为0.01（即列文伯格方法中的拉格让日乘子λ\\lambdaλ），然后求解增量方程 (H+λI)Δx=b(\\mathbf{H}+\\lambda \\mathbf{I}) \\Delta \\mathbf{x}=\\mathbf{b}(H+λI)Δx=b 得到增量后对原来的状态变量进行更新x←x+Δx\\mathbf{x} \\leftarrow \\mathbf{x}+\\Delta \\mathbf{x}x←x+Δx 2.3 迭代求解 每一层的最大迭代次数是固定的，且各不相同，高层的次数多，低层的次数少 初始的lambda设为0.01（即列文伯格方法中的拉格让日乘子λ\\lambdaλ），然后求解增量方程 得到增量后对原来的状态变量进行更新 注意位姿的更新不是李代数相加，而是指数映射到李群后相乘 1234SE3 refToNew_new = SE3::exp((Vec6)(incScaled.head<6>())) * refToNew_current;AffLight aff_g2l_new = aff_g2l_current;aff_g2l_new.a += incScaled[6];aff_g2l_new.b += incScaled[7]; 用新的状态变量重新计算误差，然后将新的误差和旧的误差做比较，考虑是否接受这次优化 bool accept = (resNew[0] / resNew[1]) < (resOld[0] / resOld[1]); 这里的resNew[0]是总的误差，resNew[1]是对应点的数量，当平均误差减小时，认为这次优化可以接受。如果可以接受，那么就缩小lambda（每次缩小为原来的二分之一），如果优化失败，那么就说明高斯牛顿法的二次函数近似效果在这里不太好，通过增大lambda（放大为原来的4倍）来改善。当某一次得到的增量小于一定值时，认为收敛了，迭代过程终止。 DSO通过比较前后两次误差的大小关系来判断，它要求每一次得到的结果至少要比上一次尝试（上一个候选位姿下的）好，否则就直接跳过；如果比上一次好，那么再看它是否小于设定的阈值，如果小的话就结束尝试。 3. 判断是否为关键帧 关键帧的选择主要考虑当前帧和前一关键帧(参考帧)在点的光流变化，不考虑旋转情况下的光流变化，曝光参数的变化，三者加权相加大于１时新建关键帧 3.1非关键帧 如果当前帧被认为是非关键帧，那么该帧就用来对活动窗口中所有的关键帧中还未成熟的点进行逆深度更新: traceNewCoarse(fh);(深度滤波) 基本原理是沿着极线进行搜索ImmaturePoint::traceOn 3.1.1 极线搜索 首先，将未成熟的点根据相对位姿和之前的逆深度投影到当前帧上 1234Vec3f pr = hostToFrame_KRKi * Vec3f(u,v, 1);Vec3f ptpMin = pr + hostToFrame_Kt*idepth_min;float uMin = ptpMin[0] / ptpMin[2];float vMin = ptpMin[1] / ptpMin[2]; 这里的(uMin，vMin)就是设逆深度最小时投影得到的像素坐标。 接下来确定极线，如果逆深度无限大,随便设一个逆深度0.01，得到另一个投影点的坐标(uMax，vMax)， 1234// project to arbitrary depth to get direction.ptpMax = pr + hostToFrame_Kt*0.01;uMax = ptpMax[0] / ptpMax[2];vMax = ptpMax[1] / ptpMax[2]; 这样就得到了极线的方向 1234// direction.float dx = uMax-uMin;float dy = vMax-vMin;float d = 1.0f / sqrtf(dx*dx+dy*dy); 这样，极线可以表示为L:={l0+λ[lx,ly]T}\\mathbf{L}:=\\left\\{\\mathbf{l}_{0}+\\lambda\\left[l_{x}, l_{y}\\right]^{T}\\right\\}L:={l0​+λ[lx​,ly​]T} 其中l0l_0l0​就是[ umin, vmin]T[\\ { u_{min} }, \\ v_{min }]^{T}[ umin​, vmin​]T ，λ是离散的步长（视差），[lx,ly]T\\left[l_{x}, l_{y}\\right]^{T}[lx​,ly​]T表示极线的方向（单位向量）。根据前面设的最大搜索范围，得到像素的最大范围 123dist = maxPixSearch;uMax = uMin + dist*dx*d;vMax = vMin + dist*dy*d; 然后在最大范围内按一定步长进行离散搜索，找到最小的和第二小的误差，比较两者的比值。最后在最小误差的位置上进行高斯牛顿优化（只有一个变量），每次迭代过程中如果误差大于前面得到的最小误差，就缩小优化步长重新来过，当增量小于一定值时停止。 3.1.2 逆深度范围更新 设Pr=KRK−1[u1v11]T=[m1m2m3]T\\mathbf{P}_{r}=\\mathbf{K R K}^{-1}\\left[\\begin{array}{lll}u_{1} & v_{1} & 1\\end{array}\\right]^{T}=\\left[\\begin{array}{lll}m_{1} & m_{2} & m_{3}\\end{array}\\right]^{T}Pr​=KRK−1[u1​​v1​​1​]T=[m1​​m2​​m3​​]T Kt=[n1n2n3]T\\mathbf{K t}=\\left[\\begin{array}{lll}n_{1} & n_{2} & n_{3}\\end{array}\\right]^{T}Kt=[n1​​n2​​n3​​]T 则投影后的像素坐标 $u_{2}=\\frac{m_{1}+n_{1} \\rho_{1}}{m_{3}+n_{3} \\rho_{1}} $$v_{2}=\\frac{m_{2}+n_{2} \\rho_{1}}{m_{3}+n_{3} \\rho_{1}}$ 把逆深度放在左边，$\\rho_{1}=\\frac{m_{3} u_{2}-m_{1}}{n_{1}-n_{3} u_{2}} $ (1) $ \\rho_{1}=\\frac{m_{3} v_{2}-m_{2}}{n_{2}-n_{3} v_{2}}$ (2) 设[u2∗,v2∗]T\\left[u_{2}^{*}, v_{2}^{*}\\right]^{T}[u2∗​,v2∗​]T为前面得到的最佳位置，并设当前像素位置的误差范围为α，离散搜索的单位步长在ｘ,y方向上的投影分别为Δu,Δv，当ｘ方向梯度较大时，我们根据公式（1）来确定逆深度范围： ρ1min⁡=m3(u2∗−αΔu)−m1n1−n3(u2∗−αΔu)\\rho_{1 \\min }=\\frac{m_{3}\\left(u_{2}^{*}-\\alpha \\Delta u\\right)-m_{1}}{n_{1}-n_{3}\\left(u_{2}^{*}-\\alpha \\Delta u\\right)}ρ1min​=n1​−n3​(u2∗​−αΔu)m3​(u2∗​−αΔu)−m1​​ ρ1max⁡=m3(u2∗+αΔu)−m1n1−n3(u2∗+αΔu)\\rho_{1 \\max }=\\frac{m_{3}\\left(u_{2}^{*}+\\alpha \\Delta u\\right)-m_{1}}{n_{1}-n_{3}\\left(u_{2}^{*}+\\alpha \\Delta u\\right)}ρ1max​=n1​−n3​(u2∗​+αΔu)m3​(u2∗​+αΔu)−m1​​ 当y方向梯度较大时，根据公式（2）来确定逆深度范围： ρ1min⁡=m3(v2∗−αΔv)−m2n2−n3(v2∗−αΔv)\\rho_{1 \\min }=\\frac{m_{3}\\left(v_{2}^{*}-\\alpha \\Delta v\\right)-m_{2}}{n_{2}-n_{3}\\left(v_{2}^{*}-\\alpha \\Delta v\\right)}ρ1min​=n2​−n3​(v2∗​−αΔv)m3​(v2∗​−αΔv)−m2​​ ρ1max⁡=m3(v2∗+αΔv)−m2n2−n3(v2∗+αΔv)\\rho_{1 \\max }=\\frac{m_{3}\\left(v_{2}^{*}+\\alpha \\Delta v\\right)-m_{2}}{n_{2}-n_{3}\\left(v_{2}^{*}+\\alpha \\Delta v\\right)}ρ1max​=n2​−n3​(v2∗​+αΔv)m3​(v2∗​+αΔv)−m2​​ 12345678910if(dx*dx>dy*dy){ idepth_min = (pr[2]*(bestU-errorInPixel*dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2]*(bestU-errorInPixel*dx)); idepth_max = (pr[2]*(bestU+errorInPixel*dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2]*(bestU+errorInPixel*dx));}else{ idepth_min = (pr[2]*(bestV-errorInPixel*dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2]*(bestV-errorInPixel*dy)); idepth_max = (pr[2]*(bestV+errorInPixel*dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2]*(bestV+errorInPixel*dy));} 接下来考虑α。为什么这里要有个α呢？前面通过离散搜索加上高斯牛顿优化的方式得到了最佳的匹配点，如果假设没有其他任何误差存在的话，我们完全可以令α＝1，这样逆深度的最大最小值就可以通过简单地扰动一个单位步长来得到。但考虑误差的话，我们会发现极线和梯度的夹角对结果有着非常大的影响。如果极线的方向和梯度的方向接近垂直的，那么稍微有一点位姿误差（必然存在）使得投影点和真实点产生了一定的误差，沿着极线搜索得到的结果就会产生巨大的偏差，如图１所示。因此，非常有必要考虑这个α，事实上，在代码中，α的计算是在极线搜索之前做的，如果得到的α太大（这意味这极线和梯度的夹角接近90度），就没有做极线搜索的必要。 DSO似乎没有直接根据公式计算视差的不确定度，α更像是一个根据人工经验设计的置信系数（我目前是这么理解的，因为实在推不出这个公式），代码如下所示： 123456float dx = setting_trace_stepsize*(uMax-uMin);float dy = setting_trace_stepsize*(vMax-vMin);float a = (Vec2f(dx,dy).transpose() * gradH * Vec2f(dx,dy));float b = (Vec2f(dy,-dx).transpose() * gradH * Vec2f(dy,-dx));float errorInPixel = 0.2f + 0.2f * (a+b) / a; 令点在主导帧中的梯度雅克比为J∇=[∇x,∇y]T\\mathbf{J}_{\\nabla}=[\\nabla x, \\nabla y]^{T}J∇​=[∇x,∇y]T，gradH就是∑J∇J∇T\\sum \\mathbf{J}_{\\nabla} \\mathbf{J}_{\\nabla}^{T}∑J∇​J∇T​ ，这里的求和符号表示对一个小块中的８个点求和，因此a则可以理解为极线与梯度的点乘的平方，b则可以理解为极线旋转90度后与梯度的点乘的平方，errorInPixel就是这里的α。可以看到，errorInPixel基本来自于变量b/a，当b/a接近于0时（此时极线和梯度方向基本平行），α≈0.4，逆深度只更新大约0.4个单位步长；而当b/a大于一定阈值时，则后续步骤直接跳过，该点被标记为IPS_BADCONDITION。","link":"/2020/11/20/DSO%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90-%E8%B7%9F%E8%B8%AA/"},{"title":"DSO优化中零空间的处理","text":"对于纯视觉SLAM，其零空间有7维：3 rotation+3 translation+1 scale；对于VIO系统而言，充分的加速度计激励可以为系统提供尺度信息，加速度计同时可以提供重力方向，使得pitch和roll可观，因此其不客观的维度为4维：3 translation+1 yaw。 SLAM中的零空间其实是整个优化问题的零空间，而不是说是优化中某个节点的零空间。就是说整个优化问题存在不可观的维度，这个不客观的维度会通过优化问题进而影响到某个节点的优化，导致那个节点出现问题，常见的比如说纯视觉SLAM在转弯的时候，表现在地图中可能是地图的尺度突然缩小，相机移动量也对应缩小，同时不会对能量函数造成影响 在数学上： 对于正规方程： HΔx=b\\mathbf{H} \\Delta x=\\mathbf{b}HΔx=b (1) 其中hession矩阵的零空间满足：HΔxns=0\\mathbf{H} \\Delta x_{n s}=0HΔxns​=0 (2) 结合两个式子，一定有：H(Δx+Δxns)=b\\mathbf{H}\\left(\\Delta x+\\Delta x_{n s}\\right)=\\mathbf{b}H(Δx+Δxns​)=b 也就是说hession矩阵不满秩 因此在零空间上的漂移不会违反系统的约束,但是会对结果产生影响 DSO初始化一般进行归一化，相当于是人为设了一个尺度，并且固定第一帧，为什么还会有漂移问题呢？在优化过程中，线性方程的解不唯一。尽管每次迭代的步长都很小，但是时间长了，也容易让整个系统在这个零空间中游荡，令尺度发生漂移。 在dso中，使用了零空间的正交化去避免零空间对最终的增量产生的影响 其中红色的箭头表示增量方程中求解出来的增量，黑色的虚线表示零空间在这个节点上可能产生的漂移，而蓝色的箭头表示最终我们正交化之后的增量结果，当正交化之后，相机最终的位置会到蓝色三角显示的位置。 位姿的零空间的基 对于VO来说,是7个自由度不可观的,即零空间是以这7个状态量为基构成的。需 要注意的是零空间的变换是global的,求基方法即增加global小量扰动。 TwcExp⁡(δξc)=Exp⁡(δξw)Twc\\mathbf{T}_{w c} \\operatorname{Exp}\\left(\\delta \\xi_{c}\\right)=\\operatorname{Exp}\\left(\\delta \\xi_{w}\\right) \\mathbf{T}_{w c}Twc​Exp(δξc​)=Exp(δξw​)Twc​ Exp⁡(δξc)=TcwExp⁡(δξw)Twc\\operatorname{Exp}\\left(\\delta \\xi_{c}\\right)=\\mathbf{T}_{c w} \\operatorname{Exp}\\left(\\delta \\xi_{w}\\right) \\mathbf{T}_{w c}Exp(δξc​)=Tcw​Exp(δξw​)Twc​ ΔTp=TExp⁡(δξ1)T−1\\Delta \\mathrm{T}_{\\mathrm{p}}=\\operatorname{TExp}\\left(\\delta \\xi_{1}\\right) \\mathrm{T}^{-1}ΔTp​=TExp(δξ1​)T−1 ΔTn=TExp⁡(δξ2)T−1\\Delta \\mathrm{T}_{\\mathrm{n}}=\\operatorname{TExp}\\left(\\delta \\xi_{2}\\right) \\mathrm{T}^{-1}ΔTn​=TExp(δξ2​)T−1 其中，δξ1=0.001,δξ2=−0.001\\delta \\xi_{1}=0.001, \\delta \\xi_{2}=-0.001δξ1​=0.001,δξ2​=−0.001 Txnullspace=log⁡(ΔTp)−log⁡(ΔTn)δξ1−δξ2T_{x_{\\text {nullspace}}}=\\frac{\\log \\left(\\Delta \\mathrm{T}_{\\mathrm{p}}\\right)-\\log \\left(\\Delta \\mathrm{T}_{\\mathrm{n}}\\right)}{\\delta \\xi_{1}-\\delta \\xi_{2}}Txnullspace​​=δξ1​−δξ2​log(ΔTp​)−log(ΔTn​)​ ： 尺度的零空间的基 ΔTp=Exp⁡(δtw1)Twc\\Delta \\mathrm{T}_{\\mathrm{p}}=\\operatorname{Exp}\\left(\\delta t_{w1}\\right) \\mathbf{T}_{w c}ΔTp​=Exp(δtw1​)Twc​ ΔTn=Exp⁡(δtw2)Twc\\Delta \\mathrm{T}_{\\mathrm{n}}=\\operatorname{Exp}\\left(\\delta t_{w2}\\right) \\mathbf{T}_{w c}ΔTn​=Exp(δtw2​)Twc​ 其中，δtw1=tcw∗1.00001,δtw2=tcw/1.00001\\delta t_{w1}=t_{cw}*1.00001, \\delta t_{w2}=t_{cw}/1.00001δtw1​=tcw​∗1.00001,δtw2​=tcw​/1.00001 txnullspace=log⁡(ΔTp)−log⁡(ΔTn)δξ1−δξ2t_{x_{\\text {nullspace}}}=\\frac{\\log \\left(\\Delta \\mathrm{T}_{\\mathrm{p}}\\right)-\\log \\left(\\Delta \\mathrm{T}_{\\mathrm{n}}\\right)}{\\delta \\xi_{1}-\\delta \\xi_{2}}txnullspace​​=δξ1​−δξ2​log(ΔTp​)−log(ΔTn​)​ 如何处理零空间 Gauge fixation:把不可观的状态固定为某一些值,具体的为固定第一帧的位姿,等价于设对应的残差雅克比为0。相当于权重趋近于无穷大。 Gauge prior:给这些状态设置先验,增加相应的惩罚项。 信息矩阵不满秩的根本原因在于整个系统求解的初值不确定，因此如果将状态量的初值固定住，就可以有效约束系统的不确定性。常用的方法包括：1）对信息矩阵中的添加单位矩阵I；2）强行将信息矩阵设定为极大值（101410^{14}1014)。（本质都是使得初值对应的Δx=0，从而固定初值。） Free gauge:让它自由演变,不管,使用伪逆或者增加阻尼(LM)使得问题可解。相 当于先验权重为0。 DSO中除了给第一帧设置先验以外，使用LM算法求解。由于LM求解过程中会对信息矩阵添加阻尼项，添加之后的信息矩阵必定是满秩的该方法虽然能够保证正则方程有解，但解可能会向零空间偏移，所以dso还使用伪逆来求解。 设空间M\\mathcal{M}M是零空间 Nx=v\\mathbf{Nx=v}Nx=v,其中，N是零空间的一组基，v是增量方程的解，通常情况下方程是无解的，但我们可以找一个唯一的解x0x_0x0​使得∥Nx−v∥\\|\\mathbf{N} \\mathbf{x}-\\mathbf{v}\\|∥Nx−v∥最小，在图中就是蓝色箭头和虚线的垂足。 这时x0=(NTN)−1NTvx_{0}=\\left(\\mathbf{N}^{\\mathrm{T}} \\mathbf{N}\\right)^{-1} \\mathbf{N}^{\\mathbf{T}} \\mathbf{v}x0​=(NTN)−1NTv, 其中(NTN)−1NT\\left(\\mathbf{N}^{T} \\mathbf{N}\\right)^{-1} \\mathbf{N}^{T}(NTN)−1NT其实就是N的伪逆N†\\mathbf{N}^{\\dagger}N†，这时v在零空间的分量为NN†V\\mathbf{NN}^{\\dagger}\\mathbf VNN†V 去除零空间影响的解为v←v−NN†v\\mathbf{v} \\leftarrow \\mathbf{v}-\\mathbf{N} \\mathbf{N}^{\\dagger} \\mathbf{v}v←v−NN†v 参考文献： On the comparison of gauge freedom handling in optimization-based visual-inertial state estimation[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 2710-2717. https://blog.csdn.net/wubaobao1993/article/details/105106301/ https://blog.csdn.net/xxxlinttp/article/details/100080080","link":"/2020/11/09/DSO%E4%BC%98%E5%8C%96%E4%B8%AD%E9%9B%B6%E7%A9%BA%E9%97%B4%E7%9A%84%E5%A4%84%E7%90%86/"},{"title":"orbslam2代码解析--回环检测","text":"LocalClosing模块是SLAM系统非常重要的一部分，由于VO过程存在累计误差（漂移），LocalClosing模块主要任务是检测闭环，即确定是否曾到达过此处，并在找到闭环之后计算Sim3变换，进行关键帧位姿、地图点的优化（后端优化），可以将这个累计误差缩小到一个可接受的范围内。闭环检测模块使用LocalMapping模块传送过来的关键帧，插入关键帧的操作是在LocalMapping线程主循环剔除冗余关键帧之后进行的，执行语句mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame)。 检测闭环 对应LoopClosing::DetectLoop()函数。检测回环候选（在KeyFrameDataBase中找到与mlpLoopKeyFrameQueue相似的闭环候选帧）并检查共视关系（检查候选帧连续性），函数会将所有通过一致性检测的候选关键帧统一放到集合mvpEnoughConsistentCandidates中，由下一步最终确定闭环帧。主要过程如下： 计算当前帧mpCurrentKF和所有与当前帧有共视关系的关键帧的Bow得分，得到最小得分**minScore** ； 根据这个最小得分minScore 从mpKeyFrameDB（关键帧库）中找出候选的关键帧集合**vpCandidateKFs**； 使用KeyFrameDatabase::DetectLoopCandidates完成候选帧的筛选，具体过程： Bow得分>minScore； 统计满足1的关键帧中有共同单词最多的单词数maxcommonwords； 筛选出共同单词数大于mincommons(=0.8maxcommons)的关键帧； 相连的关键帧分为一组，计算组得分（总分），得到最大总分bestAccScore，筛选出总分大于minScoreToRetain(=0.75*bestAccScore)的组，用组中得分最高的候选帧lAccScoreAndMatch代表该组。计算组得分的目的是剔除单独一帧得分较高，但是没有共视关键帧，作为闭环来说不够鲁棒。 对于**vpCandidateKFs里面的每一个关键帧，作为当前关键帧。找出其有共视关系的关键帧组成一个当前集合spCandidateGroup。如果当前关键帧是vpCandidateKFs中第一帧的话，直接把这个spCandidateGroup集合，以分数0直接放到mvConsistentGroups中。如果不是第一帧的话，就从mvConsistentGroups中依次取出里面的元素pair,int>的first，这些元素也是关键帧们组成以前集合sPreviousGroup。只要是当前集合中的任意一个关键帧能在以前集合里面找到，就要将当前集合的得分加1，并把当前集合放到mvConsistentGroups里面。如果当前集合的得分大于3（mnCovisibilityConsistencyTh）的话，当前帧就通过了一致性检测**，把当前帧放到**mvpEnoughConsistentCandidates，最后会找到很多候选的关键帧。下一步用sim3找出闭环帧。** 计算Sim3 对应LoopClosing::ComputeSim3()函数。该函数计算相似变换，从mvpEnoughConsistentCandidates中找出真正的闭环帧mpMatchedKF。主要过程如下： 通过SearchByBow，搜索当前关键帧中和候选帧匹配的地图点，当匹配的数目超过20，就为该候选帧和当前帧构造一个sim3Solver，即相似求解器； 用相似求解器Sim3Solver求解出候选帧与当前帧之间的相似变换Scm，这里使用RANSAC方法； 根据计算出的位姿，通过相似变换求找出更多的匹配地图点（SearchBySim3），更新vpMapPointMatches； 使用更新后的匹配，使用g2o优化Sim3位姿（OptimizeSim3），优化的结果足够好的话（内点数nInliers>20）就把候选的关键帧作为当前帧的闭环帧mpMatchedKF，break，跳过对其他候选闭环帧的判断，同时也得到了mScw； 获取mpMatchedKF及其相连关键帧对应的地图点。调用SearchByProjection将这些地图点通过上面优化得到的mScw变换投影（重投影）到当前关键帧进行匹配，若匹配点>=40个，则返回ture，进行闭环调整；否则，返回false，线程暂停5ms后继续检查Tracking发送来的关键帧队列。 注意SearchByProjection得到的当前关键帧中匹配上闭环关键帧共视地图点（mvpCurrentMatchedPoints），将用于后面CorrectLoop时当前关键帧地图点的冲突融合。 至此，完成第4、5操作后，不仅确保了当前关键帧与闭环帧之间匹配度高，而且保证了闭环帧的共视图中的地图点和当前关键帧的特征点匹配度更高（20—->40），说明该闭环帧是正确的。 闭环矫正 对应LoopClosing::CorrectLoop()函数，完成回环地图点融合和位姿图优化。具体来说，在回环检测到之后，对当前帧（回环帧）周围的帧的影响、回边的连接以及地图点做出的相应的改变。闭环矫正时，LocalMapper和Global BA必须停止。注意Global BA使用的是单独的线程。主要过程如下： 使用计算出的当前帧的相似变换Sim3，即mScw，对当前位姿进行调整，并且传播到当前关键帧相连的关键帧（相连关键帧之间相对位姿是知道的，通过当前关键帧的Sim3计算相连关键帧的Sim3）； 经过上一步处理，回环两侧的关键帧完成对齐，然后利用调整过的位姿更新这些相连关键帧对应的地图点（修正关键帧看到的地图点）； 将闭环帧及其相连帧的地图点都投影到当前帧以及相连帧上，投影匹配上的和Sim3计算过的地图点进行融合（就是替换成质量高的）； 涉及融合的关键帧还需要更新其在共视地图中的观测边关系，因为找到了闭环，需要在covisibility里面增加闭环边（不止一条边）。这是为了剥离出因为闭环产生的新的连接关系LoopConnections，用于优化Essential Graph。添加当前帧与闭环匹配帧之间的边，该边不参与优化； 然后进行EssentialGraph优化（只是优化一些主要关键帧）； 新开一个线程进行全局优化，优化所有的位姿与MapPoints。","link":"/2020/10/10/orbslam2%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90-%E5%9B%9E%E7%8E%AF%E6%A3%80%E6%B5%8B/"},{"title":"orbslam2代码解析--特征跟踪及初始化","text":"ORBSLAM分定位和SLAM两个模式，定位模式localmap不工作，不插入新的关键帧以及更新地图 在跟踪时，初始化完成后首先调用的是GrabImageMonocular函数，对图像格式进行转换，将RGB转为灰度图。 然后通过图片初始化一个Frame类 mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth) 进入Track()函数，完成四个任务： 1.单目初始化； 2.通过上一帧获得初始位姿估计或者重定位。也就是求出当前帧在世界坐标系下的位姿 3.跟踪局部地图TrackLocalMap() 求得位姿估计, 这一步判断了当前帧的特征点和map中的哪些mappoint匹配。 4.判断是否需要给localmapping插入关键帧 1. 单目初始化 双目和RGBD只需要一帧图片即可初始化，单目需要两帧才能完成，其流程为： 1.根据得到的两帧图像的特征匹配，我们就可以计算单应矩阵H，和基础矩阵F 2.计算出的单应矩阵H，和基础矩阵F会有对应的得分，根据得分情况来选择使用单应矩阵H，还是基础矩阵F来进行初始化 3.通过H或者F分解为R，t时，会有多个R，t的组合，需要将匹配的特征点通过三角化重投影后，更具投影结果来选择具体是哪个R，t的组合 4.R，t后，我们自然也通过三角化得到了通过三角化重投影成功的匹配特征点的相对第一帧的3d坐标。剩下那些不能通过R，t重投影成功的匹配点，在后面将它剔除 5.将初始化第一帧和第二帧转化为关键帧keyframe，将三角化重投影成功的匹配特征点的3d坐标包裹为mappoint，全部添加到map中。 6.在创建mappoint过程中，会记录它能被哪些keyframe的哪些特征点匹配。 1234567891011// step 4.3：表示该KeyFrame的哪个特征点可以观测到哪个3D点 pKFini->AddMapPoint(pMP,i); pKFcur->AddMapPoint(pMP,mvIniMatches[i]);// a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到pMP->AddObservation(pKFini,i);pMP->AddObservation(pKFcur,mvIniMatches[i]);// b.从众多观测到该MapPoint的特征点中挑选区分度最高的描述子pMP->ComputeDistinctiveDescriptors();// c.更新该MapPoint平均观测方向以及观测距离的范围pMP->UpdateNormalAndDepth(); 7.更新关键帧间的图连接关系 ​ 1.首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。 map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数 ​ 2.并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高） for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { // 更新具有最佳共视关系的关键帧信息 if(mit->second>nmax) { nmax=mit->second; // 找到对应权重最大的关键帧（共视程度最高的关键帧） pKFmax=mit->first; } if(mit->second>=th)//th=15 { // 对应权重需要大于阈值，对这些关键帧建立连接 vPairs.push_back(make_pair(mit->second,mit->first)); // 对方关键帧也要添加这个信息 // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重 (mit->first)->AddConnection(this,mit->second); } } ​ 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理 ​ 4.更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树 // 更新图的连接(权重) mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重 mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); 更新生成树的连接 if(mbFirstConnection && mnId!=0) { // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧 mpParent = mvpOrderedConnectedKeyFrames.front(); // 建立双向连接关系 mpParent->AddChild(this); mbFirstConnection = false; }` 8. BA优化两帧以及地图点 9. 将MapPoints的中值深度归一化到1，并归一化两帧之间变换 评估当前关键帧场景深度，q=2表示中值. 只是在单目情况下才会使用，其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值也就是中位数作为当前场景的平均深度 float medianDepth = pKFini->ComputeSceneMedianDepth(2); float invMedianDepth = 1.0f/medianDepth;//两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100 if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100) { cout << "Wrong initialization, reseting..." << endl; Reset(); return; }` 将深度中值作为单位一，归一化第二帧的位姿与所有的地图点 。 //将两帧之间的变换归一化到平均深度1的尺度下 // Scale initial baseline cv::Mat Tc2w = pKFcur->GetPose(); // x/z y/z 将z归一化到1 Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth; pKFcur->SetPose(Tc2w); // 把3D点的尺度也归一化到1 //? 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches(); for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++) { if(vpAllMapPoints[iMP]) { MapPoint* pMP = vpAllMapPoints[iMP]; pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth); } } 至此单目初始化完成。 2.相机位姿跟踪 初始化完成后，对于相机获取当前图像mCurrentFrame，通过跟踪匹配上一帧mLastFrame特征点的方式，可以获取一个相机位姿的初始值；为了兼顾计算量和跟踪鲁棒性，追踪部分主要用了三种模型：运动模型（TrackWithMotionModel）、关键帧（TrackReferenceKeyFrame）和重定位（Relocalization）。三种跟踪模型都是为了获取相机位姿一个粗略的初值，后面会通过跟踪局部地图TrackLocalMap对位姿进行BundleAdjustment（捆集调整），进一步优化位姿。 几个命名的区别 上一帧mLastFrame：即上一帧图像 上一关键帧mpLastKeyFrame：最邻近当前帧的关键帧，不一定是上一帧，因为图像帧要经过判断后，满足条件才能称为关键帧 参考关键帧mpReferenceKF：就是当前帧的上一关键帧；若创建了新的关键帧，参考关键帧就更新为新创建的关键帧；或者是局部地图中与当前帧共享地图点最多的关键帧。 1. TrackwithMotionModel 当上一帧跟踪正常的时候，我们会生成一个速度模型mVelocity，使得mVelocity不为空，下一帧就会进入TrackWithMotionModel()。 速度模型mVelocity就是上一帧和上上帧的位姿变化。由于在短时间内，速度模型不会变化太大，这样我们可以认为上一帧的速度模型（即上一帧和上上帧的位姿变化）等于当前帧的速度模型（即当前帧和上帧的位姿变化）。根据上一帧的求得的速度模型，以及上一帧在世界坐标系的位姿得到当前帧在世界坐标系中的初始位姿T0 if(!mLastFrame.mTcw.empty()) { // step 2.3：更新恒速运动模型 TrackWithMotionModel 中的mVelocity cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F); //转换成为了相机相对世界坐标系的旋转 mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3)); mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3)); mVelocity = mCurrentFrame.mTcw*LastTwc; // 其实就是 Tcl } else //否则生成空矩阵 mVelocity = cv::Mat(); 我们将上一帧匹配的mappoint，通过T0 投影到当前帧上，然后在投影位置附近搜索当前帧的特征点以加速上一帧mappoint和当前帧的特征点匹配。也就是说，以上一帧为跳板，将当前帧的特征点与空间中mappoint进行匹配。 int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR); 有了当前帧的初始位姿，以及当前帧的特征点与空间中mappoint进行匹配关系。可以在通过bundle adjustment方法优化得到当前帧的较为精确的位姿T1 得到当前帧较为精确的位姿后，当前帧的有些特征点和mappoint匹配通过这个T1 将不成立，形成误匹配于是将它标记为outliner后取消它们之间的匹配。 2. TrackReferenceKeyFrame 如果恒速模型不被满足,那么就只能够通过参考关键帧来定位了， 可以尝试和最近一个关键帧（即参考关键帧）去做匹配。利用了bag of words（BoW）来加速匹配。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配；最后，利用匹配的特征优化位姿 int nmatches = matcher.SearchByBoW( mpReferenceKF, //参考关键帧 mCurrentFrame, //当前帧 vpMapPointMatches); //存储匹配关系 3.Relocalization 如果正常的初始化不成功,或者 mState=LOST ，那么就只能重定位了。 当速度模型不能用，参考关键帧也没有的时候，就意味着tracking已经跟丢了。这时候，我们先将当前帧转化为BOW，然后将代表当前帧的BOW交给KeyFrameDatabase，它就会返回一个和当前帧很相似的集合作为候选帧。 vector<KeyFrame*>vpCandidateKFs= mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame); 接着，我们对当前帧和候选关键帧进行通过BOW加速的特征点匹配。并剔除特征匹配数少于15的候选帧。 int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); 对于剩下的候选关键帧我们通过epnp结合RANSAC算法进行筛选，并算出当前帧在世界坐标系中的位姿T2 PnPsolver* pSolver = vpPnPsolvers[i]; cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers); 对求解结果使用BA优化，如果内点较少，则反投影当前帧的地图点到候选关键帧获取额外的匹配点；若这样依然不够，放弃该候选关键帧，若足够，则将通过反投影获取的额外地图点加入，再进行优化。 如果内点满足要求(>50)则成功重定位，将最新重定位的id更新：mnLastRelocFrameId = mCurrentFrame.mnId; 否则返回false。 3.TrackLocalMap 通过上面三种模型获取了初始的相机位姿和初始的特征匹配，就可以将完整的地图（地图点）投影到当前帧中，去搜索更多的匹配。但是投影完整的地图，在large scale的场景中是很耗计算而且也没有必要的，因此，这里使用了局部地图LocalMap来进行投影匹配。对局部地图的更新包括对局部关键帧和局部地图点的更新。局部地图包含：与当前帧相连的关键帧K1（所有能观察到当前帧对应地图点的关键帧，图中Pos2），以及与K1相连的关键帧K2（一级二级相连关键帧，图中Pos1），并且限制了关键数量不超过80；K1、K2对应的地图点（图中X1）；参考关键帧Kf。局部地图就是红色椭圆圈出的部分，参与局部优化，其中红色代表取值会被优化，灰色代表取值保持不变。 1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints UpdateLocalMap(); 局部关键帧的选择策略： 策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧 策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧 ​ 最佳共视的10帧，如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧；自己的子关键帧 2.在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪 SearchLocalPoints(); 3.更新局部所有MapPoints后对位姿再次优化 Optimizer::PoseOptimization(&mCurrentFrame); 更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果 4.LocalMapping局部建图 LocalMapping线程由LocalMapping::Run()进入； 它主要做了以下几件事： 向map中插入关键帧； 新近插入的mappoint的剔除； 新建mappoint； 局部捆集调整（BA）； 剔除冗余关键帧； 首先判断是否需要插入关键帧 1：如果用户在界面上选择定位模式，那么将不插入关键帧 2：判断是否距离上一次插入关键帧的时间太短 if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && //距离上一次重定位不超过1s nKFs>mMaxFrames) //地图中的关键帧已经足够 return false; 3:得到参考关键帧跟踪到的MapPoints数量, which 参考帧地图点的被观测的数目大于等于 nMinObs int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs); 4.查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧 bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames(); 5.决策是否需要插入关键帧： 距离上一次重定位距离至少20帧； 局部地图线程空闲，或者距离上一次加入关键帧过去了20帧；如果需要关键帧插入（过了20帧）而LocalMapping线程忙，则发送信号给LocalMapping线程，停止局部地图优化，使得新的关键帧可以被及时处理（20帧，大概过去了不到1s）； 当前帧跟踪至少50个点；确保了跟踪定位的精确度； 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%；确保关键帧之间有明显的视觉变化。 创建新的关键帧 void Tracking::CreateNewKeyFrame() 将关键帧传递到LocalMapping线程（mpLocalMapper->InsertKeyFrame(pKF)），再由LocalMapping完成其他工作。此外，在创建关键帧之后，对于双目、RGB-D的情况，会使用深度值大于0的关键点重投影得到地图点，得到的地图点会和当前关键帧关联，并加入到Map。 最后记录位姿信息，用于轨迹复现","link":"/2020/09/20/orbslam2%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90-%E7%89%B9%E5%BE%81%E8%B7%9F%E8%B8%AA%E5%8F%8A%E5%88%9D%E5%A7%8B%E5%8C%96/"},{"title":"orbslam2代码解析--局部建图","text":"LocalMapping模块作用是将Tracking中送来的关键帧放在mlNewKeyFrame列表中；处理新关键帧、地图点检查剔除、生成新地图点、Local BA、关键帧剔除。主要工作在于维护地图，也就是SLAM中的Map。 Tracking模块会将满足一定条件的图像帧加入关键帧，但是其实ORB-SLAM中关键帧的加入是比较密集的，这样确保了定位的精度。同时LocalMapping线程会对关键帧进行剔除，确保了关键帧的数量不会无限增加，不会对large scale的场景造成计算负担。Tracking模块会判断是否需要将当前帧创建为关键帧，对地图中的关键帧、地图点具体的处理，包括如何加入、如何删除的工作是在LocalMapping线程完成的。 1 处理新关键帧 LocalMapping::ProcessNewKeyFrame() 处理新关键帧与局部地图点之间的关系。具体步骤如下 **获取关键帧。**从mlNewKeyFrame队列（待插入的关键帧）中弹出队首的关键帧做为当前帧，并计算该关键帧的BoW，后面三角化恢复地图点有用； **获取关键帧中与关键点关联的地图点。**将TrackLocalMap中跟踪局部地图匹配上的地图点关联到当前关键帧（在Tracking线程中只是利用关键点的匹配关系进行位姿计算，优化当前关键帧姿态）。具体来说，可以分两种情况，地图点没有关联到关键帧，则完成关联（AddObservation），更新地图点normal和描述子，这种地图点是在追踪过程创建并被关键帧关联的，它们在UpdateLastFrame过程就已经有了空间位置；否则记录该地图点为最新添加（这些地图点是在创建关键帧时创建的，已经关联到关键帧），加入mlpRecentAddedMapPoints； **更新共视图连接关系。**使用UpdateConnections函数更新加入当前关键帧之后关键帧之间的连接关系，包括更新Covisibility图和Essential图（最小生成树spanning tree）； 关键帧插入地图。 2.地图点剔除 LocalMapping::MapPointCulling()。任务是对上一函数获取到的最新加入的局部地图点mlpRecentAddedMapPoints进行检查，每个地图点要被保留，在该地图点被创建后的三个关键帧里必须要经过严格的测试，这样保证其能被正确的跟踪和三角化。满足如下条件之一就被剔除： 该地图点是坏点，直接从检查列表去掉； 跟踪（匹配上）到该地图点的普通帧帧数（IncreaseFound）<应该观测到该地图点的普通帧数量（25%*IncreaseVisible），即比值mnFound/mnVisible<0.25，设置为坏点，并从检查列表去掉。比值低说明这样的地图点该地图点虽在视野范围内，但很少被普通帧检测到； 从添加该地图点的关键帧算起，当前关键帧至少是第三个添加该地图点的关键帧的条件下，看到该地图点的帧数<=2（双目和RGBD模式是帧数<=3），设置为坏点，并从检查列表去掉；因此在地图点刚建立的阶段，要求比较严格，很容易被剔除；而且单目的要求更严格，需要三帧都看到； 若从添加该地图点的关键帧算起，一共有了大于三个关键帧，还存在列表中，则说明该地图点是高质量的，从检查列表中去掉。 3.生成新地图点（三角化方法） 对应LocalMapping::CreateNewMapPoints()函数。任务是根据当前关键帧恢复出一些新的地图点，不包括和当前关键帧匹配的局部地图点（已经在ProcessNewKeyFrame中处理，单目模式除了初始化过程会生成地图点外，其它地图点都在这里生成）。具体步骤如下： 找出与当前帧有共视关系的10个（单目模式是20个）关键帧，准备使用对极约束搜索匹配并进行三角化，循环进行以下操作，注意是在当前帧和每一帧共视关键帧之间进行； 检测基线是否过短； 计算基础矩阵F； 搜索满足对极约束的匹配； matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false); 通过各种条件判断是不是要三角化这些地图点； 完成对匹配结果的三角化，创建新的地图点； 为新的地图点进行一系列配置。 4.临近关键帧搜索更多匹配 对应LocalMapping::SearchInNeighbors()函数。如果关键帧队列中还有新的关键帧，则进行该操作，即在临近的关键帧中搜索到更多的匹配，更新并融合当前关键帧以及两级相连（共视关键帧及其共视关键帧）的关键帧的地图点。 5.Local Bundle Adjustment 对应Optimizer::LocalBundleAdjustment()函数。这里优化的是当前帧，以及与当前帧在covisibility graph里面有连接关系的那些关键帧的位姿，以及这些帧看到的地图点，其实就是对局部地图进行优化。详细分析参见优化部分博客。 6.冗余关键帧剔除 对应LocalMapping::KeyFrameCulling()函数。候选的pKF是LocalMapping中当前处理的关键帧的共视关键帧，不包括第一帧关键帧与当前关键帧。如果一个关键帧检测到的90%的地图点，在其他不少于三个具有相同或更精确尺度的关键帧里面都被检测到，就认定该关键帧冗余（该关键帧的存在提供的地图点观测信息有限），并剔除","link":"/2020/09/28/orbslam2%E4%BB%A3%E7%A0%81%E8%A7%A3%E6%9E%90-%E5%B1%80%E9%83%A8%E5%BB%BA%E5%9B%BE/"}],"tags":[{"name":"SLAM","slug":"SLAM","link":"/tags/SLAM/"},{"name":"DSO","slug":"DSO","link":"/tags/DSO/"},{"name":"后端优化","slug":"后端优化","link":"/tags/%E5%90%8E%E7%AB%AF%E4%BC%98%E5%8C%96/"},{"name":"optimization","slug":"optimization","link":"/tags/optimization/"},{"name":"初始化","slug":"初始化","link":"/tags/%E5%88%9D%E5%A7%8B%E5%8C%96/"},{"name":"Tracking","slug":"Tracking","link":"/tags/Tracking/"},{"name":"零空间","slug":"零空间","link":"/tags/%E9%9B%B6%E7%A9%BA%E9%97%B4/"},{"name":"ORBSLAM2","slug":"ORBSLAM2","link":"/tags/ORBSLAM2/"},{"name":"LocalClosing","slug":"LocalClosing","link":"/tags/LocalClosing/"},{"name":"回环检测","slug":"回环检测","link":"/tags/%E5%9B%9E%E7%8E%AF%E6%A3%80%E6%B5%8B/"},{"name":"特征跟踪","slug":"特征跟踪","link":"/tags/%E7%89%B9%E5%BE%81%E8%B7%9F%E8%B8%AA/"},{"name":"局部建图","slug":"局部建图","link":"/tags/%E5%B1%80%E9%83%A8%E5%BB%BA%E5%9B%BE/"},{"name":"LocalMapping","slug":"LocalMapping","link":"/tags/LocalMapping/"}],"categories":[{"name":"SLAM","slug":"SLAM","link":"/categories/SLAM/"},{"name":"DSO","slug":"SLAM/DSO","link":"/categories/SLAM/DSO/"},{"name":"ORBSLAM2","slug":"SLAM/ORBSLAM2","link":"/categories/SLAM/ORBSLAM2/"}]}