【LOAM系列】六：Lio-Livox代码阅读笔记

LIO-Livox

览沃官方

视觉传感器对光照敏感、雷达(旋转式)体积大成本高,不适用于小型机器人
• 固态雷达低成本、重量轻
• 固态雷达带来的新挑战:
1)特征少的场景易退化,小 FOV 雷达更明显
2)数千个特征点 +IMU 数据导致计算量太大
3)激光点采样时间不同,运动失真

1、编译与运行

1.1 编译相关问题

编译时CMakeLists.txt中c++设置为c++14，先在功能包下/src下编译livox_ros_driver2，然后进入工作空间~/YouIbot/Livox/lio_livox_ws下catkin_make 即可

运行时候出现的问题

修改代码

kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);

    if(!laserCloudNonFeatureFromLocal->empty())
    kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);

1.2 启动LIO-Livox mid360

cd ~/YouIbot/Livox/livox_ws
source devel/setup.bash
roslaunch livox_ros_driver2 msg_MID360.launch

新建终端

cd ~/YouIbot/Livox/livox_ws
source devel/setup.bash
roslaunch lio_livox mid360.launch

点云消息的主题是/livox/lidar，其类型是vox_ros_driver/CustomMsg。IMU消息的主题是/livox/IMU，其类型是sensor_msgs/IMU。

launch文件中有一些参数：

**IMU_Mode：**选择IMU信息融合策略，有3种模式：
0-不使用IMU信息，纯激光雷达里程计，使用等速模型消除运动失真
1-使用IMU预集成来消除运动失真
2-紧密耦合的IMU和激光雷达信息

**extrinic_Tlb：**激光雷达和IMU之间的外部参数，使用SE3形式。

**Use_seg：**选择分割模式进行动态对象过滤，有两种模式：
0-不使用分割方法，如果数据中的动态对象很少，则可以选择此模式
1-使用分割方法去除动态对象

查看内存占用

gnome-system-monitor
top

cpu占用10%左右

2、系统框架

系统由2个ROS节点组成：ScanRegistartion、PoseEstimation。ScanRegistartion节点中的 LidarFeatureExtractor 类动态物体剔除，然后从点云中提取角特征、表面特征、不规则特征。PoseEstimation 节点的主线程目的是估计传感器位姿，其他在 Estimator 类中的线程使用 MapManager 构建并管理特征地图。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-CxaHazny-1686291654436)(/home/jeet/snap/typora/78/.config/Typora/typora-user-images/image-20230506104911791.png)]

2.1 ScanRegistartion 节点

系统从 ScanRegistartion 节点开始处理，在提取特征点之前，首先会从点云中移除动态目标，避免动态目标影响系统的鲁棒性和运算精度。系统使用快速点云分割方法来过滤动态目标。具体是采用欧几里得聚类方法将点云分类，原始点云被分割为地面点、背景点和前景点。前景点为动态目标，在提取特征前把前景点屏蔽，以此提升系统在动态场景下的鲁棒性。

在开放场景中会出现只能提取到少量特征的状况，这样会导致参考约束不足引起运算误差。系统针对该问题设计了一种分布广泛且均匀的特征点提取方法，这种方法给6自由度提供了更多的约束，从而减少运算误差。不规则点也能在特征不明显场景中提供参考信息，因此系统也会提取不规则点特征用于点云匹配。根据点的局部几何属性，将特征点分为3类：角特征、表面特征、不规则特征。首先提取曲率较大的点和每条扫描线上的孤立点作为角点，再使用PCA（主成分分析）对表面特征和不规则特征进行分类。对于不同距离的点设置不同的阈值，从而保证特征在空间分布尽可能均匀。

**运动补偿：**Livox 激光雷达输出自带每个点的时间戳。在获取点云的时候就可从点云数据包 Custom Msg中直接读取到。而其他雷达则可能需要根据各自雷达的 SDK 所提供信息或自行手动解算得到每个点的时间戳

2.2.PoseEstimation过程

