R3live笔记：（图像处理）视觉-惯性里程计VIO部分

R3LIVE相关参考：
R3LIVE(升级R2LIVE)：编译与运行

https://blog.csdn.net/handily_1/article/details/122271243?spm=1001.2014.3001.5502
R3live：整体分析
https://blog.csdn.net/handily_1/article/details/122360134?spm=1001.2014.3001.5502
运行代码报错：Failed to load module “canberra-gtk-module“
https://blog.csdn.net/handily_1/article/details/122359275
R3live笔记：（图像处理）视觉-惯性里程计VIO部分
https://blog.csdn.net/handily_1/article/details/122377514

r3live VIO

r3live的VIO子系统绘制了全局地图的纹理，通过最小化光度误差来估计系统的状态，即：
将全局地图中一定数量的点（即跟踪的点）投影到当前图像中，
然后最小化这些点的光度误差，在ESIKF框架内迭代估计系统状态。
为了提高效率，跟踪的地图点是稀疏的，这通常需要构建输入图像的金字塔。
然而，金字塔对平移或旋转都不是不变的，也需要被估计。
r3live框架中，利用单个地图点的颜色来计算光度误差,
因为颜色是地图点的固有属性，对相机的平移和旋转都是不变的。
根据r3live框图的VIO部分：

首先利用一个帧对帧（frame-to-frame）的光流来跟踪地图点，
通过最小化跟踪地图点的Perspective-n-Point（PnP）投影误差，
然后，通过最小化跟踪点之间的帧到地图（frame-to-map的光度误差来进一步细化系统的状态估计，
有了收敛的状态估计和原始输入图像，执行纹理渲染来更新全局地图中点的颜色。

1. 帧对帧（frame-to-frame）

假设在最后一个图像帧中Ik-1跟踪到了 m个地图点 ，它们在 Ik-1上的投影为 [公式] ，利用Lucas-Kanade光流来找出它们在当前图像帧 Ik中的位置，标记为 。然后，通过ESIKF滤波器，我们计算和优化它们的投影误差来估计状态。

1）PnP投影误差 Perspective-n-Point projection error

投影误差计算如下：

其中：

公式(5)中，第一项是针孔相机模型的投影函数，第二项为在线时间校正因子（Online-temporal correction factor）。
误差公式(4)中的测量噪声包含两个源：
一个是像素跟踪误差

另一个是地图点位置误差

2）帧到帧视觉里程计ESIKF更新 Frame-to-frame VIO ESIKF update

公式(8)构成了xk的观测分布，它可以与IMU传播的先验分布相结合，以得到最大后验MAP估计：

2.帧到地图 视觉惯性里程计 Frame-to-map Visual-Inertial odometry

3.渲染全局地图的纹理 Render the texture of global map

在帧到地图的VIO更新后，我们得到了当前图像的精确位姿，然后我们执行渲染功能来更新地图点的颜色。

4.视觉惯性里程计子系统的跟踪点更新 Update of the tracking points of VIO subsystem

参考：【论文阅读】R3LIVE：A Robust RealTime RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation

---

代码

1. IMAGE_topic

订阅：

        sub_img = m_ros_node_handle.subscribe(IMAGE_topic.c_str(), 1000000, &R3LIVE::image_callback, this, ros::TransportHints