【作业记录-引入GPS约束】

前言

学习SLAM ing 看代码讲解经常走神，只看原理又浮在表面，菜鸡无能狂怒啊！

一、说明

        这是苏赟老师在视觉工坊的课程作业，将作业讲解在这里浅浅做个记录。仅供学习交流，侵删。作业是老师改的代码框架然后挖空的形式，挺用心的，有需要作业说明和代码包的评论下我看到了会私发bdu云链接，就不在这里发出来了，感觉不太好。

        问题：基于所给的 SLAM 代码框架，完成 GPS 因子构建函数，利用 GPS 测量 构建绝对位置约束，并添加到因子图中。

二、解决思路

         先看GPS的回调函数，GPS主要用xyz的位置信息，GPS测量的是地球绝对的经纬度，值可能会很大，所以记录一个初始的gps值，然后后面的值都减去这个值，即把这个初始处当做使用的坐标系，避免大数值。

        记录上一个时刻GPS数据  做一个时间同步，小于200毫秒，就扔掉，大于200毫秒，认为数据还没到，要等，正负200毫秒之间，认为时间合适留下 拿出xyz坐标信息，给到一个GPS点的坐标结构里，判断是否大于三米（不用频繁添加GPS点），因为比如像漂移到了五六米，用一个GPS去矫正才有意义，因为GPS本身精度就不高，比如空旷环境下可能是一到三米，所以等漂移大了再矫正就行。这里用的判断条件是两个GPS点之间差了三米，也可以用别的，比如漂移误差这种。 这里把噪声设置成1.5米，代表当前GPS传感器测量的误差，代表优化权重，如果设置太小，倒数为优化权重会很大，结果会很偏向于这个传感器的测量结果。  将GPS观测构件成一个GPS的factor，已经有一个实现好了的factor，然后也有对应的引入约束模型直接用就好了。

代码如下：

  void addGPSFactor()
  {
    if (gpsQueue.empty())
      return;

    // wait for system initialized and settles down
    if (cloudKeyPoses3D->points.empty())

      return;
    //机器人运动距离小于两米，也不添加约束
    if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 2.0)
      return;

    // 作业：
    // TODO: 完成GPS因子构建函数，利用GPS测量构建绝对位置约束，并添加到因子图中；
    // start:

    bool GPSFactor = false;

    //上一个GPS位置
    static PointType lastGPSPoint;

    while (!gpsQueue.empty())
    {
      if (gpsQueue.front().header.stamp.toSec()<timeLaserInfoCur - 0.2)
      {
        //太旧了不用,弹出队列
        gpsQueue.pop_front();
      }
      else if(gpsQuene.front().header.stamp.toSec()>timeLaserInfoCur + 0.2)
      {
        //太新了雷达那边数据还没来,要等待
        break;
      }
      else
      {
        nav_msgs::Odometry thisGPS = gpsQueue.front();
        gpsQueue.pop_front();

        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
        PointType curGPSPoint;
        curGPSPoint.x = gps_x;
        curGPSPoint.y = gps_y;
        curGPSPoint.z = gps_z;

        if(pointDistance(curGPSPoint,lastGPSPoint)<3.0)
          continue;
        else
          lastGPSPoint = curGPSPoint;

        gtsam::Vector Vector3(3);
        Vector3<< 1.5 , 1.5 , 1.5;
        noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
        gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
        gtSAMgraph.add(gps_factor);

        GPSFactor = true;

        break;

      }
    }
    

    // gtSAMgraph.add(gtsam::GPSFactor);

    if (GPSFactor == true)
      aLoopIsClosed = true;

    // end.
  }

---

总结

在具体构建残差，进行约束这块直接调用函数了，这一块代码感觉还挺重要的，将会继续深入学习。