在 PoseEstimation 节点中，使用 IMU 预积分或等速模型来补偿点云的运动失真，然后执行 IMU 初始化模块，如果初始化成功则系统会切换到 LIO 模式，否则系统运行在 LO 和 IMU 初始化状态。在 LO 模式下系统采用 frame-to-model 点云配准方法来估算传感器位姿。

受ORB-SLAM3启发，采用最大后验（MAP）估计方法来联合初始化 IMU 偏差、速度和重力方向。这样就不需要关心初始化处理，使得系统可以通过任意动作来进行初始化。该方法根据传感器的不确定性，在最大后验概率上保证最优，从而保证高效、稳健和准确的性能。

初始化后执行基于紧密耦合滑动窗口的传感器融合模块，从而估计滑动窗口内的 IMU 姿态、偏移和速度。同时使用一个额外的线程并行构建和维护全局地图。

3、代码阅读

3.1 相关数据类型

点云消息类型

自定义ros消息CustomMsg

std_msgs/Header header    # ROS standard message header
uint64 timebase           # The time of first point
uint32 point_num          # Total number of pointclouds
uint8  lidar_id           # Lidar device id number
uint8[3]  rsvd            # Reserved use
CustomPoint[] points      # Pointcloud data自定义的点云数据类型

上述自定义数据包中的自定义点云CustomPoint格式 :

uint32 offset_time      # offset time relative to the base time相对于这一帧点云的基准时间的时间偏移量
float32 x               # X axis, unit:m点云坐标
float32 y               # Y axis, unit:m
float32 z               # Z axis, unit:m
uint8 reflectivity      # reflectivity, 0~255
uint8 tag               # livox tag
uint8 line              # laser number in lidar激光雷达扫描的线数

reflectivity 目标反射率以 0 至 255 表示。其中 0 至 150 对应反射率介于 0 至 100% 的漫散射物体；而 151 至255 对应全反射物体

标记(Tag):主要指示探测点的其它附加信息

雷达帧LidarFrame

点云，一个IMU积分器，位姿pvq bias,时间戳

struct LidarFrame{
		pcl::PointCloud<PointType>::Ptr laserCloud;
		IMUIntegrator imuIntegrator;
		Eigen::Vector3d P;
		Eigen::Vector3d V;
		Eigen::Quaterniond Q;
		Eigen::Vector3d bg;
		Eigen::Vector3d ba;
		double timeStamp;
		LidarFrame(){
			P.setZero();
			V.setZero();
			Q.setIdentity();
			bg.setZero();
			ba.setZero();
			timeStamp = 0;
		}
	};

地图管理器 MAP_MANAGER

局部地图是一个基于时间滑窗的帧集合，一共50帧，将帧的不同特征的点存储在特征点数据中然后降采样一下

size_t Id = localMapID % localMapWindowSize;

  for (int i = 0; i < localMapWindowSize; i++) {
    *laserCloudCornerFromLocal += *localCornerMap[i];
    *laserCloudSurfFromLocal += *localSurfMap[i];
    *laserCloudNonFeatureFromLocal += *localNonFeatureMap[i];
  }

全局地图维护

50 * 50 * 50米一个cube，一共 42 * 22 * 42个cube，当激光雷达到达边界第8个cube时，移动地图，尽量保证雷达数据不会落入全局地图之外；
一个cube内存储多雷达帧的点云数据

//降采样后的一个cube中的雷达点云
    pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
//降采样后全局地图中的雷达点云
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
//存储全局地图点的Kd-tree数组，数组索引对应一个cube
    pcl::KdTreeFLANN<PointType>::Ptr laserCloudCornerKdMap[laserCloudNum];

