SLAM代码之svo代码分析

最新推荐文章于 2023-01-27 17:14:48 发布

wendox

最新推荐文章于 2023-01-27 17:14:48 发布

阅读量9.6k

点赞数 6

分类专栏： UAV SLAM

本文链接：https://blog.csdn.net/wendox/article/details/52536706

版权

本文深入剖析SVO的核心代码，包括稀疏图像对齐、特征对齐、位姿和结构优化以及地图管理。通过直接法优化图像块重投影残差获取位姿，接着详细讲解了`optimizeGaussNewton`和`Relaxation Through Feature Alignment`过程。文章还介绍了SVO中frame、point和feature成员，以及地图成员函数。

摘要由CSDN通过智能技术生成

在上文中我们从ROS的节点出发，一步步介绍了SVO ros节点的运行流程，下面我们将深入介绍SVO大的核心代码。SVO的核心主要分为3方面内容
- spase image alignment
- Feature alignment
- pose and Structure optimization
- map
首先给出总体的结构图。
这里写图片描述

首先参考论文中的内容，进行分析。使用直接法最小化图像块重投影残差来获取位姿。如图所示：其中红色的 Tk,k−1 为位姿，即优化变量。
这里写图片描述
直接法具体过程如下：
1. 准备工作。假设相邻帧之间的位姿 Tk,k−1 已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。
2. 重投影。知道 Ik−1 中的某个特征在图像平面的位置 (u,v) ，以及它的深度 d ，能够将该特征投影到三维空间 pk−1 ，该三维空间的坐标系是定义在 Ik−1 摄像机坐标系的。所以，我们要将它投影到当前帧 Ik 中，需要位姿转换 Tk,k−1 ，得到该点在当前帧坐标系中的三维坐标 pk 。最后通过摄像机内参数，投影到 Ik 的图像平面 (u′,v′) ，完成重投影。
3. 迭代优化更新位姿。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿 Tk,k−1 。
将上述过程公式化如下：通过不断优化位姿 Tk,k−1 最小化残差损失函数。