VINS_FUSION

https://blog.csdn.net/qqGHJ/article/details/127440699

VINS_FUSIOＮ意义

 VINS Fusion在VINS Mono的基础上，添加了GPS等可以获取全局观测信息的传感器，使得VINS可以利用全局信息消除累计误差，进而减小闭环依赖。此外，全局信息可以使分多次运行的VINS Mono统一到一个坐标系，从而方便协同建图和定位。

局部传感器（如ＩＭＵ，相机，雷达等）被广泛应用与建图与定位算法。尽管这些传感器能在没有ＧＰＳ信息的区域，实现良好的局部定位和建图效果，但这些传感器只能提供局部观测，限制了其应用场景：

１、第一个问题是局部观测数据缺乏全局约束，当我们每次在不同的位置运行算法时，都会得到不同坐标系下的定位和建图结果，因而难以将这些测量结果结合起来，形成全局效果。

２、第二个问题时基于局部观测的估计算法必然存在累积漂移，尽管回环检测可以在一定程度上消除漂移，但是对于数据量较大的大范围场景，算法依然难以处理。

相比于局部传感器，全局传感器（如ＧＰＳ，气压计和磁力计等）可以提供全局观测，这些传感器使用全局统一坐标系，并且输出的观测数据的方差不随时间累积而增加，但这些传感器也存在一些问题，导致无法直接用于精确定位和建图，以ＧＰＳ为例，ＧＰＳ数据通常不平滑，存在噪声，且输出速率低，因此，一个简单而直观的想法是将局部传感器和全局传感器结合起来，以达到局部精确全局零漂的效果，也即是ＶＩＮＳ 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文件中，接口定义的关键代码如下：

    /*******************************************************
     * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
     *
     * This file is part of VINS.
     *
     * Licensed under the GNU General Public License v3.0;
     * you may not use this file except in compliance with the License.
     *
     * Author: Qin Tong (qintonguav@gmail.com)
         GPS和VIO数据输入
     *******************************************************/
    #include "ros/ros.h"
    #include "globalOpt.h"
    #include <sensor_msgs/NavSatFix.h>
    #include <nav_msgs/Odometry.h>
    #include <nav_msgs/Path.h>
    #include <eigen3/Eigen/Dense>
    #include <eigen3/Eigen/Geometry>
    #include <iostream>
    #include <stdio.h>
    #include <visualization_msgs/Marker.h>
    #include <visualization_msgs/MarkerArray.h>
    #include <fstream>
    #include <queue>
    #include <mutex>
     
    GlobalOptimization globalEstimator;
    ros::Publisher pub_global_odometry, pub_global_path, pub_car;
    nav_msgs::Path *global_path;
    double last_vio_t = -1;
    std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
    std::mutex m_buf;
     
    void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car)
    {
        visualization_msgs::MarkerArray markerArray_msg;
        visualization_msgs::Marker car_mesh;
        car_mesh.header.stamp = ros::Time(t);
        car_mesh.header.frame_id = "world";
        car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
        car_mesh.action = visualization_msgs::Marker::ADD;
        car_mesh.id = 0;
     
        car_mesh.mesh_resource = "package://global_fusion/models/car.dae";
     
        Eigen::Matrix3d rot;
        rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;
        
        Eigen::Quaterniond Q;
        Q = q_w_car * rot;
        car_mesh.pose.position.x    = t_w_car.x();
        car_mesh.pose.position.y    = t_w_car.y();
        car_mesh.pose.position.z    = t_w_car.z();
        car_mesh.pose.orientation.w = Q.w();
        car_mesh.pose.orientation.x = Q.x();
        car_mesh.pose.orientation.y = Q.y();
        car_mesh.pose.orientation.z = Q.z();
     
        car_mesh.color.a = 1.0;
        car_mesh.color.r = 1.0;
        car_mesh.color.g = 0.0;
        car_mesh.color.b = 0.0;
     
        float major_scale = 2.0;
     
        car_mesh.scale.x = major_scale;
        car_mesh.scale.y = major_scale;
        car_mesh.scale.z = major_scale;
        markerArray_msg.markers.push_back(car_mesh);
        pub_car.publish(markerArray_msg);
    }
     
    void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
    {
        //printf("gps_callback! \n");
        m_buf.lock();
        gpsQueue.push(GPS_msg); //每次接收到的gps消息添加到gpsqueue队列中
        m_buf.unlock();
    }
     
    void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
    {
        //printf("vio_callback! \n");
        double t = pose_msg->header.stamp.toSec();
        last_vio_t = t;
        Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);//平移矩阵
        Eigen::Quaterniond vio_q; //旋转四元阵
        vio_q.w() = pose_msg->pose.pose.orientation.w;
        vio_q.x() = pose_msg->pose.pose.orientation.x;
        vio_q.y() = pose_msg->pose.pose.orientation.y;
        vio_q.z() = pose_msg->pose.pose.orientation.z;
        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();
            printf("vio t: %f, gps t: %f \n", t, gps_t);
            // 10ms sync tolerance 找到vio里程数据相差在10ms之内的数据，作为匹配数据
            if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
            {
                //printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
                double latitude = GPS_msg->latitude;
                double longitude = GPS_msg->longitude;
                double altitude = GPS_msg->altitude;
                //int numSats = GPS_msg->status.service;
                double pos_accuracy = GPS_msg->position_covariance[0];
                if(pos_accuracy <= 0)
                    pos_accuracy = 1;
                //printf("receive covariance %lf \n", pos_accuracy);
                //if(GPS_msg->status.status > 8)
                    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();
     
        Eigen::Vector3d global_t;
        Eigen:: Quaterniond global_q;
        globalEstimator.getGlobalOdom(global_t, global_q);
     
        nav_msgs::Odometry odometry;
        odometry.header = pose_msg->header;
        odometry.header.frame_id = "world";
        odometry.child_frame_id = "world";
        odometry.pose.pose.position.x = global_t.x();
        odometry.pose.pose.position.y = global_t.y();
        odometry.pose.pose.position.z = global_t.z();
        odometry.pose.pose.orientation.x = global_q.x();
        odometry.pose.pose.orientation.y = global_q.y();
        odometry.pose.pose.orientation.z = global_q.z();
        odometry.pose.pose.orientation.w = global_q.w();
        pub_global_odometry.publish(odometry);
        pub_global_path.publish(*global_path);
        publish_car_model(t, global_t, global_q);
     
     
        // write result to file
        std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC.precision(0);
        foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
        foutC.precision(5);
        foutC << global_t.x() << ","
                << global_t.y() << ","
                << global_t.z() << ","
                << global_q.w() << ","
                << global_q.x() << ","
                << global_q.y() << ","
                << global_q.z() << endl;
        foutC.close();
    }
     
    int main(int argc, char **argv)
    {
        ros::init(argc, argv, "globalEstimator");
        ros::NodeHandle n("~");
     
        global_path = &globalEstimator.global_path;
     
        ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
        ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
        pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
        pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
        pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
        ros::spin();
        return 0;
    }

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, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
        vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
                                  globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
        globalPoseMap[t] = globalPose;
    }
     
    // 接收上面输入的gps数据
    void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
    {
        double xyz[3];
        GPS2XYZ(latitude, longitude, altitude, xyz); // 将GPS的经纬度转到地面笛卡尔坐标系
        vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
        GPSPositionMap[t] = tmp;
        newGPS = true;
    }