在 Estimator里面开的一个线程,每两帧更新一次地图

      if(map_update_ID % map_skip_frame == 0){
        map_manager->MapIncrement(laserCloudCorner_to_map, 
                                  laserCloudSurf_to_map, 
                                  laserCloudNonFeature_to_map,
                                  transform);

3.2 点云特征提取

/**
 * @brief 检测CustomMsg的激光雷达特征点,不进行动态障碍物剔除，调用核心函数detectFeaturePoint()
 * 
 * @param[in] msg 需要检测原始CustomMsg
 * @param[out] laserCloud 将CustomMsg转换为pcl点云格式，进行了0.01近点剔除，其中点的normal法向量存储时间占比，线束信息，特征点分类，1角点，2平面点
 * @param[out] laserConerFeature 从laserCloud中提取的较少Coner特征
 * @param[out] laserSurfFeature 从laserCloud提取的较少平面特征
 * @param[in] Used_Line 线数
 * @param[in] lidar_type 雷达类型0-horizon  2-mid360
 */
void LidarFeatureExtractor::FeatureExtract(
    const livox_ros_driver::CustomMsgConstPtr& msg,
    pcl::PointCloud<PointType>::Ptr& laserCloud,
    pcl::PointCloud<PointType>::Ptr& laserConerFeature,
    pcl::PointCloud<PointType>::Ptr& laserSurfFeature,
    const int Used_Line,
    const int lidar_type)

使用多线程，每个扫描线上一个线程分别提取，核心函数detectFeaturePoint()

/** \brief 提取特征点，其只保留了不是特别平的的点为面特征，保留了断点和平面交点为角点特征
  * \param[in] cloud: 一条扫描线束上的原始雷达点云
  * \param[in] pointsLessSharp: less sharp点的索引
  * \param[in] pointsLessFlat: less flat点的索引
  */
void LidarFeatureExtractor::detectFeaturePoint(
    pcl::PointCloud<PointType>::Ptr& cloud,
    std::vector<int>& pointsLessSharp,
    std::vector<int>& pointsLessFlat)
        //Step: 1点云赋值，初始化
        //Step: 2计算点云曲率，强度的变化
        //Step: 3平面特征提取============================
        //Step: 4曲面相交处的线特征=======================
        //Step: 5break point 断点======================

点云曲率计算

对应loam里面求曲率的公式，区别在于loam里面求曲率前5后5个点，这里根据点的远近取2or3,，近点选3，远点选2

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PUaWnOCn-1686291654437)(/home/jeet/snap/typora/78/.config/Typora/typora-user-images/image-20230508115136515.png)]

求曲率的同时还求了强度的变化，diffR->0说明强度变化小

float diffR = -2 * thNumCurvSize * _laserCloud->points[i].intensity;
diffR += _laserCloud->points[i - j].intensity +  _laserCloud->points[i + j].intensity;

无虑阈值判断的时候乘 了一次深度在平方

//曲率在一定范围内，越远的点阈值可以大一些
if (cloudCurvature[ind] < thFlatThreshold * cloudDepth[ind] * thFlatThreshold * cloudDepth[ind])

对点的分类

存储在 CloudFeatureFlag[20000]数组中(这个数组存储点的顺序就是原始点的顺序)，其中
1——表示该点前后2or3个点中有3；
2——曲率最小的几个被标为3的点，或被标为3的远点，或点与前后点的角度小于15°的点；最终存为平面点
3——所有曲率小于阈值的点，注意：一部分3点后面会变成2点；
300——强度变化大于阈值且曲率很小的3个点
150——两个面的夹角构成的边缘点
100——断点，左右相邻点距离差超过1m,扫描线与邻点向量夹角大于18 100和150的为角点

平面特征提取

对于平面点的特征有四种标志为1，2，3，300，平面特征提取把一条扫描线分成了100段进行提取，最终选取标志位为2的点为平面点

