zl_vslam-优快云博客

原创 SLAM中的非线性优-3D图优化之相对位姿Between Factor右扰动实战（九）

本节主要是对代码进行说明，前几节对左右扰动的求解是以SE3的形式表示的，实际上位姿的表示还有SO(3)+t以及四元数，轴角，欧拉角等的表示，后续章节将继续进行分析t=P9T8完整代码依然在码云上https://gitee.com/zl_vslam/slam_optimizer/tree/master/3d_optimize/modules/ch6。

2025-12-19 00:00:00 328

原创 SLAM中的非线性优-3D图优化之相对位姿g2o::EdgeSE3Expmap（十）

本文总结了g2o中相对位姿约束的实现方法，参考了"白巧克力亦唯心"的博客内容。重点分析了EdgeSE3Expmap边的误差计算函数computeError()和线性化函数linearizeOplus()的实现细节，包括位姿变换、误差计算和雅可比矩阵推导。作者指出代码中存在测量标记Tij应为Tji的错误，并对比了ORB-SLAM中的实现方式。文章声明内容为转载整理，旨在作为后端优化系列的技术参考和工具查询资料。

2025-12-13 20:28:45 211

原创 SLAM中的非线性优-3D图优化之相对位姿Between Factor右扰动（八）

前两节已经详细讲解了左扰动的推导方式，本节来看下右扰动的推导，另外右扰动的方式，也被GTSAM中Between Factor所采用，待理论推导后，再来展示相关代码部分，接下来继续沿用之前的图来说明问题。本节详细推导了右扰动下的残差跟雅可比，并展示了gtsam库中的实现，与推导一摸一样，足以证明理论的正确性，下一节将详细对比左右扰动下雅可比代码实现。注意：在（六）中介绍过，右雅可比可近似为单位阵，因此i时刻雅可比可简化为如下。所谓右扰动，即扰动项放在待求量的右边，上节基础部分已展示，这里不再重复。

2025-12-12 00:00:00 431

原创 SLAM中的非线性优-3D图优化之相对位姿Between Factor左扰动（七）

本节以伴随跟BCH公式为基础，直接推导了左乘扰动后的雅可比矩阵，并且最终结论与上节完全一致，相对上节的推导，单个求扰动后的雅可比更加简洁，更加容易理解，不过实现路径虽有不同，最终结论确是一致的，因此可以得出一个结论，再求解雅可比矩阵的时候，选择合适的扰动方式，往往能事半功倍。

2025-12-05 00:00:00 297

原创 SLAM中的非线性优-3D图优化之相对位姿Between Factor左扰动（六）

本文讲解SLAM中相对位姿约束(BetweenFactor)的数学推导，重点分析基于李群李代数的残差和雅可比求解方法。该约束广泛应用于位姿图中的相邻帧约束和回环检测。文章从李代数表示出发，通过引入左扰动模型，推导两顶点(Ti,Tj)的雅可比矩阵，并利用BCH近似和伴随性质将扰动项移至等式两侧。证明过程涉及SO(3)旋转性质和平移部分的数学推导，为后续自动求导和数值求导提供理论基础。

2025-11-29 20:05:49 1025

原创 SLAM中的非线性优-3D图优化之四元数在Opencv-PNP中的应用（五）

本文补充介绍了四元数表示位姿的方法，重点讨论了如何利用视觉3D到2D重投影误差构建残差函数并计算雅可比矩阵。相比轴角和李群李代数参数化，四元数提供了一种有效的位姿表示方式，可用于优化相机姿态估计中的重投影误差。文章内容涉及位姿参数化的不同方法及其在视觉SLAM中的应用，特别是基于重投影误差的优化计算过程。

2025-11-21 00:00:00 387

原创 SLAM中的非线性优-3D图优化之李群李代数在Opencv-PNP中的应用（四）

这个推导结果与视觉SLAM经典教材（如《视觉SLAM十四讲》）中的结果是一致的。在实际的优化库（如g2o, Ceres）中，正是使用这些雅可比矩阵来指导优化方向，从而高效地求解最优的相机位姿。之前章节总结了轴角参数化，简单介绍了下四元数参数化，本节补充下李群-李代数表示的位姿，如何利用视觉3D到2D重投影误差的残差及雅可比。

