VINS Fusion GPS融合部分

概述

VINS Fusion在VINS Mono的基础上，添加了GPS等可以获取全局观测信息的传感器，使得VINS可以利用全局信息消除累计误差，进而减小闭环依赖。

局部传感器(如相机，IMU，激光雷达等)被广泛应用于建图和定位算法。尽管这些传感器能够在没有GPS信息的区域，实现良好的局部定位和局部建图效果，但是这些传感器只能提供局部观测，限制了其应用场景：

第一个问题是局部观测数据缺乏全局约束，当我们每次在不同的位置运行算法时，都会得到不同坐标系下的定位和建图结果，因而难以将这些测量结果结合起来，形成全局效果。
第二个问题是基于局部观测的估计算法必然存在累计漂移，尽管回环检测可以一定程度上消除漂移，但是对于数据量较大的大范围场景，算法依然难以处理。

相比于局部传感器，全局传感器(如GPS，气压计和磁力计等)可以提供全局观测。这些传感器使用全局统一坐标系，并且输出的观测数据的方差不随时间累计而增加。但这些传感器也存在一些问题，导致无法直接用于精确定位和建图。以GPS为例，GPS数据通常不平滑，存在噪声，且输出速率低。

因此，一个简单而直观的想法是将局部传感器和全局传感器结合起来，以达到局部精确全局零漂的效果。这也就是VINS Fusion这篇论文的核心内容。

融合原理

VINS Fusion的算法架构如图所示：

下图以因子图的方式表示观测和状态之间的约束：

其中圆形为状态量(如位姿，速度，偏置等)，黄色正方形为局部观测的约束，即来自VO/VIO的相对位姿变换；而其他颜色的正方形为全局观测的约束，比如紫色正方形为来自GPS的约束。

局部约束(残差)的构建参考vins mono论文，计算的是相邻两帧之间的位姿残差。这里只讨论GPS带来的全局约束。首先当然是将GPS坐标，也就是经纬度转换为大地坐标系。习惯上选择右手坐标系，x,y,z轴正方向分别是北东地或东北天方向。接下来就可以计算得到全局约束的残差：

其中z为GPS测量值，X为状态预测，h方程为观测方程。X可以通过上一时刻状态以及当前时刻与上一时刻的位姿变换计算出来。具体到本文的方法，就是利用VIO得到当前时刻和上一时刻的相对位姿dX，加到上一时刻的位姿上X(i-1)，得到当前时刻的位姿Xi。需要注意的是，这里的X以第一帧为原点。通过观测方程h，将当前状态的坐标转换到GPS坐标系下。这样就构建了一个全局约束。

之后的优化就交给BA优化器进行迭代优化，vins fusion沿用了ceres作为优化器。

代码实现

1. GPS和VIO数据输入

需要明确的一点是，VIO的输出是相对于第一帧的累计位姿变换，也就是以第一帧为原点。VINS Fusion接收vio输出的局部位姿变换(相对于第一帧)，以及gps输出的经纬度坐标，进行融合。接受数据输入的接口在global_fusion/src/globaOptNode.cpp文件中，接口定义的关键代码如下：

void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
   
    m_buf.lock();
    gpsQueue.push(GPS_msg); // 每次接收到的gps消息添加到gpsQueue队列中
    m_buf.unlock();
}

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
   
    double t = pose_msg->header.stamp.toSec();
    last_vio_t = t;
    Eigen::Vector3d vio_t; // 平移矩阵，转换的代码省略
    Eigen::Quaterniond vio_q; // 旋转四元数，转换的代码省略
    globalEstimator.inputOdom(t, vio_t, vio_q); // 将时间，平移，四元数作为预测添加进globalEstimator

    m_buf.lock();
    while(!gpsQueue.empty())
    {
   
        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_t = GPS_msg->header.stamp.toSec();

        // 找到和vio里程计数据相差在10ms以内的gps数据，作为匹配数据
        if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
        {
   
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            double pos_accuracy = GPS_msg->position_covariance[0];
            globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy); // 关键，将gps数据作为观测输入到globalEstimator中
            gpsQueue.pop(); // 满足条件的gps信息弹出
            break;
        }
        else if(gps_t < t - 0.01)
            gpsQueue.pop(); // 将过时的gps信息弹出
        else if(gps_t > t + 0.01)
            break; // 说明gps信息是后来的，就不要改动gps队列了，退出
    }
    m_buf.unlock();
    // ...其余省略
}

2. GPS和VIO融合

VINS Fusion融合GPS和VIO数据的代码在global_fusion/src/globalOpt.cpp文件中，下面进行详细介绍。

a. 接收GPS数据，接收VIO数据并转到GPS坐标系

// 接收上面输入的vio数据
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
   
	vector<double> localPose{
   OdomP.x(), OdomP.y(), OdomP.z(), 
    					     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;
    // 利用vio的局部坐标进行坐标变换，得到当前帧的全局位姿
    Eigen::Quaterniond globalQ;
    globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
    Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0