话说SLAM中的点云上采样

目录

1 什么是点云上采样，为什么需要点云上采样

2 以LIO-SAM为例进行方法1的讲解

2.1 IMU预积分代码解析 + 点云加密思想

2.1.1 收到IMU信息的回调函数  

2.2.2 优化函数置位  resetOptimization

2.2.3 TF类

2.3.3.1 流程图

2.3.3.2 代码详细注释

2.2.4 总结

2.2 图像投影代码解析 + 点云加密思想

2.2.1 本部分流程图

2.2.2 构造函数解析

2.2.3  IMU回调函数解析

2.2.4  后端里程计回调函数

2.2.5 原始点云回调函数

2.2.5.1 缓存点云信息函数cachePointCloud 

2.5.5.2  获取运动补偿所需的信息 deskewInfo

**2.5.5.3 把点云投影到一个矩阵上 保存每个点的信息 projectPointCloud

2.5.5.4  剔除点的信息 cloudExtraction

2.5.5.5 发布点云信息 publishClouds

2.5.5.6 系统置位  resetParameters

2.2.6 总结

2.3 提取特征点代码解析 + 点云加密思想

2.3.1 流程图

2.3.2 遮挡点判断的数学原理

2.3.3 计算曲率calculateSmoothness

2.3.4 移除遮挡点markOccludedPoints

***2.3.5 特征提取extractFeatures

2.3.6 发布点云信息

2.4 地图优化代码解析 + 点云加密思想（地图优化部分）

2.4.1 构造函数分析 mapOptimization()

2.4.2  laserCloudInfoHandler 回调函数解析

2.4.3  updateInitialGuess函数解析

****2.4.4  提取当前帧的相关关键帧并且构建点云局部地图extractSurroundingKeyFrames

****2.4.5  对当前帧进行下采样downsampleCurrentScan

2.4.6 scan2MapOptimization 求得一个Rt使得当前帧能最好的匹配到局部地图上面去

2.4.7 判断是否插入关键帧 saveKeyFramesAndFactor

2.4.8 调整全局轨迹 correctPoses

2.4.9  发布lidar里程计信息 publishOdometry

2.4.10 发布点云关键帧信息

2.5 地图优化代码解析 + 点云加密思想（地图优化部分）

2.5.1 闭环检测主进程

2.5.2 执行回环检测

2.5.3 检测有没有外部通知的回环信息detectLoopClosureExternal

2.5.4 检测内部回环 detectLoopClosureDistance

****2.5.5 取出回环候选帧的点云  loopFindNearKeyframes

2.5.6 执行ICP计算位姿变换  ICP solver

2.5.7 发布可视化回环信息 /lio_sam/mapping/loop_closure_constraints

2.6 地图保存代码解析 + 点云加密思想（地图优化部分）

****2.6.1 发布全局地图 publishGlobalMap

2.6.2 保存地图saveMapService

---

1 什么是点云上采样，为什么需要点云上采样

        点云上采样，又被称为点云密集化，旨在将稀疏点云作为输入，并生成密集点云作为输出。在众多SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）算法中，许多算法的设计目标是用于自动驾驶或者嵌入到边缘计算设备中（如TX2、STM32）。

由于这些背景要求下的计算时间复杂度至关重要，研究者通常对传感器生成的点云进行处理。例如，对于基于视觉SLAM的算法，点云通常通过深度图、双目测距或三角测量等方式生成。为了控制计算复杂度，研究者会对这些点云进行下采样，以减少点的数量。然而，在某些特定场景下，需要获得密集的点云数据，比如在古建筑扫描、林下建图和激光点云赋色等应用中。

        在SLAM领域，点云上采样的方法可以分为以下几种：

        1. **禁用点云下采样的代码部分**（虽然这会增加计算复杂性，但由于非线性优化迭代次数的增加，也会提升系统精度）。

        2. 通过**线性插值**的方式对稀疏点云进行上采样（虽然会影响SLAM系统的精度，但能够使点云呈现更加平滑的外观）。

        3. **提升传感器性能**（通过投入更多资金购买分辨率更高的设备）。

        4.**基于邻域信息的非线性插值**：使用点云中每个点的邻近点信息，进行更复杂的插值，以生成更真实的密集点云。这种方法考虑了点与点之间的关系，可以在保持准确性的同时提升点云密度。

        5.**基于深度学习的生成模型**：利用深度学习技术，如生成对抗网络（GAN）或变分自编码器（VAE），从稀疏点云中生成密集点云。这种方法在一些场景下可以生成更具细节的点云数据。

        6.**多传感器融合**：结合多个传感器的数据，如相机和激光雷达，利用它们的互补优势进行点云融合。这可以提高点云的密度和准确性，尤其在多传感器SLAM系统中。

        这篇文章我们以LIO-SAM为例子进行方法一的讲解。