2025-11-16 11:40:59 298

原创 SLAM中的非线性优-3D图优化之轴角在Opencv-PNP中的应用（三）

本文介绍了基于视觉SLAM的PNP求解优化方法。通过特征提取匹配79个点对，利用OpenCV接口对比了不同求解方式（带LM优化和不带优化）的结果差异。重点展示了使用Ceres框架实现解析雅可比矩阵和残差的优化过程，包含初始化位姿估计、构建优化问题、配置选项及结果验证。实验表明该方法的优化结果与OpenCV求解基本一致，验证了算法的正确性。文中还提供了完整代码实现，但尚未加入信息矩阵和鲁棒核函数，可作为基础验证方案。该方法为后续相对位姿残差及雅可比推导奠定了基础。

2025-11-15 21:54:26 484

原创 SLAM中的非线性优-3D图优化之轴角在Opencv-PNP中的应用（二）

本节是上节内容的延续，主要展示如何利用轴角来跟四元数来求解旋转部分的雅可比跟残差，其他不变的地方不重复列出来了，接下来请看精彩内容一. 罗德里格斯参数化二. 计算各个偏导数(1) 由于 θ = |w| = √(w_x² + w_y² + w_z²)（2）∂uj / ∂wj 其中δij是克罗内克δ函数（3）最终通过整合得出结果（4）归一化坐标导数(5) 特殊情况的验证当theta趋近于0时如果只绕单一轴旋转，如z轴这与直接对旋转矩阵求导的结果一致。三. 四元数参数化(1) 旋转用单位四元

2025-11-14 00:00:00 803

原创 SLAM中的非线性优-3D图优化之轴角在Opencv-PNP中的应用（一）

本文主要讲解了3D图优化中基于PNP问题的轴角表示方法。通过源码分析展示了相机位姿估计过程：首先将3D点从世界坐标系转换到相机坐标系，然后进行归一化处理并计算径向和切向畸变，最终得到重投影误差。重点推导了误差函数对旋转和平移的雅可比矩阵，包括畸变矫正后的坐标对旋转向量和平移向量的偏导数计算。该优化方法通过迭代求解位姿参数，适用于计算机视觉中的相机位姿估计问题。

2025-11-08 18:10:04 255

原创 SLAM中的非线性优化-2D图优化之视觉惯性VIO(二十-终结篇)

本文介绍了2D视觉惯性里程计（VIO）的开发过程，重点讲解了一个基于滑动窗口的优化算法实现。作者参考VINS-Mono算法，结合轮速里程计等地面机器人特有信息进行简化改进。系统通过200帧数据进行IMU偏置估计和初始化，随后利用IMU预积分、轮速计数据构建约束，并采用10帧滑动窗口进行特征匹配和三角化。文章提供了核心代码片段，展示了滑动窗口初始化和优化处理流程。作者指出该工程开发耗时约10天，后续将继续优化并添加回环检测功能。作为2D SLAM系列的最后一讲，接下来将转向3D SLAM的相关开发工作。

2025-10-06 17:31:50 974

原创 SLAM中的非线性优化-2D图优化之零空间实战（十六）

本文介绍了SLAM后端优化的实战技巧，包括舒尔补、先验Factor、鲁棒核函数、FEJ和BA优化等滑动窗口方法。通过构建2D轮式里程计预积分和10帧滑动窗口，实现了边缘化处理最老帧的功能。代码展示了如何初始化状态、处理里程计数据、进行预积分预测，并添加位姿图边进行优化。优化器通过迭代更新状态，最终输出优化后的位姿结果。完整代码可在Gitee仓库获取，相关原理可参考作者优快云博客系列《SLAM中非线性优化的从古至今》。

2025-08-02 22:33:17 438

原创 SLAM中的非线性优化-2D图优化之激光SLAM基于优化的前端匹配实战（十九）

本文介绍了基于Ceres的2D激光扫描匹配优化方法。通过构建OccupiedGridCostFunction2D类实现概率栅格地图与点云数据的匹配，采用双线性插值计算概率值，并推导了概率对位姿的雅可比矩阵。重点展示了如何通过NearestSearch类构建似然域来平滑概率值，解决直接获取邻域概率值差异过大的问题。文中详细解析了核心代码实现，包括残差计算、雅可比矩阵推导等关键步骤，并验证了该方法在实际项目中的有效性。最后预告下一讲将介绍2D图优化的终极应用——2D版视觉惯性SLAM系统。