//每一段按照曲率，强度变化从小到大排序
if (cloudCurvature[ind] < thFlatThreshold * cloudDepth[ind] * thFlatThreshold * cloudDepth[ind])
            {
                CloudFeatureFlag[ind] = 3;
//选择曲率小的点标记为平面(这里标记点的个数没有限制只要阈值满足即标位3)，且前后紧邻的两个点标为1
if (((CloudFeatureFlag[ind] == 3) && (smallestPickedNum <= thNumFlat)) ||
   ((CloudFeatureFlag[ind] == 3) && (cloudDepth[ind] > thDistanceFaraway)) ||
                cloudAngle[ind] == 1)
            {
                smallestPickedNum++;
                CloudFeatureFlag[ind] = 2;
                if (cloudDepth[ind] > thDistanceFaraway)
                {
                    thDistanceFaraway_fea++;
                }
            }
//对前面标记为3和1的点再进新判断表为2，并找出强度变化大的3个点
    if (cloudCurvature[idx] < 0.7 * thFlatThreshold * cloudDepth[idx] *
                                          thFlatThreshold * cloudDepth[idx] &&
                sharpestPickedNum <= 3 && cloudReflect[idx] > 20.0)
            {
                sharpestPickedNum++;
                CloudFeatureFlag[idx] = 300;
            }

平面交线的角点

CloudFeatureFlag[20000]标志数组存储为150,，表示两个面的角点为一种角点特征

如果一个点，左右的四个点曲率小于阈值，则计算左右点的方向向量，其中近的权重小，远的权重大；同时，计算左右最远点的距离；如果，左右向量夹角cos的绝对值小于0.5(即角度大于30°)，且两侧的线都挺长，则标记当前点为150

double cc = fabs(norm_left.dot(norm_right) / (norm_left.norm() * norm_right.norm()));
if (cc < 0.5 && last_dis > 0.05 && current_dis > 0.05)
            {  
                //两个向量的夹角要大于30°，且左右点的距离要大于0.05
                CloudFeatureFlag[i] = 150;
            }

break piont断点

如果当前点左右相邻的点的距离相差特别大，且相邻点向量不与扫描线平行(角度大于18°）认为当前点为断点

调用**plane_judge()**函数判断左边和右边四个点是否构成面,但是这个判断的结果最后没用到

bool left_is_plane = plane_judge(left_list, 100);

 Eigen::JacobiSVD<Eigen::Matrix3d> svd(_matA1, Eigen::ComputeThinU | Eigen::ComputeThinV);
    _matD1 = svd.singularValues();//特征值向量，按特征值降序排序。
    _matV1 = svd.matrixU();//特征向量祖晨的矩阵
    //?: 最大特征值 < 阈值 * 次大特征值
    if (_matD1(0, 0) < plane_threshold * _matD1(1, 0))
    {
        return true;
    }

PCA分析点云是否构成一个面，是一条线（有一个特征值相对于另外两个特征值特别大）或者一个平面（有一个特征值相对于另外两个特征值特别的小）。

/** \brief PCA主成分分析判断point_list点是否组成平面
  * \param[in] point_list: 需要判断的点的集合
  * \param[in] plane_threshold 特征值大小比较阈值，
  * \return 是否是平面 最大特征值 < 阈值* 次大特征值则是，给的阈值为100如果返回是false那大概率是一条线
  */
bool LidarFeatureExtractor::plane_judge(
    const std::vector<PointType>& point_list, const int plane_threshold)
       //?: 最大特征值 < 阈值* 次大特征值
    if (_matD1(0, 0) < plane_threshold * _matD1(1, 0))
    {
        return true;
    }

主要是使用相邻两点的向量与点雷达的向量夹角来判断，夹角大于18°左右，且

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-c3e29bgX-1686291654438)(/home/jeet/图片/BE130BABCFB584D2516CE59813E9A708.jpg)]

// 计算当前点的与左邻近点向量，与当前点到激光雷达的向量的夹角cos绝对值
double cc = fabs(surf_vector.dot(lidar_vector) / (surf_vector.norm() * lidar_vector.norm()));
if (cc < 0.95)
                {  //(max_dis < 2*min_dis) && left_surf_dis < 0.05 * depth  && left_is_plane &&
                   // 如果右点比左点远，则当前点标记100
                    if (depth_right > depth_left)
                    {
                        CloudFeatureFlag[i] = 100;
                    }
                    else
                    {
                        // 如果右点距离小于等于左点，但右点距离为0，则当前点也标记为100
                        if (depth_right == 0)
                            CloudFeatureFlag[i] = 100;
                    }
                }

断点被标记为100

CloudFeatureFlag[i] = 100;

小于1m的点不提取特征，所有标2的点，为平面点，标100（断点）和150（两平面交点）为角点特征

