r2live学习随笔

在fast-lio2的基础上融入了相机。

这个误差状态迭代卡尔曼滤波器应该就是fast-lio系列的方法，取了一个更加贴切的名字。

关于流和运动模型的就不再多做介绍了。看一下数据接收顺序吧

和
中的两个传播从融合最近的LiDAR/相机测量（例如，第k次测量，见第IV-I节）之后的最佳状态和协方差估计开始，并重复直到接收下一个LiDAR/摄像机测量（例如第k+1次测量）。

前向传播模型可以参考fast-lio fast-lio2。

先验分布

在k+1激光雷达/相机的测量处，传播的状态估计和协方差就变成了

在融合k+1时刻的测量值之前，为状态xk+1提供了先验分布

迭代更新的初始化（注意–IKF的迭代是依靠MAP的–高斯牛顿方法）

什么叫误差状态捏

他这里符号又变了，但是含义还是我在FAST-LIO中写得一样。

观测误差

上面是状态误差

激光雷达的观测

符号更具体了
：从这里就表明了没有进行边点残差的计算。

视觉观测

如果k+1时刻是相机图像，1、提取FAST 角特征点，对应于三维空间的地标
。2、KLT光流法跟踪滑动窗口中关键帧看到的特征点集中的特征。
如果特征点丢失了或者没有被跟踪到，就用优化后的相机位姿再三角化新的特征点。
用k+1时刻视觉地标与跟踪的特征点之间的重投影误差更新当前状态估计。
因此，特征点ps的测量残差就计算为。先把空间点姿态转到相机坐标系下，再将位置给变换到相机坐标系下。再根据小孔成像模型变换到二维图像上（测量残差带状态估计是因为，测量值的变换都是基于状态估计的）

需要知道进行投影的这两个点本身也都是带有噪声的，如下

然后进行一阶泰勒展开（老规矩，先线性化了再说）

误差状态迭代卡尔曼滤波的更新

11 14 19 都融合到最大后验估计中–这个最小化需要高斯牛顿法进行迭代

迭代卡尔曼增益需要这三个东西，H(测量雅可比) , P（预测误差状态协方差）,R（测量噪声协方差）–可以类比EKF

最终，根据论文《The Iterated Kalman Filter Update as a Gauss-Newton Method》可以得到

状态更新公式：（推导都在之前fast的博客里）

因子图优化

歪日，还有个因子图优化
之前提过未跟踪的视觉地标：对新添加的关键帧中未跟踪的视觉地标进行三角测量，以创建新的视觉地标。

这种三角测量的精度很低的。

利用因子图优化来优化图像关键帧的滑动窗口内的相机姿态和视觉地标。
如下：
Marginalization 边缘化
这个因子IMU预积分的约束也包括在内，以将LiDAR因子与相机因子联系起来。
图优化与VINS-mono类似

IMU预积分没提过这事啊–框架里也没有啊