2025-07-20 16:45:01 255

原创 SLAM中的非线性优化-2D图优化之激光SLAM基于优化的前端匹配（十八）

本文介绍了基于优化的前端匹配算法原理，重点分析了其与Cartographer算法在插值方法上的差异（双线性插值 vs 三次样条插值）。通过链式法则和双线性插值技术，该算法将离散的栅格空间转换为连续可导函数，从而简化了位姿概率导数的求解过程。文章指出该方法计算步骤简单高效，并预告后续将进入实战应用环节的讲解。

2025-07-19 21:30:01 322

原创 SLAM中的非线性优化-2D图优化之激光SLAM cartographer前端匹配（十七）

本文介绍了Cartographer中2D激光SLAM前端ceres_scan_mather_2d的实现原理。该方案通过Ceres求解器构建三个残差项进行位姿优化：1) 地图概率残差（使用三次样条插值连接离散栅格与连续空间）；2) 平移误差（优化前后位移差）；3) 旋转误差（优化前后角度差）。作者展示了基于NCLT数据集的实验结果，黄色为里程计轨迹，白色为激光匹配轨迹，可见随时间推移两者偏差逐渐增大。文中指出该实现是非ROS版本，使用Pangolin进行可视化，并预告下一讲将深入讲解概率地图对位姿求导的理论细

2025-07-13 21:13:13 952

原创 SLAM中的非线性优化-2D图优化之零空间（十五）

这节在进行讲解SLAM中一个重要概念，零空间，讲它有啥用呢？因为SLAM中零空间的存在，才需要FEJ或固定约束存在，本节内容不属于2D图优化独有，先看看什么是零空间概念；是一个核心概念，它深刻揭示了SLAM问题的内在结构和优化过程中的关键特性。

2025-06-28 22:04:04 1026

原创 SLAM中的非线性优化-2D图优化之鲁棒核函数（十四）

本节讲解了鲁棒核函数以及LM算法的流程，并且仿真代码包含了上一节边缘化，舒尔补以及FEJ的实现，至此基础知识基本都得以实现，下一讲进入2D激光的优化实现，请继续关注。

2025-06-20 07:00:00 808

原创 SLAM中的非线性优化-2D图优化之边缘化（舒尔补）/先验Factor/FEJ实战（十三）

本文介绍了边缘化过程的实现与应用，通过模拟匀速直线运动的小车轨迹进行滑动窗口优化。开发者创建了一个窗口大小为5的优化器，连续添加10个位姿顶点和对应的里程计约束边，其中每5步添加回环约束以增强优化效果。代码展示了完整的边缘化流程，包括舒尔补、先验因子和FEJ（First Estimate Jacobians）方法的使用。优化的位姿结果实时输出，最终展示全部10个位姿的优化坐标。该实现代码量精简，便于理解边缘化理论的实际应用，后续还将推出真实数据的实验案例。完整代码已开源在码云平台。

2025-06-13 08:00:00 698

原创 SLAM中的非线性优化-2D图优化之边缘化（舒尔补）/先验Factor/FEJ（十二）

本文探讨了视觉惯性里程计中预积分和图优化存在的多解问题。当线性化点未固定时，系统会因状态更新而发散，类似于量子纠缠现象。为解决这一风险，需要引入先验信息来固定线性化点，确保解的唯一性。文中提出使用舒尔补操作和首次估计雅可比（FEJ）方法来规定先验信息，从而约束优化过程中状态变量的移动范围，避免系统发散。这种方法类似于限制刚体移动范围，使其仅在初始位置附近变化，最终获得稳定可靠的优化结果。

2025-06-08 13:22:48 589

原创 SLAM中的非线性优化-2D图优化之IMU预积分实战（十一）