2 以LIO-SAM为例进行方法1的讲解

2.1 IMU预积分代码解析 + 点云加密思想

        LIO-SAM中IMU预积分部分的流程图如下：

        我们首先看main函数

int main(int argc, char** argv)
{
    ros::init(argc, argv, "roboat_loam");
    
    IMUPreintegration ImuP;

    TransformFusion TF;

    ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");

    // 分配4个线程执行这个节点
    ros::MultiThreadedSpinner spinner(4);

    // 程序阻塞，后台等待回调函数的执行
    spinner.spin();
    
    return 0;
}

1. `int main(int argc, char** argv)`: 这是C++程序的入口函数，接受命令行参数`argc`和`argv`。

2. `ros::init(argc, argv, "roboat_loam")`: 这行代码初始化了ROS节点，使用了参数`argc`和`argv`，并指定了节点的名称为"roboat_loam"。

3. `IMUPreintegration ImuP;`: 这行代码创建了一个名为`ImuP`的`IMUPreintegration`对象。它是一个自定义类的实例化对象，表示进行IMU预积分的功能。

4. `TransformFusion TF;`: 这行代码创建了一个名为`TF`的`TransformFusion`对象。它是一个自定义类的实例化对象，表示进行坐标变换融合的功能。

5. `ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m")`: 这行代码输出一个ROS信息日志，显示"IMU Preintegration Started"。使用了特殊的字符转义序列来设置输出文本的颜色为绿色。

6. `ros::MultiThreadedSpinner spinner(4)`: 这行代码创建了一个`MultiThreadedSpinner`对象，用于多线程执行节点的回调函数。指定了线程数为4，表示使用4个线程执行回调函数。

7. `spinner.spin()`: 这行代码启动了多线程的回调函数执行，并且会阻塞程序，使其等待回调函数的执行。

8. `return 0;`: 这行代码表示程序正常结束，返回0。

        综上所述，这段代码初始化了ROS节点，创建了`IMUPreintegration`和`TransformFusion`对象，并输出一个ROS信息日志。然后，通过多线程的方式执行节点的回调函数，直到所有回调函数执行完成后，程序结束并返回0。

