LIO-SAM后端中的后端里程计、GPS、回环因子

这部分主要是讲如何添加odom因子、gps因子、回环因子和获取优化后的信息给到下一次优化使用

讲解该函数void saveKeyFramesAndFactor()

增加odom因子 addOdomFactor()

• 第一帧时 直接用imu原始给的信息，只是置信度设置差一点，由于不可观的有xyz和yaw角，这4个的置信度会设置的更差。对第0个节点增加先验约束并加入节点信息

        // 如果是第一帧关键帧
        if (cloudKeyPoses3D->points.empty())
        {
   
            // 置信度就设置差一点，尤其是不可观的平移和yaw角
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            // 增加先验约束，对第0个节点增加约束
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            // 加入节点信息
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));

• 如果不是第一帧就添加帧间约束，这是帧间约束置信度就设置高一些，其中poseFrom是当前帧（还没优化过的）poseTo是上一帧优化过的关键帧

	else{
   
            // 如果不是第一帧，就增加帧间约束
            // 这时帧间约束置信度就设置高一些
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            // 转成gtsam的格式
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);
            // 这是一个帧间约束，分别输入两个节点的id，帧间约束大小以及置信度
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            // 加入节点信息
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }

增加GPS因子 addGPSFactor()

GPS的使用比较谨慎，所以有很多判断退出的条件

• 没有GPS信息，退出
• 有GPS信息但是没有关键帧，退出
• 第一个关键帧和最后一个关键帧相差太近（不是起步就出发回环），退出
• 上次优化后的置信度较高，退出
  开始处理GPS信息

1. 把距离当前帧比较早的帧都抛弃
2. 如果GPS的信息比lidar早就等一下lidar再计算
3. 筛选处理的GPS数据在时间上距离当前帧是比较近的，把这个数据取出来就pop出来
4. 获取GPS信息的置信度，置信度不高就跳过不使用（GPS信息里面自带的置信度）
5. 置信度高的话取出GPS的位置，z 的信息不使用
6. GPS的x或者y太小说明还没有初始化好，退出
7. 每相隔5m才添加GPS观测
8. 将GPS的置信度的标准差设置成最小1m（即不特别信任GPS）
9. 将当前帧位姿，GPS位姿，GPS噪声加入GPS因子图中

    void addGPSFactor()
    {
   
        // 如果没有gps信息就算了
        if (gpsQueue.empty()