- 博客(72)
- 收藏
- 关注
RSS订阅原创 VINS-Mono+Fusion源码解析系列（二十一）：滑动窗口
（1）大致流程若滑窗中倒数第二帧为关键帧，则执行边缘化最老帧流程若滑窗中倒数第二帧不是关键帧，则执行边缘化倒数第二帧流程（2）代码实现。
2023-12-04 13:46:10
577
原创 VINS-Mono+Fusion源码解析系列（二十）：边缘化约束
当滑动窗口新增关键帧时，由于由于滑动窗口大小一定，新进来一帧关键帧意味着必然有老的一帧被剔除出去。直接暴力剔除被边缘化帧会损失掉与之相关的信息，因此需要通过舒尔补的方式，在保留它与剩余帧之间的联系前提下将其剔除。 对于滑动窗口中优化求解的增量方程，有：Hδx=bH\delta x = bHδx=b 根据待边缘化的状态量，对上述方程进行划分，如下所示：[ΛaΛbΛbTΛc][δxaδxb]=[gagb]\left[ \begin{array}{cc} \Lambda_a & \L
2023-10-27 08:34:12
381
1
原创 VINS-Mono+Fusion源码解析系列（十九）：视觉重投影约束
1. VINS-Mono中的视觉重投影（1）理论分析 如下图所示，由于VINS-Mono中滑窗里面维护的位姿是IMU坐标系下的位姿，因此对于当前第iii帧中的特征点，一方面先通过逆深度投影到第iii帧的相机归一化坐标系中，然后通过相机与IMU之间的外参投影到第iii帧IMU坐标系中，再通过IMU坐标系之间的转换得到它在第jjj帧IMU坐标系中的投影（实际上是先转换到世界坐标系中，然后再从世界坐标系转换到第jjj帧IMU坐标系中），然后通过相机与IMU之间的外参将其转换到第jjj帧相机坐标系中，最后通过
2023-10-25 08:46:26
357
原创 VINS-Mono+Fusion源码解析系列（十八）：IMU预积分约束
（1）使用方法 损失函数F(x)F(x)F(x)由残差函数f(x)f(x)f(x)组成：minxF(x)=12∣∣f(x)∣∣22\min_x F(x) = \frac{1}{2}||f(x)||_2^2xminF(x)=21∣∣f(x)∣∣22 当考虑方差时，可以写为：minxF(x)=f(x)TΩf(x)\min_x F(x) = f(x)^T\Omega f(x)xminF(x)=f(x)TΩf(x) 利用高斯牛顿方法进行优化求解，可得到如下增量方程：
2023-10-25 08:46:08
283
原创 VINS-Mono+Fusion源码解析系列（十七）：非线性优化
（1）大致思路 按照后端流程，VIO初始化initialStructure完成之后，执行非线性优化solveOdometry。（2）代码实现。
2023-10-25 08:45:36
200
原创 VINS-Mono+Fusion源码解析系列（十六）：重力修正
以上视觉惯性对齐做的是通过对每一组图像帧和IMU预积分之间的关系，求出每一帧的速度，枢纽帧的重力加速度和尺度。在求解时，没有引入重力的先验知识，而在大部分情况下重力的先验是已知的，因此需要利用重力的先验知识对估计出的枢纽帧处的重力加速度进行修正。 取出前面已经求出的枢纽帧下的重力方向，将其大小设为先验值9.81，作为当前重力的大小和方向。在ORB-SLAM3中，将重力先验直接放到整体中基于最大后验估计进行优化，而在VINS-Mono中是分两步进行的。，二者的矢量和为重力运动的方向。
2023-10-25 08:45:19
304
原创 VINS-Mono+Fusion源码解析系列（十五）：视觉惯性对齐
1. VisualIMUAlignment（1）大致思路通过solveGyroscopeBias求出陀螺仪零偏通过LinearAlignment求出每帧的速度，加速度修正以及平移尺度s（2）代码实现/** * @brief * * @param[in] all_image_frame 每帧的位姿和对应的预积分量 * @param[out] Bgs 陀螺仪零偏 * @param[out] g 重力向量 * @param[out] x 其他状态量 * @return true
2023-10-25 08:45:01
271
原创 VINS-Mono+Fusion源码解析系列（十三）：纯视觉SFM
假设滑窗中一共有11帧，首先需要选取一个枢纽帧，利用枢纽帧和最后一帧通过求出这两帧之间的相对位姿。对枢纽帧的要求是：一方面要求枢纽帧离最后一帧尽可能远，因为离最后一帧比较近时，二者之间的平移比较小，使得三角化精度较差，甚至会无法进行对极约束（平移为零的情况）。另一方面，要求枢纽帧与最后一帧之间不能距离太远，否则会使得二者之间关联的特征点比较少，导致求解出的位姿可信度不高，并且无法三角化出尽可能多的点。因此，在保证匹配点足够多的情况下，枢纽帧离最后一帧应该尽可能远，如下图中选取第4帧作为枢纽帧。
2023-10-25 08:44:30
395
原创 VINS-Mono+Fusion源码解析系列（十二）：单目VIO初始化
（1）为什么需要初始化？ 基于优化框架的VIO非常依赖于初始值，因为解优化问题类似于一个迭代下降的过程，以初始值为起点，通过高斯牛顿或L-M算法，不断迭代寻找极小值。为保证最终的极小值为最小值，防止陷入局部极小值，就要求给定的起始初值离最小值足够接近（要求有良好的初值），并且这样迭代的次数就会更少，计算量也会减少，从而保证系统实时性。（2）初始化哪些变量？
2023-10-25 08:44:14
337
原创 VINS-Mono+Fusion源码解析系列（十一）：后端processIMU与processImage
主要是维持滑窗中的预积分量pre_integrations，为后续的优化提高状态初值。
2023-10-25 08:43:58
559
原创 VINS-Mono+Fusion源码解析系列（九）：后端流程
（1）大致流程ros节点初始化读取配置文件参数readParameters优化器参数设置setParameter：包括外参与时延，以及视觉重投影的置信度接收imu消息，执行回调函数imu_callback接收前端视觉光流结果，执行回调函数feature_callback接收前端重启命令，执行回调函数restart_callback（在前端光流跟丢之后进行重启）接收回环检测消息，执行回调函数relocalization_callback处理核心线程process（2）代码实现。
2023-10-24 08:31:18
119
原创 VINS-Mono+Fusion源码解析系列（八）：IMU预积分的代码实现
代码中的下标与上述理论推导公式中的不一样，这里的k表示初始的第i个图像帧，i与i+1表示初始图像帧之后的两相邻IMU时刻。（1）预积分的构造函数。（2）预积分调用接口。
2023-10-24 08:31:01
331
原创 VINS-Mono+Fusion源码解析系列（七）：IMU预积分中关于IMU零偏的建模
因此，优化后的预积分量也是会发生变化的。若每优化一次就重新计算一次预积分，这样会造成较大的耗时。 在预积分中，由于时间间隔较短，偏置bias认为是不变的。 那么，各个状态关于零偏的雅可比就是。求导的部分，其他的以此类推。
2023-10-24 08:30:47
132
原创 VINS-Mono+Fusion源码解析系列（六）：IMU预积分中误差的分析与影响
前面在推导预积分时，imu的读数会减去它的偏置与噪声项，其中偏置可以作为状态量获取，但是噪声项是没有办法获得的，我们能做的就是拿到imu数据减去偏置后直接使用，因此噪声项引入了不确定性，需要求出预积分的信息矩阵（协方差矩阵的逆）。通过下面的误差卡尔曼滤波来引出预积分的误差传递方程，从而可进一步推导出协方差矩阵的形式。 因此，只要推导出预积分的误差传递方程中的矩阵F和G，就可计算出协方差矩阵。
2023-10-24 08:30:29
129
原创 VINS-Mono+Fusion源码解析系列（五）：IMU预积分的误差传递
1. 连续时间下IMU预积分误差传递的推导 首先，给出连续时间下IMU预积分误差δzt\delta z_tδzt的传递方程，然后对各预积分量的误差传递形式进行推导：（1）位置项δα˙tbk\delta \dot{\alpha}_t^{b_k}δα˙tbk的推导 真实值下的位置导数等于其对应的真实速度，即P˙t=Vt\dot{P}_t = V_tP˙t=Vt，测量值下的位置导数也等于其对应的测量值速度，即P˙=V\dot{P} = VP˙=V。又由于真实值 = 测量值 + 误差项，
2023-10-24 08:30:07
201
原创 VINS-Mono+Fusion源码解析系列（四）：IMU预积分的推导
1. PVQ模型在t时刻，IMU在世界坐标系下的位置P、速度V、姿态q（PVQ模型）的导数模型如下：p˙btw=vtwv˙tw=atwq˙btw=qbtw⊗[012wtb]\dot{p}_{b_t}^w = v_t^w\\\dot{v}_t^w=a_t^w\\\dot{q}_{b_t}^w = q_{b_t}^w \otimes \left[\begin{array}{cc} 0 \\ \frac{1}{2}w_t^b \end{array} \right]p˙btw=vtwv˙tw
2023-10-24 08:28:56
352
原创 VINS-Mono+Fusion源码解析系列（三）：IMU预积分前置知识
旋转矩阵：优点是运算方便，使用9个量描述3自由度旋转。缺点是引入额外约束（如要求满足正交性、行列式为1等），求导困难旋转向量：以单位向量n为旋转轴，绕其旋转角度θ。可紧凑的表示旋转，缺点是具有周期性(若限定在某一周期范围内，如[-pi, pi]，则不具有周期性)欧拉角：比较直观，对用户友好；需要指定旋转顺序，存在万向节死锁现象，无法进行球面平滑差值。
2023-10-24 08:28:37
237
原创 VINS-Mono+Fusion源码解析系列（二）：视觉前端的图像处理
（1）大致流程检查时间戳是否异常：图像时间间隔超过1s，或者时间差错乱：当前帧时间戳 < 上一帧时间戳检查频率：控制向后端发送频率，以及运行时频率隐性异常处理读取图片并进行相应处理（readImage）：光流追踪 + 去畸变 + 均匀化等更新特征点ID（updateID）向后端发送特征点信息：特征点的id + 去畸变后的像素坐标 + 特征点的速度（2） 整体代码(4) 运行时的频率隐性异常freqΔtframecount。
2023-10-24 08:28:13
395
原创 VINS-Mono+Fusion源码解析系列（一）：视觉前端流程
本系列文章以VINS-Mono+Fusion源码流程为导向，对源码进行了详细的逐行注释，并结合原理首推IMU预积分，对滑动窗口、边缘化等进行了详细阐述，旨在细致入微地对VINS-Mono+Fusion做一个全面整体的了解。
2023-10-24 08:27:47
262
原创 ORB-SLAM2源码逐行解析系列（二十四）：ORB-SLAM2中的全局优化
全局优化函数BundleAdjustment主要用于优化所有关键帧的位姿和地图点。因此，其顶点是待优化的所有关键帧的位姿和所有地图点。 由于地图点和观测到它的关键帧存在投影约束关系，因此全局优化中的边是二元边。边的两个顶点分别是地图点坐标以及观测到它的关键帧的位姿。 对于单目相机模式，边的类型为g2o::EdgeSE3ProjectXYZ。g2o::VertexSE3Expmap，顶点中地图点的类型为g2o::VertexSBAPointXYZ。（2）全局优化与局部优化的区别。
2023-10-24 08:25:46
140
原创 ORB-SLAM2源码逐行解析系列（二十三）：ORB-SLAM2闭环线程中的本质图优化
边在Sim(3)优化中是指地图点和Sim(3)位姿的投影关系，由于地图点和Sim(3)位姿都参与优化，因此是二元边。 Sim(3)位姿优化发生在闭环检测过程中，仅对形成闭环的Sim(3)位姿进行优化，不优化地图点。但由于地图点与（当前关键帧之间）产生约束，因此顶点包括待优化的（当前关键帧的）Sim(3)位姿和地图点（固定住不优化）。v2：二元边的第1个顶点，即第j帧在世界坐标系下的Sim(3)位姿Sjw，观测量C表示观测到的第i帧到第j帧之间的Sim(3)变换Sji。
2023-10-23 08:34:24
78
原创 ORB-SLAM2源码逐行解析系列（二十二）：ORB-SLAM2闭环线程中的Sim3优化
边在Sim(3)优化中是指地图点和Sim(3)位姿的投影关系，由于地图点和Sim(3)位姿都参与优化，因此是二元边。 正向投影：cam_map1(project(v1->estimate().map(v2->estimate())))表示用v1估计的位姿g2oS12把v2估计的相机2坐标系下的三维点转换到相机1坐标系下，然后通过project函数归一化三维点，再通过cam_map1函数用内参将其转化为像素坐标。v2->estimate()表示估计的相机2坐标系下的三维点坐标，
2023-10-23 08:29:24
112
原创 ORB-SLAM2源码逐行解析系列（二十一）：ORB-SLAM2局部建图线程中的优化方法
局部建图优化中的边是二元边，它连接的需要优化的两个顶点分别是：_vertices[0]，表示待优化的局部关键帧的地图点； 由于局部关键帧所观测到的地图点也会被当前关键帧的二级共视关键帧观测到，即二级共视关键帧对这些地图点也会产生约束，因此顶点包括需要优化的当前帧的局部关键帧和对应的局部地图点，以及不参与优化的当前帧的二级共视关键帧（在优化时被固定住）。其中，局部关键帧是指当前关键帧的一级共视关键帧，局部地图点是指局部关键帧观测到的所有地图点。添加待优化的位姿顶点——局部关键帧的位姿。
2023-10-23 08:29:06
153
原创 ORB-SLAM2源码逐行解析系列（二十）：ORB-SLAM2跟踪线程中的优化方法
v1->estimate()表示顶点的估计值，在优化过程中不断更新，参与到重投影误差的计算当中。 因此，顶点类型是g2o::VertexSE3Expmap.对于单目相机，边的类型为g2o::EdgeSE3ProjectXYZOnlyPose，对于双目/RGB-D相机，边的类型为g2o::EdgeStereoSE3ProjectXYZOnlyPose。在跟踪线程中，PoseOptimization主要是获取3D-2D的最小化重投影误差，在优化过程中只对当前关键帧的位姿进行优化，不优化地图点的坐标。
2023-10-23 08:28:47
101
原创 ORB-SLAM2源码逐行解析系列（十八）：局部建图线程
（1）局部建图线程的内容 局部建图线程接收跟踪线程输入的关键帧，利用关键帧的共视关系生成新的地图点，搜索融合相邻关键帧的地图点，然后进行局部地图优化，删除冗余关键帧等操作，最后将处理后的关键帧发送给闭环线程。（2）局部建图线程的作用 局部建图线程的目的是让已有的关键帧之间产生更多的联系，生成更多可靠的地图点，优化共视关键帧的位姿及其地图点，使得跟踪更稳定，参与闭环的关键帧位姿更准确。
2023-10-23 08:28:07
125
原创 ORB-SLAM2源码逐行解析系列（十七）：ORB-SLAM2中的跟踪方法
（1）原理 参考关键帧跟踪是将当前普通帧（位姿未知）和它对应的参考关键帧（位姿已知）进行特征匹配及优化，从而估计当前普通帧的位姿。其中，可通过基于词袋的SearchByBoW来加速两帧之间的特征匹配。（2）应用场景（3）大致流程（4）代码实现。
2023-10-23 08:27:44
175
原创 ORB-SLAM2源码逐行解析系列（十六）：ORB-SLAM2特征匹配之地图点融合
如果投影的地图点能匹配到当前关键帧的特征点，并且该特征点自身有对应的地图点，那么选择这两个地图点中被观测数目最多的那个来替换这两个地图点。 当地图点规模比较大时，会存在很多邻近的地图点，有的来自跟踪线程，有的来自局部建图线程，有的来自闭环线程，这些邻近的地图点很可能是同一个路标点在不同阶段形成的“影子”。投影到当前关键帧中。如果投影的地图点能匹配到当前关键帧的特征点，并且该特征点自身有对应的地图点，那么选择这两个地图点中被观测数目最多的那个来替换这两个地图点。（3）闭环线程中的地图点融合。
2023-10-23 08:27:22
163
原创 ORB-SLAM2源码逐行解析系列（十五）：ORB-SLAM2特征匹配之基于Sim3变换的特征匹配
在闭环线程中，由于闭环候选帧与当前关键帧时间间隔较远，没有先验信息，因此最早是通过词袋进行搜索匹配的（SearchByBoW），但是词袋搜索会存在漏匹配。此时，可采用初步估计的Sim(3)位姿对当前关键帧和闭环候选关键帧进行相互投影匹配，目的是生成更多的匹配点对。（3）相互投影匹配的大致流程。（4）相互投影匹配的代码实现。（1）相互投影匹配的目的。（2）相互投影匹配的思路。
2023-10-23 08:26:50
100
原创 ORB-SLAM2源码逐行解析系列（十四）：ORB-SLAM2特征匹配之基于投影点的特征匹配
投影匹配的大致思路是将地图点通过相机位姿投影到当前帧的二位像素平面上，然后在对应像素坐标周围的圆形区域内寻找候选匹配点，通过一定的筛选条件剔除误匹配，最终在当前帧中得到与该地图点匹配的特征点。 当相机发生运动（前进或后退）时，特征点的尺度会发生变化，需要确定图像金字塔的层级搜索范围，才能保证特征点能够正确匹配。 对于单目相机，由于无法获得绝对尺度，因此不进行相机前进或后退的判定，在搜索匹配时限定在当前金字塔层级。判断相机后退，则当前层的特征点尺度变小，需要在更低层上进行匹配。
2023-10-23 08:26:24
184
原创 ORB-SLAM2源码逐行解析系列（十三）：ORB-SLAM2特征匹配之基于词袋的特征匹配
词袋（Bag of Words, BoW）最早用于自然语言领域，其中Words表示文本中的单词。在视觉SLAM中，Words表示图像中的局部信息，如特征点。需要注意是的，在词袋中的单词是没有顺序的，只关心单词出现的次数。这样既简化了词袋模型的表达方式，又节省了存储空间，可实现高效索引。 词袋模型的使用 **注意：**在实际使用时，如无特殊要求，通常使用已训练好的字典，调用第三方库加载字典，将特征点转换为特征向量和词袋向量，无需手动实现这一操作。
2023-05-16 21:47:50
626
1
原创 ORB-SLAM2源码逐行解析系列（十二）：ORB-SLAM2特征匹配之单目初始化中的特征匹配
通常取直方图中前3个bin中的特征点对为正常的匹配点对，其余的均为误匹配点对，需要剔除掉。 在ORB-SLAM2中进行单目初始化时，其思想是：认为参与初始化的两帧默认是比较接近的，即在第一帧提取的特征点坐标对应的第二帧中的位置画一个圆形邻域（代码中为正方形），与之匹配的特征点应该落在这一邻域内，称为候选匹配点。 因此，ORB-SLAM2中单目初始化需要先确定候选匹配点，然后将当前特征点与候选匹配点进行匹配，获取满足匹配条件的候选匹配点。（2）ORB-SLAM2中单目初始化的特征匹配。
2023-05-16 09:18:05
310
原创 ORB-SLAM2源码逐行解析系列（十一）：ORB-SLAM2中的关键帧
函数的目的是为当前关键帧新建或更新与其共视的关键帧的连接权重，因此主要是更新当前关键帧所属变量mConnectedKeyFrameWeights里面共视关键帧的权重。 成员变量mConnectedKeyFrameWeights中存放的是与当前关键帧共视的关键帧及对应的权重，因此需要遍历mConnectedKeyFrameWeights中的每个元素，调用EraseConnection删除其与当前关键帧的联系。（2）为该待删除关键帧的子关键帧重新寻找合适的父关键帧。（1）重新排序的作用。
2023-05-16 09:17:37
807
原创 ORB-SLAM2源码逐行解析系列（十）：ORB-SLAM2中的地图点
public :/*** @brief 给定坐标与keyframe构造MapPoint* @details 被调用:* 双目: StereoInitialization(),CreateNewKeyFrame(),LocalMapping::CreateNewMapPoints()* 单目: CreateInitialMapMonocular(), LocalMapping::CreateNewMapPoints()* @param[in] Pos MapPoint的坐标（wrt世界坐标系）
2023-05-16 09:17:18
334
原创 ORB-SLAM2源码逐行解析系列（九）：ORB-SLAM2中基于F矩阵的单目初始化
1. 计算基础矩阵F（1）大致流程对匹配的特征点对进行归一化进行RANSAC迭代，在每次迭代中执行如下操作：根据前面得到的mvSets，获取用于当前迭代的经归一化后的8对匹配点8点法计算基础矩阵ComputeF21将基于归一化点的基础矩阵恢复为基于输入的匹配特征点的基础矩阵将特征点到对应极线的距离作为误差，结合卡方分布获取当前基础矩阵的得分CheckFundamental更新最佳评分时的基础矩阵与内点标记（2）代码实现/** * @brief 计算基础矩阵，假设场景为非平面情
2023-05-16 09:15:56
237
原创 ORB-SLAM2源码逐行解析系列（八）：ORB-SLAM2中基于H矩阵的单目初始化
通过单应矩阵对已经判断为内点的特征点进行双向投影，计算加权重投影误差，结合卡方分布获取当前单应矩阵的得分，最终选择所有迭代中得分最高的一组对应的单应矩阵作为最优的解。，并且上式是在理想状态下的结果，实际上特征点和单应矩阵都存在误差，等式右边不为0。 通过下式累积所有特征点对当前单应矩阵的评分，误差越大，该单应矩阵评分越低。表示自由度为2的卡方分布在显著性水平为0.05时对应的临界阈值。 为了转化为齐次方程，左右两边同时叉乘。进行奇异值分解，得到的右奇异矩阵的最后一列就是。
2023-05-15 08:14:48
194
原创 ORB-SLAM2源码逐行解析系列（七）：ORB-SLAM2中的单目初始化
*** @brief 单目SLAM初始化相关，双目和RGBD不会使用这个类// 定义一个int-int的pair类型，用于描述两帧中特征点的匹配关系 public :/*** @brief 根据参考帧构造初始化器* @param[in] ReferenceFrame 参考帧* @param[in] sigma 测量误差* @param[in] iterations RANSAC迭代次数/**
2023-05-15 08:13:33
373
原创 ORB-SLAM2源码逐行解析系列（六）：ORB-SLAM2中的单目Track
ORB-SLAM2中跟踪线程主要分为两个阶段，第一个阶段包括3种跟踪方式：参考关键帧跟踪、恒速模型跟踪和重定位跟踪，它们的目的是保证能够跟得上，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，它将当前帧的局部关键帧对应的局部地图点投影到该帧中，得到更多的特征点匹配关系，对第一阶段的位姿再次进行优化，得到相对准确的位姿（这几种跟踪方式的具体实现后面会详细介绍）。
2023-05-15 08:13:17
253
1
空空如也
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人