- 博客(87)
- 收藏
- 关注
RSS订阅原创 05 Ceres
minx12∑iρi∥fixi1xik∥2stlj⩽xj⩽ujxmin21i∑ρi∥fixi1xik∥2s.t.lj⩽xj⩽uj其中ρi∥fixi1xik∥2ρi∥fixi1xik∥2为残差块；f⋅f(\cdot)f⋅为代价函数，也即误差项；ρ⋅ρ⋅。
2024-03-28 16:33:33
728
原创 04 牛顿法、高斯牛顿法及 Cpp 实现
（1）最速下降法Δx∗−JxkΔx∗−Jxk（2）牛顿法HΔx−JHΔx−J（3）高斯牛顿法JxJT⏟HxxΔx−Jxfx⏟gxHxJxJTxΔxgx−Jxfx需要注意的是，最速下降法和牛顿法属于同一类型，他们的JJ和HH都是针对FxFx（也就是整个目标函数）；而高斯牛顿法的JJ是针对fxfx（也就是误差项）。此外，为减小数值，目标函数FxF。
2024-03-23 14:51:30
817
原创 03 图优化
选择你想要的图里的节点与边的类型，确定它们的参数化形式；往图里加入实际的节点和边；选择初值，开始迭代；每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；求解稀疏线性方程HkΔx=−bk，得到梯度方向；继续用GN或LM进行迭代。如果迭代结束，返回优化值。实际上，g2o能帮我们做好第3-6步，只需要做前两步即可。
2024-03-15 09:47:16
636
原创 02 RANSAC算法 及 Python 实现
RANSAC （Random Sample Consensus，随机抽样一致）算法的基本假设是样本中包含正确数据(inliers即内点，可以被模型描述的数据)，也包含异常数据(outliers 即外点，偏离正常范围很远、无法适应数学模型的数据)，也就是说数据集中含有噪声。我们的目的就是找出使内点最多的模型参数（类似最小二乘法，最小二乘法试图找到满足所有点的参数，而 RANSAC 是为了消除误匹配，尽量找到更多内点，去除外点）。
2024-03-13 19:19:03
665
原创 01 LM 算法及 Cpp 实现
太小，则说明实际减小的值远小于近似减小的值，即近似效果较差，需缩小近似范围，即增大阻尼系数。太大，则说明实际减小的值大于近似减小的值，则需放大近似范围，即减小。，需要计算的 step 就是这些待定系数的 step。其中，分子是实际下降的值，分母是近似下降的值。法（见《十四讲》），这里用阻尼法进行推导。那么问题转化为，对于每次迭代，求解最优的。接近于 1 ，则近似效果好；优化目标是待定系数，把他们看做。LM 算法有两种推导方式，即。的分母的计算（我的更快？可以看做是对于过大的。求导，并令其等于零，
2024-03-06 20:50:20
884
原创 09 闭环线程 LoopClosing
由于累计误差的存在，检测到回环后，当前关键帧和闭环关键帧的位姿并不是相等的，这时就需要计算二者之间的相似变换矩阵，用于后续优化。若当前关键帧组在之前的连续关键帧组中找到连续关系，则当前的连续关键帧组的连续长度加1；若当前关键帧组在之前的连续关键帧组中没能找到连续关系，则当前关键帧组的连续长度为0。若某关键帧组的连续长度达到3，则认为该关键帧实现闭环。② 将闭环候选关键帧和其共视关键帧组合成为关键帧组。③ 在当前关键组和前一帧的连续关键组间寻找连续关系，，闭环候选关键帧取自于。类型，表示关键帧组，
2024-03-02 16:57:02
868
原创 08 局部建图线程 LocalMapping
如果一个关键帧的地图点 90% 都能被超过 3 个其他关键帧观测到，则该帧冗余。（具体来说，就是遍历该关键帧的地图点，若某一地图点被超过3个其他关键帧观测到，则该地图点冗余，最终冗余地图点超过 90 %，则该关键帧冗余。局部建图线程接收跟踪线程输入的关键帧，利用关键帧的共视关系生成新的地图点，搜索融合相邻关键帧的地图点，然后进行局部地图优化、删除冗余关键帧等操作，最后将处理后的关键帧发送给闭环线程。若地图点反投影对应位置上不存在地图点，则直接添加观测；将地图点与帧中图像的特征点匹配，实现地图点融合。
2024-02-28 11:13:31
577
原创 07 跟踪线程 Tracking
第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。用局部地图点进行投影匹配，得到更多匹配关系，已经是当前帧地图点的不需要再投影，除此之外的点如果在当前帧的视野范围内，就进行投影匹配。函数，更新局部关键帧：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧；这是跟踪线程的第二阶段，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，从而对第一阶段的位姿再次优化得到相对准确的位姿。
2024-02-27 21:36:18
598
原创 06 单目初始化器 Initializer
用 current frame，也就是用 SLAM 逻辑上的第二帧来初始化整个 SLAM，得到最开始两帧之间的。计算重投影误差，也就是计算投影点与对应匹配点的像素距离的平方（计算值与真实值），作为卡方值。④ 三维点和两帧图像光心夹角需满足一定条件，夹角越大，视差越大，误差越小，三角化结果越准确；用均值和一阶距绝对值将关键点归一化，归一化可以使特征点分布均匀，增强计算稳定性。将帧 1 特征点左乘单应矩阵，得到帧 2 中对应特征点的计算值。，但由于误差存在，左式并不会恒等于零，于是，我们将它的值作为误差。
2024-01-31 09:44:20
872
原创 05 关键帧 KeyFrame
在 ORB-SLAM2 中，保存所有关键帧构成的最小生成树（优先选择权重大的边作为生成树的边），在回环闭合时只需对最小生成树做 BA 优化就能以最小代价优化所有关键帧和地图点的位姿，相比于优化共视图大大减少了计算量。当一个关键帧被删除时，其父关键帧和所有子关键帧的生成树信息也会受到影响，需要为其所有子关键帧寻找新的父关键帧，如果父关键帧找的不好的话，就会产生回环，导致生成树就断开。新关键帧的父关键帧会被设为其共视程度最高的共视关键帧。进行初始化和维护，基于当前关键帧看到的地图点信息重新生成共视关键帧。
2024-01-23 20:20:21
886
原创 04 帧 Frame
（1）粗匹配：根据特征点描述子距离和金字塔层级判断匹配.粗匹配关系是按行寻找的，对于左目图像中每个特征点，在右目图像对应行上寻找匹配特征点。通过这种预处理，在后面 SLAM 系统的其他部分中不用再区分双目特征点和 RGBD 特征点，它们都会以双目特征点的形式被处理（仅通过判断。双目相机分别提取到左右目特征点后对特征点进行双目匹配，并通过双目视差估计特征点深度。对于单目特征点（单目相机输入的特征点或没有找到右目匹配的左目图像特征点），其。双目/RGBD 相机可以得到特征点的立体信息，包括右目特征点信息（
2024-01-06 10:43:48
1088
原创 03 地图点 MapPoint
删除地图点对关键帧的观测，若地图点对关键帧的观测少于 2，则地图点无法被三角化，就删除该地图点。）时当前关键帧和闭环关键帧上的地图点发生冲突时，会使用闭环关键帧的地图点替换当前关键帧的地图点。将闭环关键帧的共视关键帧组中所有地图点投影到当前关键帧的共视关键帧组中，发生冲突时就会替换。若当前地图点对参考关键帧的观测被删除，则取第一个观测到当前地图点的关键帧做参考关键帧。中所有观测到本地图的关键帧取平均得到，平均观测距离是根据参考关键帧得到的。地图点的平均观测距离是根据其参考关键帧计算的,那么参考关键帧。
2023-12-30 21:40:49
936
原创 01 整体代码运行流程
并且，Tracking 线程产生关键帧的频率和时机是不固定的，因此需要三个线程同时进行，LocalMapping 和 LoopClosing 不断循环查询 Tracking 是否产生了关键帧。但实际编程中，我们将 Tracking 线程视为主线程，LocalMapping 和 LoopClosing 为子线程，Tracking 通过持有两个子线程的指针实现对其控制。即为加锁，锁的有效性仅限于大括号内，也就是说，到大括号外，锁就会自动释放。开头的变量表示为某类的成员变量；
2023-12-17 11:32:12
943
原创 09 光流法实践
相比于特征点法，直接法完全依靠优化来求解相机位姿。（1）直接法优点可以省去计算特征点、描述子的时间；只需要像素梯度即可，不需要特征点，因此可以在缺失特征点的场合下使用；可构件半稠密乃至稠密地图，这是特征点法做不到的。（2）直接法缺点非凸性：优化时容易进入极小，只有在运动很小的时候直接法才能成功，引入金字塔在一定程度上可减小非凸性影响。单个像素没有区分度：选点少时效果较差，一般用 500 个点以上；灰度不变假设是很强的假设，相机会自动调整曝光参数、或者光照变化，都会使得图像整体亮度发生变化。
2023-12-14 15:30:39
1211
原创 08 计算相机运动
一、基础矩阵、单应矩阵用像素坐标求解；本质矩阵、三角测量用归一化坐标求解。p2Hp1p2Hp1二、在 ORB-SLAM 中这几种方法的顺序为：提取两张图像的匹配点，利用对极几何计算HH矩阵和FF矩阵，从而恢复RRtt；用三角测量计算各关键点对应的 3D 坐标；
2023-12-12 21:18:27
1534
原创 07 手写 BA 优化
注意残差块的维度：待优化变量分成两块，相机内外参和空间点坐标；残差为像素理论值与实际值之差，为2维。世界坐标系 --> 相机坐标系（外参）–> 归一化坐标 --> 去畸变 --> 像素坐标系（内参）两个顶点，分别表示相机内外参（9维）、空间点（3维），边为二元边，连接两个顶点。由此得到的估计值与实际观测到的像素坐标作差，得到二维误差项。原理见笔记 《后端 1》
2023-12-06 10:20:49
706
原创 06 g2o 学习
几个需要注意的点：（1）迭代形式为HΔxbHΔxb，也就是求出每次迭代步长Δx\Delta xΔx，三个算法可选：高斯牛顿、LM 和 Dog-Leg；（2）定义顶点：顶点也就是待优化变量，它继承自基础类 BaseVertex<D, T>，其中 D 为 int 类型，表示维度，T 为数据类型。例如g2o::BaseVertex<3, Eigen::Vector3d> // 三维点，Eigen::Vector3d 类型。
2023-12-04 12:32:06
925
原创 05 Ceres
minx12∑iρi∥fixi1xik∥2stlj⩽xj⩽ujxmin21i∑ρi∥fixi1xik∥2s.t.lj⩽xj⩽uj其中ρi∥fixi1xik∥2ρi∥fixi1xik∥2为残差块；f⋅f(\cdot)f⋅为代价函数，也即误差项；ρ⋅ρ⋅。
2023-11-30 21:41:15
888
原创 04 牛顿法、高斯牛顿法及 Cpp 实现
（1）最速下降法Δx∗−JxkΔx∗−Jxk（2）牛顿法HΔx−JHΔx−J（3）高斯牛顿法JxJT⏟HxxΔx−Jxfx⏟gxHxJxJTxΔxgx−Jxfx需要注意的是，最速下降法和牛顿法属于同一类型，他们的JJ和HH都是针对FxFx（也就是整个目标函数）；而高斯牛顿法的JJ是针对fxfx（也就是误差项）。此外，为减小数值，目标函数FxF。
2023-11-27 09:51:21
1181
原创 03 图优化
选择你想要的图里的节点与边的类型，确定它们的参数化形式；往图里加入实际的节点和边；选择初值，开始迭代；每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；求解稀疏线性方程HkΔx=−bk，得到梯度方向；继续用GN或LM进行迭代。如果迭代结束，返回优化值。实际上，g2o能帮我们做好第3-6步，只需要做前两步即可。
2023-11-26 11:20:06
949
原创 02 RANSAC算法 及 Python 实现
RANSAC （Random Sample Consensus，随机抽样一致）算法的基本假设是样本中包含正确数据(inliers即内点，可以被模型描述的数据)，也包含异常数据(outliers 即外点，偏离正常范围很远、无法适应数学模型的数据)，也就是说数据集中含有噪声。我们的目的就是找出使内点最多的模型参数（类似最小二乘法，最小二乘法试图找到满足所有点的参数，而 RANSAC 是为了消除误匹配，尽量找到更多内点，去除外点）。
2023-11-25 16:05:57
1631
原创 01 LM 算法及 Cpp 实现
太小，则说明实际减小的值远小于近似减小的值，即近似效果较差，需缩小近似范围，即增大阻尼系数。太大，则说明实际减小的值大于近似减小的值，则需放大近似范围，即减小。，需要计算的 step 就是这些待定系数的 step。其中，分子是实际下降的值，分母是近似下降的值。法（见《十四讲》），这里用阻尼法进行推导。那么问题转化为，对于每次迭代，求解最优的。接近于 1 ，则近似效果好；优化目标是待定系数，把他们看做。LM 算法有两种推导方式，即。的分母的计算（我的更快？可以看做是对于过大的。求导，并令其等于零，
2023-11-24 10:01:05
1039
原创 《视觉SLAM十四讲》-- 建图
定位：导航：机器人在地图中进行路径规划；避障重建交互：人与地图之间的互动（2）几类地图稀疏地图稠密地图语义地图。
2023-11-20 21:49:02
191
原创 《视觉SLAM十四讲》-- 回环检测
前端提供特征点的提取和轨迹、地图的初值，后端负责数据的优化。但是如果像视觉里程计那样只考虑相邻时间上的关键帧，则会出现累积误差，无法构建全局一致的轨迹和地图。回环检测的关键是有效地检测出相机经过同一个地方这件事。
2023-11-19 12:52:27
154
原创 《视觉SLAM十四讲》-- 后端 2
（2）放开思路，我们完全可以构建一个只有轨迹的图优化，而位姿节点的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。这样的方式，省去了大量特征点优化的计算，只保留了关键帧的轨迹，构建了所谓的。（1）随着时间的流逝，机器人的运动轨迹会越来越长，地图规模也会越来越大，BA 的计算效率就会下降。同时我们发现，经过若干次迭代后，收敛的特征点位置变化很小，发散的外点则已被剔除，因此在后续优化中没有必要再将收敛点考虑进来，而是只把他们当做位姿估计的约束。了解雅克比求导后，剩下的部分就是普通的图优化。
2023-11-17 09:35:44
254
原创 《视觉SLAM十四讲》-- 后端 1（下）
矩阵划分为四个区域，不难看出，左上角为对角块矩阵，且每个对角块元素的维度与相机位姿维度相同，同样的，右下角也是对角块矩阵，且每个对角块元素的维度与路标点维度相同。在前面的 BA 问题中，我们将最小化误差项的二范数平方和作为目标函数，这样虽然直观，但是如果出现误匹配，该误差项会很大，从而将对整体函数产生较大影响，进而影响最终优化结果。对此，我们将原先误差的二范数度量替换成一个增长没那么快的函数，同时保证光滑性质，使得优化结果更加稳健（减小误匹配项的影响），这样的函数称为。对其他相机变量和路标点的偏导都为零。
2023-11-15 10:11:03
361
原创 《视觉SLAM十四讲》-- 后端 1（上）
（1）两种处理方式批量式：使用过去和未来的信息来更新自己的状态的处理方式；渐进式：仅适用过去的甚至仅是前一个时刻的信息来更新自己的状态的处理方式。
2023-11-14 21:23:14
327
原创 《视觉SLAM十四讲》-- 视觉里程计2
根据像素灰度信息同时估计相机运动和点的投影，不要求必须为角点，甚至可以是随机点。灰度不变假设是很强的假设，相机会自动调整曝光参数、或者光照变化，都会使得图像整体亮度发生变化。（1）我们认为图像是随时间变化的，也就是说图像可以看做时间的函数，那么，一个在。，但上式是一个二元一次方程，条件不足，因此还需引入额外的约束。使用特征点时，忽略了特征点以外的所有信息，而丢弃了部分可能有用的信息；假设一个空间点在各个视角下成像的灰度是不变的，我们有许多个空间点。：同一个空间点的像素灰度值，在各个图像中是固定不变的。
2023-11-13 15:18:40
52
原创 《视觉SLAM十四讲》-- 视觉里程计 1（下）
6.4.1 直接线性变换（DLT）已知空间中某点 P=[X,Y,Z,1]T\boldsymbol{P}=[X, Y, Z, 1]^TP=[X,Y,Z,1]T，在图像中的归一化坐标 为 [u,v,1]T[u, v, 1]^T[u,v,1]T，求解相机运动 R\boldsymbol{R}R、t\boldsymbol{t}t。定义增广矩阵 [R∣t][\boldsymbol{R}|\boldsymbol{t}][R∣t] 为一个 3×43\times43×4 矩阵，包含了旋转和平移信息（区别于变换矩阵）。满足
2023-11-12 19:11:59
114
原创 《视觉SLAM十四讲》-- 视觉里程计 1（上）
文章目录06 视觉里程计 16.1 特征点法6.1.1 特征提取6.1.2 特征匹配6.1.3 计算相机运动6.2 2D-2D：对极几何6.2.1 对极约束6.2.2 本质矩阵6.2.3 单应矩阵6.2.4 小结6.3 三角测量6.4 3D-2D: PnP06 视觉里程计 1一个 SLAM 系统分为前端和后端，前端也称为视觉里程计，它根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。包括特征点法和直接法两大类。6.1 特征点法（1）视觉 SLAM 中，将图像特征点作为路标，从而估计出相
2023-11-11 19:52:53
186
原创 《视觉SLAM十四讲》-- 非线性优化
文章目录05 非线性优化5.1 批量状态估计与最大后验估计5.2 最小二乘的引出5.3 非线性最小二乘5.3.1 一阶和二阶梯度法5.3.2 高斯牛顿法5.3.3 列文伯格-马夸尔特方法5.4 实践：曲线拟合问题05 非线性优化5.1 批量状态估计与最大后验估计（1）两种状态估计方法：增量/渐进式（滤波器）：数据是随时间逐渐到来的；批量式：一次给定所有的数据，估计所有的变量。（2）经典的 SLAM 模型由一个运动方程和一个观测方程组成{xk=f(xk−1,uk)+wkzk,j=h(
2023-11-09 16:47:02
62
原创 《视觉SLAM十四讲》-- 相机与图像
但实际相机得到的图像并不是倒像，我们等价地将成像平面对称地放到相机前方，这样就可以把负号去掉，对于径向畸变，离中心距离越远，畸变越严重；针孔模型描述了一束光线通过针孔后，在针孔背面投影成像的关系（类似小孔成像原理）。将世界坐标转换到相机坐标后，再除掉最后一维的数值，这相当于把最后一维作。RGB-D 相机容易受到日光或其他传感器的干扰，因此不能在室外使用。（2）在相机组装过程中，透镜和成像平面无法完全平行，会产生。将三维空间上的点投影到归一化平面，得到坐标。其中，负号表示成的像是倒立的。
2023-11-08 15:38:32
358
原创 《视觉SLAM十四讲》-- 李群与李代数
（1）每个李群都有与之对应的李代数。李代数描述了李群单位元附近的正切空间性质。（2）李代数由一个集合V\mathbb{V}V，一个数域F\mathbb{F}F，和一个二元运算 [ , ] （称为李括号）组成。满足以下性质：封闭性双线性自反性雅克比等价例如，三维向量R3R3上定义的叉乘×\times×是一种李括号，因此gR3R3×gR3R3×构成了李代数。
2023-11-07 17:14:47
128
原创 《视觉SLAM十四讲》-- 三维空间的刚体运动
（1）2D 情况下，可用单位复数表达旋转，在复数平面中，结合欧拉公式，如下图，向量zz旋转 90° ，相当于乘上iii。（2）类似的，在三维空间中，采用四元数描述旋转。四元数有一个实部，三个虚部（分别对应 x、y、z轴）。qq0q1iq2jq3k(2-14)\tag{2-14}qq0q1iq2jq3k2-14写成向量形式qsvsq0∈Rvq1q2q3T∈R3(2-15)
2023-11-05 18:52:36
160
原创 《视觉SLAM十四讲》-- 概述与预备知识
（1）SLAM 是 Simultaneous Localization and Mapping 的缩写，即同时定位与地图构建。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型。（2）两大基本问题：定位：我在什么地方？建图：周围环境是什么样的？（3）准确的定位需要精确的地图，精确的地图来自准确的定位。（4）传感器内质的（放置在机器人身上，用于感受机器人本体信息）：IMU（惯性测量单元）、激光、相机外质的（安装于环境中，监测机器人的运动状态）：二维码、GPS导轨。
2023-11-04 21:02:21
784
原创 《视觉SLAM十四讲》公式推导（三）
文章目录CH3-8 证明旋转后的四元数虚部为零，实部为罗德里格斯公式结果CH4 李群与李代数CH4-1 SO(3) 上的指数映射CH4-2 SE(3) 上的指数映射CH4-3 李代数求导对极几何：本质矩阵奇异值分解矩阵内积和迹CH3-8 证明旋转后的四元数虚部为零，实部为罗德里格斯公式结果前面已经推导过v′=pvp∗=pvp−1v'=pvp^*=pvp^{-1}v′=pvp∗=pvp−1其中，v=[0,v⃗]v=[0,\vec{v}]v=[0,v]，p=[cosθ2,sinθ2u⃗]p=[\
2023-10-26 21:44:26
123
原创 《视觉SLAM十四讲》公式推导（二）
CH3-5 四元数表示旋转三维空间中任意点均可用一个纯虚四元数表示即p=[0,v]T\boldsymbol{p}=[0,\boldsymbol{v}]^Tp=[0,v]T，经一个单位四元数q\boldsymbol{q}q的旋转后，得到p′\boldsymbol{p'}p′，则p′=qpq−1(3-5-1)\boldsymbol{p'}=\boldsymbol{q}\boldsymbol{p}\boldsymbol{q^{-1}}\tag{3-5-1}p′=qpq−1(3-5-1)最终p′\bol
2023-10-22 19:14:07
198
原创 《视觉SLAM十四讲》公式推导（一）
文章目录CH3 三维空间刚体运动CH3-1 旋转矩阵的推导CH3-2 旋转矩阵是正交矩阵的证明CH3-3 变换矩阵的逆的推导CH3-4 罗德里格斯公式推导CH3 三维空间刚体运动CH3-1 旋转矩阵的推导（1）二维空间中的旋转矩阵 易得{x′=∣OP′∣cos(θ+β)=∣OP∣(cosθ⋅cosβ−sinθ⋅sinβ)=xcosβ−ysinβy′=∣OP′∣sin(θ+β)=∣OP∣(sinθ⋅cosβ+cosθ⋅sinβ)=ycosβ+xsinβ\left\{\begin{ma
2023-10-15 14:30:10
119
原创 C++核心编程
*作用：**函数名可以相同，提高复用性同一个作用域下函数名称相同函数参数类型不同或者个数不同或者顺序不同函数的返回值不可以作为函数重载的条件//函数重载需要函数都在同一个作用域下cout << "func 的调用！" << endl;cout << "func (int a) 的调用！" << endl;cout << "func (double a)的调用！" << endl;cout << "func (int a ,double b) 的调用！" << endl;
2023-10-12 09:26:02
89
空空如也
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人