上面出现了VIO数据的局部坐标转到GPS坐标的计算过程，其公式如下：

这个公式中的GPS2VIO出现在后面的优化过程中，计算方法为：

b. 融合优化

这里是融合的关键代码，可以看出其流程如下：

    构建t_array和q_array，用来存入平移和旋转变量，方便输入优化方程，以及在优化后取出。
    利用RelativeRTError::Create()构建VIO两帧之间的约束，输入优化方程
    利用TError::Create()构建GPS构成的全局约束，输入优化方程
    取出优化后的数据

    void GlobalOptimization::optimize()
    {
        while(true)
        {
            if(newGPS)
            {
                newGPS = false;
                // ceres定义部分略去
                // add param
                mPoseMap.lock();
                int length = localPoseMap.size();
                // ********************************************************
                // ***  1. 构建t_array, q_array用来存优化变量，等优化后取出  ***
                // ********************************************************
                double t_array[length][3];
                double q_array[length][4];
                map<double, vector<double>>::iterator iter;
                iter = globalPoseMap.begin();
                for (int i = 0; i < length; i++, iter++)
                {
                    // 取出数据部分省略
                    // 添加了parameterblock
                    problem.AddParameterBlock(q_array[i], 4, local_parameterization);
                    problem.AddParameterBlock(t_array[i], 3);
                }
     
                map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
                int i = 0;
                for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
                {
                    // ********************************************************
                    // *********************   2. VIO约束   *******************
                    // ********************************************************
                    iterVIONext = iterVIO;
                    iterVIONext++;
                    if(iterVIONext != localPoseMap.end())
                    {
                        Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity(); // 第i帧的变换矩阵
                        Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity(); // 第j帧的变换矩阵
                        // 取出数据部分省略
                        Eigen::Matrix4d iTj = wTi.inverse() * wTj; // 第j帧到第i帧的变换矩阵
                        Eigen::Quaterniond iQj; // 第j帧到第i帧的旋转
                        iQj = iTj.block<3, 3>(0, 0);
                        Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);  // 第j帧到第i帧的平移
     
                        ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
                                                                                    iQj.w(), iQj.x(), iQj.y(), iQj.z(),
                                                                                    0.1, 0.01);
                        problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
                    }
                     
                    // ********************************************************
                    // *********************   3. GPS约束   *******************
                    // ********************************************************
                    double t = iterVIO->first;
                    iterGPS = GPSPositionMap.find(t);
                    if (iterGPS != GPSPositionMap.end())
                    {
                        ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
                                                                           iterGPS->second[2], iterGPS->second[3]);
                        problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
                    }
     
                }
                ceres::Solve(options, &problem, &summary);
                 
                // ********************************************************
                // *******************   4. 取出优化结果   *****************
                // ********************************************************
                iter = globalPoseMap.begin();
                for (int i = 0; i < length; i++, iter++)
                {
                    // 取出优化结果
                    vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
                                              q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
                    iter->second = globalPose;
                    if(i == length - 1)
                    {
                        Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
                        Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
                        // 剩余的存入部分省略，得到WGPS_T_WVIO
                        WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
                    }
                }
                updateGlobalPath();
                mPoseMap.unlock();
            }
            std::chrono::milliseconds dura(2000);
            std::this_thread::sleep_for(dura);
        }
        return;
    }

