qq_49561752的博客

​ 误差主要来源于平移积分过程中认为角度恒定。直线运动时不受影响，以及 Δt 很小时误差影响也较小。代码实现：下面旋转矩阵展开就是上面的形式。手推即可验证，有时间补充。​ 平移积分过程中认为角度为运动前后的均值，误差比欧拉积分小一点。积分、群空间闭式积分。在我们代码中，分别实现了前面两种递归公式！作为观测值，而且观测方程刚好是线性的！​ 常见的轮速计积分的方式有三种：欧拉积分、二阶。对于imu和wheel的处理(有时间再补代码)① 简单处理：找到wheel最近的imu数据。② 线性插值处理，更精确！

边缘化，就是说如何删除滑窗里的状态，这里使用最简单的策略，把最老的一帧去掉，删掉协方差矩阵中对应块，去掉的这帧里的所有特征点都被拿来做更新。，等同IMU测量的角速度。左右轮速的速度我们是可以计算得到的，但是半径R一开始肯定未知，所以利用下面公式计算，其中。这里先引出论文中的公式，貌似顺序写反了，δp在前，δθ在后，更多详细推导可以参考论文。2 状态增广：当新的frame到来，利用里程计位姿，通过外参预测相机位姿，更新。来讲，前面定义世界系与初始轮速坐标系重合，轮速只有绕z轴的旋转，所以坐标系。