HAP雷达的特征提取

提取了两类点，平面点和非规则点，用最KD树最近邻搜索朝招10个点，点的协方差矩阵PCA主成分分析法，通过比较特征值分出两类特征点。

1）大于100m的点全标为不规则点，最大特征值 < 2倍最小特征值
2）次大特征值-最小特征值> 0.8* 最大特征值(两个特征值接近且很大) || 最大特征值大于2倍次大特征值且大于1/0.13倍最小特征值，(长方体带状区域)则被划分为less Fat 较平的点

3）排除1，2的点就认为是角点，问题？：函数没有哪里给normal_z赋值为1.0

  for (const auto& p : laserCloud->points)
    {
        //?: 并没有哪里给normal_z赋值为1.0
        if (std::fabs(p.normal_z - 1.0) < 1e-5)
             
    }

3.3 动态物体分割

三维栅格，一个栅格的大小0.4 * 0.4 * 0.2，一共x600,y200,z100,存贮0 or 1表示这个栅格内是否有点
保留的点的坐标的范围 x:-40~200m,， y:-40_{40m，z：-2.5}17.5m

主要进行地面与地面上的点分割，然后对地面上的点进行聚类，在后序点云匹配中对具有聚类标签的点进行剔除

1）在雷达坐标系下最高点与均值高度差小于阈值0.4；点数大于3；均值高度小于2 的点为地面点；

2）然后利用PCA分析求地面点集的平均法向量和质心点坐标；

3）基于地平面假设将地面点投影到世界坐标系Z主向上(这里是否可以使用IMU信息进行点云投影的坐标？)

4）使用相对高度对地面进行标记，在一个4*4格内点的z坐标不高于最低点+0.5，高于地面的点进行后序处理

5）对地面上的点进行聚类，分类后的标签1-在一条（极坐标）扫描线上被挡住的点和第一次聚类的点；2~6前面非聚类点所聚类的点云太高或太宽，长；>10点云聚类的标签

6）最后剔除掉的点是具有聚类标签的点

        if (idtrans[i] > 9 && dis < 50 * 50)
        {
            laserCloud->points[i].normal_z = 0;//非地面点，且聚类出来有标签的点
        }

3.4 IMU预积分

在帧与帧之间使用imu的积分模型，等效于在全过程中使用的预积分，(Vins_mono是全局只有一个预积分器，每次调用预积分函数的时候使用预积分模型，delta_q是每次从0开始积分)， LIO-Livox每一个雷达帧具有一个积分器，在每个雷达帧内进行积分，在全局的process函数里面进行更新。

Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3,3) * delta_Rb;

基于误差状态的卡尔曼滤波

IMUIntegrator::PreIntegration(函数中进行IMU的预积分，状态直接累加的，然后维护每帧的雅克比和协方差

(图片公式有点错误)

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SAB7o5hJ-1686291654438)(/home/jeet/图片/Typora图片/IMU预积分状态传播1.png)]

 //todo: 计算A矩阵和B矩阵更新雅克比矩阵和协方差矩阵
    Eigen::Matrix<double,15,15> A = Eigen::Matrix<double,15,15>::Identity();
    A.block<3,3>(0,3) = -0.5*dq.matrix()*Sophus::SO3d::hat(acc)*dt2;
    A.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt;
    A.block<3,3>(0,12) = -0.5*dq.matrix()*dt2;
    A.block<3,3>(3,3) = dR.transpose();
    A.block<3,3>(3,9) = - Jr*dt;
    A.block<3,3>(6,3) = -dq.matrix()*Sophus::SO3d::hat(acc)*dt;
    A.block<3,3>(6,12) = -dq.matrix()*dt;
    Eigen::Matrix<double,15,12> B = Eigen::Matrix<double,15,12>::Zero();
    B.block<3,3>(0,3) = 0.5*dq.matrix()*dt2;
    B.block<3,3>(3,0) = Jr*dt;
    B.block<3,3>(6,3) = dq.matrix()*dt;
    B.block<3,3>(9,6) = Eigen::Matrix3d::Identity()*dt;
    B.block<3,3>(12,9) = Eigen::Matrix3d::Identity()*dt;
    jacobian = A * jacobian;
    covariance = A * covariance * A.transpose() + B * noise * B.transpose();
    //note: 积分模型
    dp += dv*dt + 0.5*dq.matrix()*acc*dt2;
    dv += dq.matrix()*acc*dt;