上述代码中出现了一个关键的部分，即WGPS_T_WVIO的计算。从之前的代码中知道，这个4*4矩阵是用来做VIO到GPS坐标系的变换的。按道理讲，这个矩阵应当是不需要反复计算的，毕竟第一帧和GPS的坐标变换是确定的。但是运行结果来看，这个矩阵的值是在变化的，而且有时变化比较大。这里不太懂，知乎刘知说是为了避免VIO漂移过大。
3. 总结(摘自知乎刘知)

    使用场景

根据上文中分析的优化策略，global fusion的应用场景应该是GPS频率较低，VIO频率较高的系统。fusion 默认发布频率位10hz，而现在的GPS可以达到20hz，如果在这种系统上使用，你可能还需要修改下VIO或者GPS频率。另外，使用的GPS是常见的误差较大的GPS，并不是RTK-GPS。
    2. GPS与VIO时间不同步

上文提到了，在多传感器融合系统中，传感器往往需要做时钟同步，那么global Fusion需要么？GPS分为为很多种，我们常见的GPS模块精度较低，十几米甚至几十米的误差，这种情况下，同不同步没那么重要了，因为GPS方差太大。另外一种比较常见的是RTK-GPS ，在无遮挡的情况下，室外精度可以达到 2cm之内，输出频率可以达到20hz，这种情况下，不同步时间戳会对系统产生影响，如果VIO要和RTK做松耦合，这点还需要注意。
三、前后端详解