本文介绍了三轴IMU预积分的实战应用，作者指出由于时间限制，当前代码还存在问题，基于仿真的数据仅前几帧能够收敛。该内容主要起教学示范作用，后续将持续更新完善。作者邀请读者在评论区指出问题，并对关注博客的读者表示感谢。最后简要提及将展示代码实现环节。文章简短说明了项目现状、目的和互动方式。

2025-06-01 20:57:07 379 5

原创 SLAM中的非线性优化-2D图优化之三轴IMU预积分（十）

上节讲完了预积分相关的传播，噪声传播，残差雅可比等的推导，事实上，优化问题的目标是估计最优状态，而预积分实际上是一种观测，但若只有预积分，一种观测，则其等价于直接积分，若要完成优化，还需要融合其它观测，本节继续考虑如下位姿图。如图所示，绿色表示两帧间的观测，蓝色表示直接当前观测，我们依然考虑两帧间的边由轮速里程计跟IMU预积分组成，当前观测由gps观测组成，轮速里程计我们采用。(1-3)位置对加速度偏置求导, 带入上一讲公式（1）并省略掉无关项。(1-0)先位置对位置求导。(1)先位置对位置求导；

2025-05-17 18:46:59 778

原创 SLAM中的非线性优化-2D图优化之三轴IMU预积分（九）

本节主要讲解了预积分在SLAM（同步定位与地图构建）中的应用，特别是如何利用SO(2)李代数进行优化。首先介绍了SO(2)的定义及其李代数so(2)，包括指数映射和对数映射，这些工具用于描述二维旋转的局部扰动或增量。接着，讨论了在SLAM中如何利用这些数学工具进行优化问题、扰动模型和插值。此外，还详细介绍了预积分残差和雅可比矩阵的推导，这些在IMU应用中用于优化状态变量，如旋转、平移、线速度和IMU零偏。最后，总结了预积分过程的关键步骤，包括计算预积分观测量、噪声协方差矩阵和雅可比矩阵

2025-05-10 21:14:46 896

原创 SLAM中的非线性优化-2D图优化之三轴IMU预积分（八）

由直接积分可以看到，优化时候i时刻偏置因为在状态中，也会随之改变，如果零偏改变，预积分就要重新计算，但这样就与直接积分没有多大区别了，因此这里做写技巧性的改变，假定与积分观测是随着零偏线性变化的。仿照（2）式，并将（13）中更新后的角度带入可得,目的式要将上式变为（3）（4）（5）式后加减一系列跟delta相关的量，因此这里旋转部分需要分离delta_bwi。上式除了旋转部分与六轴IMU预积分有区别外，速度，位置均保持一致，主要看这两项对应雅可比怎么求解。先上结论，由上节中的公式（3）(4) (5)可得。

2025-05-03 21:51:06 1618

原创 SLAM中的非线性优化-2D图优化之三轴IMU预积分（七）

本讲开始正式三轴imu预积分推导，由于imu预积分较为复杂，因此同样分为多个章节讲解，预积分模型按大方向分，主要为假设噪声传播阶段零偏不更新跟优化过程中零偏更新的处理过程；上式是两帧间的递推公式，也叫直接积分，仅单帧间的变换，实际上两个关键帧间起码会大于等于两帧，因此推导下通用的公式，时间用i和j时刻表示，为了把直接积分变为预积分，只需再上式两边同时乘以i时刻旋转的逆即可，记为。速度项也同样的道理，就不打公式了，唯独旋转部分不需要这么复杂，只需SE2累加即可。

2025-04-20 10:14:01 747

原创 SLAM中的非线性优化-2D图优化之三轴IMU预积分前传（六）

IMU的观测也会随着更新，而更新方法就是从i时刻积分导j时刻，对低频imu倒影响不大，实际上imu频率可达100hz，SLAM中的i和j时刻一般代表两个关键帧数据，而两帧间可能会存在上百帧imu数据，如果直接积分，则算法非常耗时，此处后续在3D图优化中，会有真实数据，可以直观感受到，而预积分则采取的是一阶近似，即i时刻更新，则j时刻直接更新掉，而不需要一帧一帧累计。由上式可知，状态是由角度表示的，而旋转矩阵正好对应着各自的角度，因此最终推导的误差项表示如下，懒得打公式了，拷贝一段，i=1， j=2.

2025-04-12 19:16:56 956

zl_vslam的博客