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())
            return;

        // wait for system initialized and settles down
        // 如果有gps但是没有关键帧信息也算了
        if (cloudKeyPoses3D->points.empty())
            return;
        else
        {
            // 第一个关键帧和最后一个关键帧相差很近也算了，要么刚起步，要么会触发回环
            if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
                return;
        }

        // pose covariance small, no need to correct
        // gtsam反馈的当前x，y的置信度，如果置信度比较高也不需要gps来打扰
        if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
            return;

        // last gps position
        static PointType lastGPSPoint;

        while (!gpsQueue.empty())
        {
            // 把距离当前帧比较早的帧都抛弃
            if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
            {
                // message too old
                gpsQueue.pop_front();
            }
            // 比较晚就索性再等等lidar计算
            else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
            {
                // message too new
                break;
            }
            else
            {
                // 说明这个gps时间上距离当前帧已经比较近了,那就把这个数据取出来
                nav_msgs::Odometry thisGPS = gpsQueue.front();
                gpsQueue.pop_front();

                // GPS too noisy, skip
                float noise_x = thisGPS.pose.covariance[0];
                float noise_y = thisGPS.pose.covariance[7];
                float noise_z = thisGPS.pose.covariance[14];
                // 如果gps的置信度不高，也没有必要使用了
                if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
                    continue;
                // 取出gps的位置
                float gps_x = thisGPS.pose.pose.position.x;
                float gps_y = thisGPS.pose.pose.position.y;
                float gps_z = thisGPS.pose.pose.position.z;
                // 通常gps的z没有x y准，因此这里可以不使用z值
                if (!useGpsElevation)
                {
                    gps_z = transformTobeMapped[5];
                    noise_z = 0.01;
                }

                // GPS not properly initialized (0,0,0)
                // gps的x或者y太小说明还没有初始化好
                if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
                    continue;

                // Add GPS every a few meters
                PointType curGPSPoint;
                curGPSPoint.x = gps_x;
                curGPSPoint.y = gps_y;
                curGPSPoint.z = gps_z;
                // 加入gps观测不宜太频繁，相邻不超过5m
                if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
                    continue;
                else
                    lastGPSPoint = curGPSPoint;

                gtsam::Vector Vector3(3);
                // gps的置信度，标准差设置成最小1m，也就是不会特别信任gps信号
                Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
                noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
                // 调用gtsam中集成的gps的约束
                gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
                gtSAMgraph.add(gps_factor);
                // 加入gps之后等同于回环，需要触发较多的isam update
                aLoopIsClosed = true;
                break;
            }
        }
    }

添加回环因子 addLoopFactor()

回环信息队列里面的数据由回环检测线程塞进来，具体处理见LIO-SAM后端中的回环检测及位姿计算

1. 回环信息队列为空，退出
2. 有信息，把队列里的回环约束都添加进来
   回环约束的信息有 **当前帧位姿信息、回环帧位姿信息、两帧间位姿变换、回环置信度（icp得分）**把这些信息添加进因子图中
3. 清空回环相关队列

    void addLoopFactor()
    {
        // 有一个专门的回环检测线程会检测回环，检测到就会给这个队列塞入回环结果
        if (loopIndexQueue.empty())
            return;
        // 把队列里所有的回环约束都添加进来
        for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
        {
            int indexFrom = loopIndexQueue[i].first;
            int indexTo = loopIndexQueue[i].second;
            // 这是一个帧间约束
            gtsam::Pose3 poseBetween = loopPoseQueue[i];
            // 回环的置信度就是icp的得分
            gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
            // 加入约束
            gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
        }
        // 清空回环相关队列
        loopIndexQueue.clear();
        loopPoseQueue.clear();
        loopNoiseQueue.clear();
        // 标志位置true
        aLoopIsClosed = true;
    }

因子图更新

• 上面所有因子加完了就调用isam接口更新图模型，如果加入了GPS的约束或者回环约束，则isam需要进行更多次的更新（代码中是5次）

        // 所有因子加完了，就调用isam接口更新图模型
        // update iSAM
        isam->update(gtSAMgraph, initialEstimate);
        isam->update();
        // 如果加入了gps的约束或者回环约束，isam需要进行更多次的优化
        if (aLoopIsClosed == true)
        {
            isam->update();
            isam->update();
            isam->update();
            isam->update();
            isam->update();
        }

• 清空约束和节点信息

        // 将约束和节点信息清空，他们已经被加入到isam中去了，因此这里清空不会影响整个优化
        gtSAMgraph.resize(0);
        initialEstimate.clear();

• 将优化后的结果取出来放入cloudKeyPoses3D和cloudKeyPoses6D中，6D的带有旋转位姿信息，3D仅有xyz信息

        // 下面保存关键帧信息
        //save key poses
        PointType thisPose3D;
        PointTypePose thisPose6D;
        Pose3 latestEstimate;

        isamCurrentEstimate = isam->calculateEstimate();
        // 取出优化后的最新关键帧位姿
        latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
        // cout << "****************************************************" << endl;
        // isamCurrentEstimate.print("Current estimate: ");
        // 平移信息取出来保存进cloudKeyPoses3D这个结构中，其中索引作为intensity值
        thisPose3D.x = latestEstimate.translation().x();
        thisPose3D.y = latestEstimate.translation().y();
        thisPose3D.z = latestEstimate.translation().z();
        thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
        cloudKeyPoses3D->push_back(thisPose3D);
        // 6D姿态同样保留下来
        thisPose6D.x = thisPose3D.x;
        thisPose6D.y = thisPose3D.y;
        thisPose6D.z = thisPose3D.z;
        thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index
        thisPose6D.roll  = latestEstimate.rotation().roll();
        thisPose6D.pitch = latestEstimate.rotation().pitch();
        thisPose6D.yaw   = latestEstimate.rotation().yaw();
        thisPose6D.time = timeLaserInfoCur;
        cloudKeyPoses6D->push_back(thisPose6D);

• 将优化后的位姿放入transformTobeMapped中，作为当前最佳估计值，给到下一次循环使用（后面将有一篇文章对重要变量数据的流转进行梳理），数组中的顺序为(roll , pitch , yaw , x , y , z )

        // 保存当前位姿的置信度
        poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);

        // save updated transform
        // 将优化后的位姿更新到transformTobeMapped数组中，作为当前最佳估计值
        transformTobeMapped[0] = latestEstimate.rotation().roll();
        transformTobeMapped[1] = latestEstimate.rotation().pitch();
        transformTobeMapped[2] = latestEstimate.rotation().yaw();
        transformTobeMapped[3] = latestEstimate.translation().x();
        transformTobeMapped[4] = latestEstimate.translation().y();
        transformTobeMapped[5] = latestEstimate.translation().z();

• 把当前帧的点云的角点和面点拷贝出来，然后push_back到cornerCloudKeyFrames和surfCloudKeyFrames中保存起来，这里保存的是所有关键帧的点云，可以通过索引获取对应关键帧的点云

        // save all the received edge and surf points
        pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
        // 当前帧的点云的角点和面点分别拷贝一下
        pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);
        pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);

        // save key frame cloud
        // 关键帧的点云保存下来
        cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        surfCloudKeyFrames.push_back(thisSurfKeyFrame);

        // save path for visualization
        // 根据当前最新位姿更新rviz可视化
        updatePath(thisPose6D);