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); // 将时间，平移，四元数作为预测添加进globalEstimatorm_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, 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. 融合优化

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

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

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坐标系的变换的。

具体的程序在 Factors.h 里面的结构体，如下：

struct RelativeRTError
{//结构体初始化RelativeRTError(double t_x, double t_y, double t_z, double q_w, double q_x, double q_y, double q_z,double t_var, double q_var):t_x(t_x), t_y(t_y), t_z(t_z), q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),t_var(t_var), q_var(q_var){}//构建代价函数template <typename T>bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const{T t_w_ij[3]; //VIO和GPS优化后，世界坐标系 i、j帧的位置增量t_w_ij[0] = tj[0] - ti[0];t_w_ij[1] = tj[1] - ti[1];t_w_ij[2] = tj[2] - ti[2];T i_q_w[4]; //i帧的四元数逆QuaternionInverse(w_q_i, i_q_w);//世界坐标系下，i、j帧的位置增量t_w_ij，经i_q_w 转换到i帧的坐标系，与 t_x 求差！T t_i_ij[3];ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);//四元素旋转，得到姿态//残差定义为增量差residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);T relative_q[4]; //传入VIO观测的四元数增量relative_q[0] = T(q_w);relative_q[1] = T(q_x);relative_q[2] = T(q_y);relative_q[3] = T(q_z);T q_i_j[4]; //状态量计算的四元数增量ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);T relative_q_inv[4];QuaternionInverse(relative_q, relative_q_inv);T error_q[4]; //状态量计算的增量乘上测量量的逆，定义了残差ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); residuals[3] = T(2) * error_q[1] / T(q_var);residuals[4] = T(2) * error_q[2] / T(q_var);residuals[5] = T(2) * error_q[3] / T(q_var);return true;}static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,const double q_w, const double q_x, const double q_y, const double q_z,const double t_var, const double q_var) {return (new ceres::AutoDiffCostFunction<RelativeRTError, 6, 4, 3, 4, 3>(new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));}double t_x, t_y, t_z, t_norm;double q_w, q_x, q_y, q_z;double t_var, q_var;};

注意：哪些是VIO的两帧间的变化，哪些是优化后 globalPoseMap 的位姿态！

思路：VIO和GPS优化后的POS增量投影到 VIO下，然后和VIO下的POS 增量作差，求残差！

我以前看这些代码很迷糊，但是你把Ceres熟悉之后，在明白VIO和GPS的优化原理，就很简单，下图是示意图：
VIO的数据和状态量的残差定义为：（j时刻的状态量 - i时刻的状态量）得到的增量-vio增量; 其意味着融合后的位置必须

和GPS位置尽可能重合，而两帧间的增量要和VIO尽可能相等。这对理解坐标转换至关重要，这样的处理意味着vio的数据

并不对融合后的绝对位置做约束，只要求融合后的位置增量和vio的增量误差尽可能小, 所以融合后的位置会在GPS坐标系下。

再来看一下 struct TError 结构体：

struct TError
{//定义数据的 t_x 、t_y 、 t_z 和 var 的析构函数TError(double t_x, double t_y, double t_z, double var):t_x(t_x), t_y(t_y), t_z(t_z), var(var){}template <typename T>bool operator()(const T* tj, T* residuals) const{residuals[0] = (tj[0] - T(t_x)) / T(var);residuals[1] = (tj[1] - T(t_y)) / T(var);residuals[2] = (tj[2] - T(t_z)) / T(var);return true;}//损失函数static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var) {return (new ceres::AutoDiffCostFunction<TError, 3, 3>(new TError(t_x, t_y, t_z, var)));}double t_x, t_y, t_z, var;};

理解：根据程序可知，残差是：VIO/GPS融合后的位置（优化变量） - gps观测值，到此，优化部分结束！

注意：

WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse()

这导致了WGPS_T_WVIO这个外参包括了VIO计算的误差。什么意思呢，本来当vio足够精准时，

这样算出的WGPS_T_WVIO肯定是一个固定值。实际把WGPS_T_WVIO输出看，会随着时间一直变化。

变化原因是因为vio的结果和融合后的结果均带误差，WGPS_T_body * WVIO_T_body.inverse() 使得误差包含在里面了。

总结

优化流程：

首先呢，判断是否有gps数据，整体的算法就是在ceres架构下的优化算法。
所以总体的步骤就是ceres优化的步骤，首先添加优化参数块（AddParameterBlock函数），参数为globalPoseMap中的6维位姿（代码中旋转用四元数表示，所以共7维）。
之后添加CostFunction，通过构建factor重载operator方法实现（具体实现需要学习ceres库）。该部分有两个factor，一个是位姿图优化，另一个则是利用gps进行位置约束。
将factor添加后，进行ceres求解，更新此时gps和vio间的坐标系转换参数，之后再利用updateGlobalPath函数更新位姿。

https://blog.csdn.net/weixin_41843971/article/details/86748719

残差项：

接着开始添加残差项，总共有两个残差项分别是：vio factor以及gps factor

– vio factor：

残差项的costfunction创建由 relativeRTError来提供。观测值由vio的结果提供。此时计算的是以i时刻作为参考，从i到j这两个时刻的位移值以及四元数的旋转值作为观测值传递进入代价函数中。此时iPj代表了i到j的位移，iQj代表了i到j的四元数变换。添加残差项的时候，需要添加当前i时刻的位姿以及j时刻的位姿。即用观测值来估计i时刻的位姿以及j时刻的位姿。

– gps factor：

残差项的costfunction创建由 TError来提供。观测值由Gps数据的结果提供。添加残差项的时候，只需要添加当前i时刻的位姿。

TError 及 RelativeRTError

直观上理解：
{0, 1，2，3，4, 5，6…}
要估计出这些时刻，每个时刻的位姿。
我有的是两个方面的观测值，一方面是GPS得到的每个时刻的位置（x,y,z）（并且GPS信号可以提供在该时刻得到这个位置的精度posAccuracy），没有累计误差，精度较低。另一方面是VIO得到的每个时刻的位置（x,y,z）以及对应的姿态四元数（w,x,y,z）,存在累计误差，短时间内精度较高。为了得到更好的一个融合结果，因此我们采用这个策略：观测值中，初始位置由GPS提供，并且vio观测值信任的是i到j时刻的位移以及姿态变化量。并不信任vio得到的一个绝对位移量以及绝对的旋转姿态量。只信任短期的i到j的变化量，用这个变化量作为整个代价函数的观测值，进行求解。
因此两个残差项TError及RelativeRTError分别对应的就是GPS位置信号以及vio短时间内估计的i到j时刻的位姿变化量对最终结果的影响。迭代求解的过程中均采用了AutoDiffCostFunction自动求解微分来进行迭代。
（1）TError
TError（x,y,z,accuracy），最后一项是定位精度，可以由GPS系统提供。残差除了直接观测值与真值相减以外，还除了这个accuracy作为分母。意味着精度越高，accuracy越小，对结果的影响就越大。
（2）RelativeRTError
RelativeRTError（dx,dy,dz,dqw,dqx,dqy,dqz，t_var,q_var），最后两项是位移以及旋转之间的权重分配比例，并且为了使得与TError对应。在程序中，应该是根据经验把最后两项设置成一个常值，分别对应0.1以及0.01。残差的得到就根据实际值与观测值之间的偏差直接得出。

原文链接：https://blog.csdn.net/huanghaihui_123/article/details/87183055

Local Factor: 局部观测约束,VIO相对位姿变换, 计算的是相邻两帧之间位姿的残差