VINS_FUSIOＮ意义

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

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

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

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

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

其算法框架如下：

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

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

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

$\mathbf{z}_t^{GPS}-h_t^{GPS}(X) = \mathbf{z}_t^{GPS}-h_t^{GPS}(\mathbf{x_t})=\mathbf{P}_t^{GPS}-\mathbf{P}_t^{w}$

其中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);//将时间、平移，四元数作为预测添加到globalEstimatorm_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 filestd::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坐标的计算过程，其公式如下：

$T_{GPS} = T_{GPS2VIO}*T_{VIO}$

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

$T_{GPS2VIO} = T_{body2GPS}^{-1}*T_{body2VIO}$

b. 融合优化

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

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

void GlobalOptimization::optimize()
{while(true){if(newGPS){newGPS = false;// ceres定义部分略去// add parammPoseMap.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++){// 取出数据部分省略// 添加了parameterblockproblem.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_WVIOWGPS_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做松耦合，这点还需要注意。

三、前后端详解

VINS_FUSION相关推荐

ROS kinetic 运行s_msckf和 vins_fusion
s_msckf:采用多状态约束的双目vio系统 !!!注意imuCallback:接收IMU数据,将IMU数据存到imu_msg_buffer中,这里只会利用开头200帧IMU数据进行静止初始化,不做 ...
Airsim中运行OpenVINS和VINS_Fusion
Airsim中运行OpenVINS和VINS_Fusion 1. 简介 2. 参考 3. 步骤 3.1 编译 3.2 运行 3.3 运行结果 3.4 相机和IMU参数配置 1. 简介本文简介在Air ...
VINS学习03——使用Realense_D435i运行Vins_Fusion
1. 简介上一篇跑了一下VINS_Fusion自带的demo,用的是几个常用的开源数据集,这篇文章主要是将VINS_Fusion用在自己的实验室设备上,在进行前期参数标定.config文件修改.精度 ...
VINS_FUSION编译运行
一.ROS安装见前文二.ceres安装 GitHub地址: GitHub - ceres-solver/ceres-solver: A large scale non-linear optimiz ...
[学习记录]realsence d455 +vins-fusion+px4+ego_planner下无人机的悬停与控制
写在前面:持续更新修改...... my env:ubuntu20 my pixhawk:2.4.8 my px4 firmware:1.9.0 stablepx4fmu-v2-1.6.0.px4v ...
Ubuntu20.04＋gdb/vscode调试ROS(VINS-Mono)程序
平台Ubuntu20.04 + ROS noetic 程序算法:VINS-mono 在阅读本文之前,建议先行了解基本的vscode调试工具与流程,以及如何安装vscode的ROS扩展,详情请参考博文: ...
VINS学习04———Omni教程
1. 本文简介本文依照港科大开源的代码和论文文章主要内容:对无人机集群实现协同定位.参与融合的定位因子有以下4点全向鱼眼相机的VIO定位:VINS-Fisheye 基于地图定位:视觉特征点协同建 ...
Intel Realsense T265使用教程
Intel Realsense T265使用教程 0 更新日志 1 T265参数 1.1 硬件参数 1.2 坐标系描述 2 T265 数据读取 2.1 环境安装 2.2 读取T265内外参数信息 2. ...
vins-fusion gps融合相关总结
1 . 简介: VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 相比于局部传感器,全局传感 ...

VINS_FUSION