2.1.1 收到IMU信息的回调函数  

    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard lock(mtx);

        // ROS_TIME函数用于获取当前odomMsg的时间戳的值
        double currentCorrectionTime = ROS_TIME(odomMsg);

        // make sure we have imu data to integrate
        // 判断IMU队列是否有数据
        if (imuQueOpt.empty())
            return;

        // 获取里程计 平移 XYZ 旋转xyzw
        float p_x = odomMsg->pose.pose.position.x;
        float p_y = odomMsg->pose.pose.position.y;
        float p_z = odomMsg->pose.pose.position.z;
        float r_x = odomMsg->pose.pose.orientation.x;
        float r_y = odomMsg->pose.pose.orientation.y;
        float r_z = odomMsg->pose.pose.orientation.z;
        float r_w = odomMsg->pose.pose.orientation.w;

        // 有退化风险（里程计的精度有风险下降）
        bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;

        // 位姿转化成gtsam格式（旋转 + 平移）
        gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));


        // 0. 初始化
        if (systemInitialized == false)
        {
            // 将优化问题复位
            resetOptimization();

            // 把IMU数据扔掉
            while (!imuQueOpt.empty())
            {
                // 如果IMU数据的时间戳 < 当前里程计的时间戳
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
                    // 被扔掉IMU数据的时间戳，这个变量最终保存刚好小于当前里程计时间戳的帧
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }
                else
                    break;
            }

             添加约束

            // 将LIDAR的位姿转换到IMU坐标系下
            prevPose_ = lidarPose.compose(lidar2Imu);
            gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise);
            graphFactors.add(priorPose);

            // gtsam::Vector3表示三自由度速度，由于刚开始确实不知道是多少，直接置为0了
            prevVel_ = gtsam::Vector3(0, 0, 0);
            // 设置其初始值和置信度
            gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise);
            // 约束加入到因子中
            graphFactors.add(priorVel);



            // gtsam::imuBias::ConstantBias表示IMU零偏   X(Pose3 xyzrpy)  Y(Vel xyz) B(BIAS axayazgxgygz)
            prevBias_ = gtsam::imuBias::ConstantBias();
            // 这段代码是使用GTSAM库中的PriorFactor类创建一个先验因子（PriorFactor），用于对IMU偏差（imuBias::ConstantBias）进行约束。
            //具体解释如下：
            // gtsam::PriorFactor：这是一个GTSAM库中的PriorFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
            // priorBias：这是创建的先验因子对象的名称。
            // B(0)：这是一个表示IMU偏差的变量，其中(0)是索引号，用于区分多个偏差变量。这里使用索引号为0的偏差变量。
            // prevBias_：这是一个先前的IMU偏差的变量，它被用作先验因子的先验值。
            // priorBiasNoise：这是一个表示偏差噪声的对象，用于指定对IMU偏差的不确定性。
            // 综合起来，这段代码创建了一个先验因子，它约束了IMU偏差变量B(0)的值必须接近于先前的偏差值prevBias_，并且在计算图优化过程中使用了指定的偏差噪声来表示对这个约束的不确定性。
            gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise);
            // 将约束添加到因子中
            graphFactors.add(priorBias);


             添加状态

            // 添加状态量
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);

             约束graphFactors + 状态 graphValues
            // 约束和状态添加完了，开始优化
            optimizer.update(graphFactors, graphValues);
            // 清空变量 方便下一次进行优化
            graphFactors.resize(0);
            graphValues.clear();

            // 预积分的接口，使用初始零值进行初始化
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
            
            key = 1;
            // 初始化标志位置为true
            systemInitialized = true;
            return;
        }


        // reset graph for speed
        // 执行100次 当isam优化器中加入较多的约束后，为了避免运算时间变长，直接清空 初始化因子图优化问题
        if (key == 100)
        {
            // get updated noise before reset
            // 取出最新时刻位姿 速度 零偏的协方差矩阵
            gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
            gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
            gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
            // reset graph
            // 复位整个优化问题
            resetOptimization();
            // add pose
            gtsam::PriorFactor priorPose(X(0), prevPose_, updatedPoseNoise);
            graphFactors.add(priorPose);
            // add velocity
            gtsam::PriorFactor priorVel(V(0), prevVel_, updatedVelNoise);
            graphFactors.add(priorVel);
            // add bias
            gtsam::PriorFactor priorBias(B(0), prevBias_, updatedBiasNoise);
            graphFactors.add(priorBias);
            // add values
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);
            // optimize once
            optimizer.update(graphFactors, graphValues);
            graphFactors.resize(0);
            graphValues.clear();

            key = 1;
        }


        // 1. 初始化结束后，我们已经有一个LIDAR坐标系转换到IMU坐标系下的一个位姿，下面开始形成预积分的约束
        while (!imuQueOpt.empty())
        {
            // pop and integrate imu data that is between two optimizations
            sensor_msgs::Imu *thisImu = &imuQueOpt.front();
            double imuTime = ROS_TIME(thisImu);
            // IMU的时间戳要小于LIDAR的时间戳
            if (imuTime < currentCorrectionTime - delta_t)
            {
                // 计算两个IMU之间的时间戳
                double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
                // 调用预积分接口将IMU数据送进去处理
                imuIntegratorOpt_->integrateMeasurement(
                        gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
                
                lastImuT_opt = imuTime;
                imuQueOpt.pop_front();
            }
            else
                break;
        }

        // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
        const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast(*imuIntegratorOpt_);

        // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
        // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
        gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
        // 约束加入因子图
        graphFactors.add(imu_factor);

        // 这段代码是使用GTSAM库中的BetweenFactor类创建一个因子（BetweenFactor），用于表示两个IMU偏差变量之间的关系。
        // graphFactors：这是一个图（graph）对象，用于存储因子（factor）。
        // gtsam::BetweenFactor：这是一个GTSAM库中的BetweenFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
        // B(key - 1)：这是表示先前的IMU偏差变量的变量，其中key - 1是索引号，用于区分多个偏差变量。这里使用索引号为key - 1的偏差变量。
        // B(key)：这是表示当前的IMU偏差变量的变量，其中key是索引号，用于区分多个偏差变量。这里使用索引号为key的偏差变量。
        // gtsam::imuBias::ConstantBias()：这是一个默认构造的IMU偏差对象，作为两个偏差变量之间的初始估计。
        // gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)：这是一个表示噪声模型的对象，用于表示两个偏差变量之间的不确定性。imuIntegratorOpt_->deltaTij()表示IMU积分器在两个时间步之间的时间差，noiseModelBetweenBias是一个噪声模型，通过对两个时间步之间的时间差进行标准化来定义偏差之间的不确定性。
        // 综合起来，这段代码创建了一个因子，它表示了两个IMU偏差变量之间的关系，并使用指定的噪声模型来表示这个关系的不确定性。然后，该因子被添加到一个图对象中，以便在后续的图优化过程中使用。
        graphFactors.add(gtsam::BetweenFactor(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                         gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));

        // 这段代码将激光雷达位姿(lidarPose)与激光雷达坐标系到IMU坐标系之间的变换关系(lidar2Imu)进行组合操作，得到了当前的位姿(curPose)。
        // 这通常用于将激光雷达位姿转换到IMU坐标系下的位姿
        gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
        // LIDAR位姿补偿到IMU坐标系下同时根据是否退化选择不同的置信度作为这一帧的先验估计
        gtsam::PriorFactor pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
        // 加入到因子图
        graphFactors.add(pose_factor);

        // 根据上一时刻的状态 结合预积分的结果 对当前状态进行预测  prevState_上一关键帧的位姿 + prevBias_上一关键帧的零偏
        // 这段代码使用GTSAM库中的ImuIntegrator类的predict()函数，基于先前的状态和偏差预测生成一个新的导航状态。
        // gtsam::NavState propState_：这是一个NavState类型的对象，用于存储生成的预测位姿
        // imuIntegratorOpt_：这是一个ImuIntegrator类的指针或引用，用于执行IMU积分和状态预测操作。
        // prevState_：这是一个先前的导航状态对象，作为IMU积分的起始状态。
        // prevBias_：这是一个先前的IMU偏差对象，用作IMU积分的起始偏差。
        // predict(prevState_, prevBias_)：这是ImuIntegrator类的成员函数，根据先前的状态和偏差，通过IMU积分预测生成一个新的位姿状态。
        // 综合起来，这段代码使用ImuIntegrator类的predict()函数，基于先前的导航状态和偏差，进行IMU积分预测，生成一个新的导航状态，并将其存储在propState_中。
        // 这个预测的导航状态通常用于在导航和状态估计问题中进行运动预测和滤波更新。
        gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);

        // 预测量作为初始值插入到因子图中
        graphValues.insert(X(key), propState_.pose());
        graphValues.insert(V(key), propState_.v());
        graphValues.insert(B(key), prevBias_);

        // 执行因子图优化
        optimizer.update(graphFactors, graphValues);
        optimizer.update();

        // 清空
        graphFactors.resize(0);
        graphValues.clear();

        // 获取优化结果
        gtsam::Values result = optimizer.calculateEstimate();

        // 获取优化后的当前状态作为当前帧的最佳估计
        prevPose_  = result.at(X(key));
        prevVel_   = result.at(V(key));
        prevState_ = gtsam::NavState(prevPose_, prevVel_);
        prevBias_  = result.at(B(key));

        // 当前约束任务已完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先诀条件
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
        // check optimization
        // 一个简单的失败检测
        if (failureDetection(prevVel_, prevBias_))
        {
            // 状态异常直接复位
            resetParams();
            return;
        }


        // 2. 优化之后，根据最新的IMU状态进行传播
        // 因为我们“同时”获得IMU和LIDAR里程计位姿，通常IMU数据会来的更早一点，LIDAR数据并不是原始数据而是一个里程计的数据（比如我们10s只能拿到第9s的odom的数据）
        // 而IMU数据是实时得到的（系统时间100s 可以收到99s的IMU 只能收到90s的odom），然而我们需要实时性，利用余下的IMU推算位姿
        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        double lastImuQT = -1;

        // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }

        // 如果有新于LIAR状态的IMU数据
        if (!imuQueImu.empty())
        {
            // 将这个预积分变量复位
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);

            for (int i = 0; i < (int)imuQueImu.size(); ++i)
            {
                sensor_msgs::Imu *thisImu = &imuQueImu[i];
                double imuTime = ROS_TIME(thisImu);
                double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

                imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                                                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
                lastImuQT = imuTime;
            }
        }

        // 每做一次优化 ++key 到key==100 执行前段代码
        ++key;
        doneFirstOpt = true;
    }

        注释已经很详细了，我们来讲解一下步骤：

        1.检查IMU队列imuQueOpt是否有数据，取出里程计的数据。并将odom的位姿转化成gtsam格式。

        2.如果系统没有初始化（systemInitialized == false）：将优化问题置位（初始化graphFactors、graphValues），将imuQueOpt比当前里程计数据时间戳小的数据扔掉。

        计算初始值：
        LIDAR位姿：用传进来的ODOM坐标转换到LIDAR坐标系为初始值

        速度：0

        IMU零偏：设置

        将预积分的接口（imuIntegratorImu_、imuIntegratorOpt_）使用初始零值进行初始化并初始化bias，将key设置为1（第一次优化）。将初始化位置为1。

        3.初始化结束后，我们已经有一个LIDAR坐标系转换到IMU坐标系下的一个位姿，下面开始形成预积分的约束。