3.5 基于滑窗的位姿估计

局部地图滑窗大小50帧，优化窗口大小2帧(初始化之前滑窗大小是20帧且每三帧取一帧)， 每次优化都优化窗口你的所有帧的位姿(2帧），每两帧更新一次地图

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Znz764YE-1686291654439)(/home/jeet/文档/学习笔记/LIO-Livox框架.png)]

畸变矫正

由于点云中的每个点都是具有相对于此帧第一个点的时间偏移的，因此可以直接使用匀速假设进行畸变矫正。

/**
 * @brief 一帧雷达点云去畸变，slerp对旋转四元数插值作为此点的旋转四元数，平移直接使用线性插值
 *        最后的点云坐标是去畸变后投影到此帧最后一个点的坐标
 * 
 * @param[in] cloud 此帧雷达点云
 * @param[in] dRlc 旋转矩阵
 * @param[in] dtlc 平移向量
 */
void RemoveLidarDistortion(pcl::PointCloud<PointType>::Ptr& cloud,
                           const Eigen::Matrix3d& dRlc, const Eigen::Vector3d& dtlc){
  int PointsNum = cloud->points.size();
  for (int i = 0; i < PointsNum; i++) {
    Eigen::Vector3d startP;
    float s = cloud->points[i].normal_x;//点云的点的时间偏移在一帧中的占比
    Eigen::Quaterniond qlc = Eigen::Quaterniond(dRlc).normalized();
    //slerp对旋转四元数插值作为此点的旋转四元素，平移直接使用线性插值
    Eigen::Quaterniond delta_qlc = Eigen::Quaterniond::Identity().slerp(s, qlc).normalized();
    const Eigen::Vector3d delta_Plc = s * dtlc;

    startP = delta_qlc * Eigen::Vector3d(cloud->points[i].x,cloud->points[i].y,cloud->points[i].z) + delta_Plc;
    Eigen::Vector3d _po = dRlc.transpose() * (startP - dtlc);

    cloud->points[i].x = _po(0);
    cloud->points[i].y = _po(1);
    cloud->points[i].z = _po(2);
    cloud->points[i].normal_x = 1.0;
  }
}

舒尔补操作求条件概率

		Eigen::VectorXd bmm = b.segment(0, m);//待边缘化的大小Eigen::Vector::segment就是取向量从0开始m个数据
		Eigen::MatrixXd Amr = A.block(0, m, m, n);//把A矩阵分为4个对应的矩阵块
		Eigen::MatrixXd Arm = A.block(m, 0, n, m);
		Eigen::MatrixXd Arr = A.block(m, m, n, n);
		Eigen::VectorXd brr = b.segment(m, n);
		A = Arr - Arm * Amm_inv * Amr;//计算舒尔补
		b = brr - Arm * Amm_inv * bmm;

初始化

第一帧位姿设为单位阵，未初始化时，旋转使用IMU角速度积分更新，平移使用匀速模型

地图初始化

Step1: 重力对齐

  //只优化旋转
  problem_quat.AddParameterBlock(para_quat, 4, quatParam);
   
   //添加残差R * g - a/|a| * 9.8
  problem_quat.AddResidualBlock(Cost_Initial_G::Create(average_acc),
                                nullptr,
                                para_quat);

优化了滑窗内所有帧的速度，初始化帧的bias,重力对齐的R，

