r3live---学习

整个系统分为两个子系统，LIO系统和VIO系统如下

LIO用于重建全局地图的几何结构，VIO用于渲染地图的纹理色彩。
关于数学符号–
errorstate iterated Kalman filterESIKF 误差状态迭代卡尔曼滤波器
**罗德里格斯变换–Rodrigues’ transformation？**用于旋转矩阵和旋转向量–

状态向量

文章所说的全状态是什么意思呢？就是什么状态都包含。相比于fast-lio系列，状态向量又多了相机到imu系的外参，相机时间与IMU时间间隔，以及相机的内参。

1、IMU在全局系下的姿态和位置，速度，陀螺仪偏差、加速度计偏差、重力。
2、相机到IMU的姿态和位置（外参）
3、相机到IMU的时间偏移量（激光雷达已和IMU同步了）
4、最后一个是相机的内参向量 （焦距，偏移）

地图表示

体素是地图的最小单元，0.1m0.1m0.1m
体素的激活状态（某个时间内有点加入）
跟点有关的信息有三种：1、点的格式（全局三维坐标，RGB）；2、坐标和rgb的协方差（估计误差）；3、点创建和渲染时的时间戳

激光雷达惯性里程计

通过IMU的反向传播补偿连续运动中激光雷达的运动畸变。
通过ESIKF最小化点到平面的残差，估计系统状态。
之后将点加入全局地图并未VIO系统提供深度。
进一步阅读fast-lio，fast-lio2,r2live

视觉惯性里程计

我们的VIO子系统渲染全局地图的纹理，通过最小化光度误差来估计系统状态。
最小化光度误差估计状态–将全局地图中一定数量的点投影到图像中，ESIKF迭代估计这些点的光度误差（单帧图像RGB）。
构建图像金字塔（不是旋转不变的–也是需要被估计的）-稀疏化点云以提高效率。–这个咋估计

科普小课堂—“图像金字塔通常不具有旋转平移不变性。这是因为在构建图像金字塔时，通常会对原始图像进行不同尺度的缩放，而这个过程通常是基于固定的平移和旋转操作进行的。因此，当旋转或平移图像时，不同尺度的金字塔图像可能会相互错位，从而导致图像特征的丢失或变形。为了解决这个问题，通常需要对金字塔图像进行一些特殊的处理，比如在金字塔的不同层级中使用旋转和平移不变的特征描述符等。”

帧到帧（光流法最小化PNP的投影误差），帧到图（光度误差）—二步流程确保稳定和快速。
有了收敛的状态和原始图像，就可以实现纹理渲染更新地图中点的颜色

帧到帧的VIO

光流法确定跟踪点

文中跟踪的含义是指能够定位到投影在上一帧图像上的点在这一阵图像上的投影位置
上一帧图像
跟踪了m个地图点这些点在上一帧图像上的投影是
Lucas ́Kanade optical flow–找出它们在当前帧图像中的位置
跟踪的地图点在当前帧图像上对应的投影位置是（也就是说，图像跟踪的点是LIO已经建好的点云地图的）
用ESIKF对投影误差进行优化。
步骤已经很清晰了，1、找到位置，2、投影误差，3、ESIKF优化。

pnp计算跟踪点的投影误差

第一步找到位置可以通过Lucas ́Kanade 的光流法实现（图像位置，空间位置）

第二步：采用PNP计算投影误差，如下
以地图点Ps为例子，
点的相机空间坐标：k是当前图像序号

第一项是激光雷达点的旋转：从全局空间旋转到当前图像的相机空间
第二项是当前图像到IMU的平移旋转到相机空间中
第三项是当前IMU在全局空间的位置旋转到相机空间

变换得到了C下的点坐标后 进行投影误差的计算。（x 下尖代表ESIKF迭代中的当前状态估计）

第一项是图像上的投影点第二项是通过变换关系得到的投影点。


第二项展开后的第二项

分母项时两帧图像之间的时间间隔，分子项是相机和IMU之间的时间偏移。在线时间校正因子–《Online temporal calibration for monocular visualinertial systems,》

中的测量噪声主要由两个来源：中的像素跟踪误差；和地图点的位置误差，也就是说这两项是包含噪声的。
–
对测量误差在
对测量误差函数进行一阶泰勒展开–线性化。

H是雅各比矩阵，a的协方差是平面位置的协方差加上空间位置协方差？

ESIKF 更新

加上前面的状态误差（省略了），测量（投影）误差

第一项的详细描述–阅读**《R2live: A robust, real-time, lidar-inertial-visual tightly-coupled state estimator and mapping,》** E部分
上述是单个点的投影误差，将所有点的信息综合一下

阅读该文章去了解迭代卡尔曼滤波的增益**《Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,》**

状态更新—这种迭代卡尔曼滤波使用的是高斯牛顿法迭代更新的

帧到地图的VIO

光度更新

执行完帧到帧的VIO更新后，已经有一个比较好的估计状态了
那然后为什么还要执行帧到地图的光度误差？

帧到地图的光度误差–第一项是全局地图中点的颜色，第二项是当前图像观测到的颜色。

光度的测量误差函数

如何获得第二项以及其协方差？

利用帧到帧VIO更新得到的状态和点在相机坐标系下的三维坐标，利用小孔成像模型估计地图点在图像中的位置，有了位置自然就知道颜色了。

然后线性内插邻近像素的RGB颜色。

考虑到上面两项都是带有噪声的，即（为啥第一项有两个噪声？上次渲染后的估计误差，第二项的环境错觉是什么东西？？？脉冲随机游走过程噪声？？）“脉冲随机游走过程噪声是指一种随机过程的噪声模型，该模型描述的是一个物理系统中由于各种未知因素的影响所引起的不确定性。这种噪声模型假设在任意时刻，系统的状态会因为一个随机的脉冲而发生突变，并且这个脉冲的大小和方向是随机的，因此它可以被看作是一个随机游走过程。脉冲随机游走过程噪声在许多科学和工程领域中被广泛应用，如控制理论、信号处理、通信系统、金融工程等。”

同投影误差，对光度误差进行一阶泰勒展开（已知地图中点的颜色，地图点的全局坐标，状态）

ESIKF更新

和帧到帧的投影误差一样，帧到地图的光度误差作为测量误差，与状态误差一起构成MAP
目标函数

所有点

和帧到帧的ESIKF一样

全局地图渲染

经过帧到地图的VIO更新后，现在相机的位姿是精确的

当前帧图像观测到的颜色是
地图中存在的颜色是
通过贝叶斯更新进行融合–关于这个推导，我对贝叶斯不够理解，最后还是跟林博联系了一下，其实就是贝叶斯条件概率。如下：

但是要展开整理一下，最后的形式我是参考一维正态分布贝叶斯的结果的。如下


对应到本文，最后整理得到的结果就是

更新viO中追踪的点

移除
中误差（投影、光流）较大的点，
投影点云中的每个点到当前帧
将点添加到