using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

        取出imuQueOpt中一帧数据（在两帧ODOM之间的IMU数据帧）。

        imuIntegratorOpt_计算预积分。

const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast(*imuIntegratorOpt_);

        将两帧之间的预积分结果存储到preint_imu中。

        增加约束：通过创建`gtsam::ImuFactor`对象，将相邻两帧之间的位姿、速度和零偏之间的约束添加到因子图中。这个因子使用了上一帧的位姿和速度（`X(key - 1)`和`V(key - 1)`），当前帧的位姿和速度（`X(key)`和`V(key)`），以及上一帧的零偏（`B(key - 1)`）作为输入。同时，还使用了预积分的结果（`preint_imu`）来定义约束。

        // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
        const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast(*imuIntegratorOpt_);

        // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
        // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
        gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
        // 约束加入因子图
        graphFactors.add(imu_factor);

        取出预积分结果作为约束初始值，执行因子图优化。

        4.更新当前帧最佳优化值

        prevPose_  = result.at(X(key));
        prevVel_   = result.at(V(key));
        prevState_ = gtsam::NavState(prevPose_, prevVel_);
        prevBias_  = result.at(B(key));

        5.当前约束任务已完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先诀条件。

imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

        6.进行失败检测：利用速度和IMU偏置，做一个简单的判断。如果失败了则重置系统。

    bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
    {
        Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
        // 当前速度超过30m/s 108km/h认为是异常状态
        if (vel.norm() > 30)
        {
            ROS_WARN("Large velocity, reset IMU-preintegration!");
            return true;
        }

        Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
        Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
        // 零偏太大，那也不太正常
        if (ba.norm() > 1.0 || bg.norm() > 1.0)
        {
            ROS_WARN("Large bias, reset IMU-preintegration!");
            return true;
        }

        return false;
    }

    void resetParams()
    {
        lastImuT_imu = -1;
        doneFirstOpt = false;
        systemInitialized = false;
    }

        7.优化之后，根据最新的IMU状态进行传播，因为我们“同时”获得IMU和LIDAR里程计位姿，通常IMU数据会来的更早一点，LIDAR数据并不是原始数据而是一个里程计的数据（比如我们10s只能拿到第9s的odom的数据）而IMU数据是实时得到的（系统时间100s 可以收到99s的IMU 只能收到90s的odom），然而我们需要实时性，利用余下的IMU推算位姿。

        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        double lastImuQT = -1;

        // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }

        // 如果有新于LIAR状态的IMU数据
        if (!imuQueImu.empty())
        {
            // 将这个预积分变量复位
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);

            for (int i = 0; i < (int)imuQueImu.size(); ++i)
            {
                sensor_msgs::Imu *thisImu = &imuQueImu[i];
                double imuTime = ROS_TIME(thisImu);
                double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

                imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                                                        gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
                lastImuQT = imuTime;
            }
        }

        // 每做一次优化 ++key 到key==100 执行前段代码
        ++key;
        doneFirstOpt = true;

        8.执行100次 当isam优化器中加入较多的约束后，为了避免运算时间变长，直接清空 初始化因子图优化问题。

        if (key == 100)
        {
            // get updated noise before reset
            // 取出最