残差项

 // add CostFunction重力对齐的点R，先验值是前面重力对齐优化得到的
  problem.AddResidualBlock(Cost_Initialization_Prior_R::Create(prior_r, sqrt_information_r),
                           nullptr,
                           para_r);
  //加速度计bias代价函数，先值为0
  problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_ba, sqrt_information_ba),
                           nullptr,
                           para_ba);
  //陀螺仪残差先验为0
  problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_bg, sqrt_information_bg),
                           nullptr,
                           para_bg);
  //滑窗内所有帧的速度残差，先验值是帧内匀速假设得到的
  for(int i = 0; i < v_size; i++) {
    problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_v[i], sqrt_information_v),
                             nullptr,
                             para_v[i]);
  }
   //滑窗内所有帧的IMU预计分残差
   for(int i = 1; i < v_size; i++) {
    problem.AddResidualBlock(Cost_Initialization_IMU::Create(iter_next->imuIntegrator,
                                                                   ri,
                                                                   rj,
                                                                   pj-pi,
                                                                   Eigen::LLT<Eigen::Matrix<double, 9, 9>>
                                                                    (iter_next->imuIntegrator.GetCovariance().block<9,9>(0,0).inverse())
                                                                    .matrixL().transpose()),
                             nullptr,
                             para_r,
                             para_v[i-1],
                             para_v[i],
                             para_ba,
                             para_bg);
  }

3.6 紧耦合位姿优化

优化项
$$X=[p,q,v,bg,ba]$$
每一个窗口内的帧使用5次ceres优化器迭代，ceres内部迭代10次

IMU预积分残差函数

lio_livox_ws/src/LIO-Livox/include/utils/ceresfunc.h -> struct Cost_NavState_PRV_Bias

struct Cost_NavState_PRV_Bias
{
	Cost_NavState_PRV_Bias(IMUIntegrator& measure_,
							Eigen::Vector3d& GravityVec_,
							Eigen::Matrix<double, 15, 15>  sqrt_information_):
					imu_measure(measure_),
					GravityVec(GravityVec_),
					sqrt_information(std::move(sqrt_information_)){}

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-cVUZpkgx-1686291654439)(/home/jeet/snap/typora/78/.config/Typora/typora-user-images/image-20230510150823053.png)]

//note: 计算残差的时候使用线性化的方法减去了bias变化量带来的误差
        //平移p的残差，两帧之间的平移残差
		Eigen::Matrix<T, 3, 1> rPij = RiT*(Pj - Pi - Vi*dTij - 0.5*GravityVec.cast<T>()*dT2) -
						(dPij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BG).cast<T>()*dbgi +
						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BA).cast<T>()*dbai);
        //bias的变化给旋转带来的误差
		Sophus::SO3<T> dR_dbg = Sophus::SO3<T>::exp(
						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_R, IMUIntegrator::O_BG).cast<T>()*dbgi);
		Sophus::SO3<T> rRij = (dRij * dR_dbg).inverse() * RiT * SO3_Rj;
		Eigen::Matrix<T, 3, 1> rPhiij = rRij.log();
        //速度残差
		Eigen::Matrix<T, 3, 1> rVij = RiT*(Vj - Vi - GravityVec.cast<T>()*dTij) -
						(dVij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BG).cast<T>()*dbgi +
										imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BA).cast<T>()*dbai);

IMU预积分的残差是直接使用状态卡尔曼滤波推导出来的

//残差左乘信息矩阵
		eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());
 Eigen::LLT<Eigen::Matrix<double, 15, 15>> (frame_curr->imuIntegrator.GetCovariance().inverse())                      .matrixL().transpose()),

边缘化残差

和vins一样的边缘化逻辑

 //如有上一次的边缘化信息
    if (last_marginalization_info){
      // 根据上一次边缘化的信息构建一个新的边缘化因子
      auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
      //todo: 添加边缘化残差
      problem.AddResidualBlock(marginalization_factor, nullptr,
                               last_marginalization_parameter_blocks);
    }

点的匹配残差

一共有三种特征点，但是，HAP雷达没有Corner角点特征，mid360和horizon雷达没有NonFeature不规则特征点
点匹配的残差给的信息矩阵: 1/IMUIntegrator::lidar_m) 协方差1.5e-3
在点匹配的残差里面，如果在全局地图中找到匹配点，就会从加入点到全局地图的匹配残差，再使用kd-tee最近邻搜索，在局部地图(50帧)里面搜索最近的线特征点，平面特征点，(不规则特征点)。使用主成分分析法计算匹配到的5个点的方向向量。
三个线程同时添加点匹配的 残差

	std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudCornerLast;//滑窗内每一帧角点点云
	std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudSurfLast;//滑窗内每一帧平面点云
	std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudNonFeatureLast;//滑窗内每一帧不规则特征点云

/**
 * @brief 通过激光雷达云和地图的匹配以及紧耦合的IMU信息来估计激光雷达姿态
 * 
 * @param[in] lidarFrameList 当前雷达帧，包含IMU积分信息，预测位姿
 * @param[in] exTlb 外参
 * @param[in] gravity 重力向量
 */
void Estimator::Estimate(std::list<LidarFrame>& lidarFrameList,
                         const Eigen::Matrix4d& exTlb,
                         const Eigen::Vector3d& gravity){

点到线的残差

点到线的距离l，再给了一个权重系数：其中rp是点到世界坐标系原点的距离
$$1-0.9\ast l/{\sqrt{r}}_p$$

      T _weight = T(1) - T(0.9) * ceres::abs(ld2) / ceres::sqrt(
              ceres::sqrt( P_to_Map(0) * P_to_Map(0) +
                           P_to_Map(1) * P_to_Map(1) +
                           P_to_Map(2) * P_to_Map(2) )); 
residual[0] = T(sqrt_information(0)) * _weight * ld2;//点到线的残差是一维的

//note: 如果全局地图对应的cube位置有点则在全局地图里面查找5个近邻点
if(GlobalCornerMap[id].points.size() > 100)
//note: 如果局部地图里面有点，那就尝试进行最近邻搜索匹配
if(laserCloudCornerLocal->points.size() > 20 )

点到面的残差

最近邻查找5个点，直接五点法QR分解求出平面的方程，对abc法向量归一化，，然后求出点在面上的投影点，求点到面的距离

点与NonFeature点之间的残差

点与不规则点之间的残差，其实就是这5个点拟合一个平面，然后计算点到面的距离

添加点匹配残差

正常跟踪流程下windowSize == SLIDEWINDOWSIZE = 2，如果第一次计算参加就特别小的点以后就不会添加它的残差了

边缘化

优化完成之后，边缘化掉滑窗中的第一帧的状态参数

待边缘化的信息矩阵来自于三部分，上一次边缘化的信息矩阵+ IMU预积分的信息矩阵+ 匹配点的信息矩阵

 //根据上一次的边缘化结果创建此时的边缘化因子，因为边缘化的雅克比矩阵是从一开始逐渐传递的
auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
auto *residual_block_info = new ResidualBlockInfo(marginalization_factor, nullptr,
                                                          last_marginalization_parameter_blocks,
                                                          drop_set);
        //添加上一次边缘化时候的信息矩阵                                               
marginalization_info->addResidualBlockInfo(residual_block_info);
//================================================================

auto *residual_block_info = new ResidualBlockInfo(IMU_Cost, nullptr,
                                                        std::vector<double *>{para_PR[0], para_VBias[0], para_PR[1], para_VBias[1]},
                                                        std::vector<int>{0, 1});
      //todo: 添加与边缘化帧相关联的下一帧的IMU预积分的残差，边缘化时IMU预积分约束只会影响下一帧
      marginalization_info->addResidualBlockInfo(residual_block_info);
//=================================================================
auto *residual_block_info = new ResidualBlockInfo(e, nullptr,
                                                            std::vector<double *>{para_PR[0]},
                                                            std::vector<int>{0});
marginalization_info->addResidualBlockInfo(residual_block_info);

相关参数

点云分割相关参数

1）lio_livox_ws/src/LIO-Livox/src/segment/segment.cpp文件

GndSeg()函数

if(pGndImg1[tmpLabel1[pid]]+0.5>fPoints[pid*4+2])

2）0.5是地面筛选时，一个栅格内的点云不超过最低点的0.5

SegObjects（）函数：前后景分割kd-tree搜索阈值

    SegObjects(pLabel,cloud,kdtree,0.7);