注：这是我在知乎写的文章，现搬运至此。原文链接：https://zhuanlan.zhihu.com/p/75492883
VINS-Fusion开源已经很长时间了，但是一直也没时间看。最近需要用到gps与VO融合，就先学习了global fusion节点。global fusion节点中数据融合的思路非常巧妙，令人赞叹，并且代码比较简单，很容易读懂。下面是一些学习源码的体会与大家分享，如有不当之处，欢迎批评指正～
1.整体流程分析
首先 global Estimator 节点订阅两个节点：
- 1.VIO输出的 nav_msgs::Odometry 类型消息，这个定位信息包含了VIO的位置和姿态，其坐标系原点位于VIO的第一帧处。
1 ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
- 2.GPS输出的sensor_msgs::NavSatFixConstPtr 类型消息，这个是全局定位信息，用经纬度来表示，其坐标原点位于该GPS坐标系下定义的0经度0纬度处。
1 ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
我们再分别看其回调函数(这里贴出来的代码只保留了主干部分)
先看GPS回调函数，很简单，只是把GPS消息存储到了队列里面
1 void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg) 2 { 3 //printf("gps_callback! \n"); 4 m_buf.lock(); 5 gpsQueue.push(GPS_msg); 6 m_buf.unlock(); 7 }
VIO回调函数，请看注释：
1 void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) 2 { 3 double t = pose_msg->header.stamp.toSec(); 4 last_vio_t = t; 5 // 获取VIO输出的位置(三维向量)，姿态(四元数) 6 Eigen::Vector3d vio_t; 7 Eigen::Quaterniond vio_q; 8 ...... 9 /// 位姿传入global Estimator中 10 globalEstimator.inputOdom(t, vio_t, vio_q); 11 m_buf.lock(); 12 // 寻找与VIO时间戳相对应的GPS消息 13 // 细心的读者可能会疑惑，这里需不需要对GPS和VIO进行硬件上的时间戳同步呢？ 14 // 这个问题请看总结与讨论 15 while(!gpsQueue.empty()) 16 { 17 // 获取最老的GPS数据和其时间 18 sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front(); 19 double gps_t = GPS_msg->header.stamp.toSec(); 20 // 10ms sync tolerance 21 // +- 10ms的时间偏差 22 if(gps_t >= t - 0.01 && gps_t <= t + 0.01) 23 { /// gps的经纬度，海拔高度 24 double latitude = GPS_msg->latitude; 25 double longitude = GPS_msg->longitude; 26 double altitude = GPS_msg->altitude; 27 // gps 数据的方差 28 double pos_accuracy = GPS_msg->position_covariance[0]; 29 if(pos_accuracy <= 0) 30 pos_accuracy = 1; 31 //printf("receive covariance %lf \n", pos_accuracy); 32 /// GPS_msg->status.status 这个数字代表了GPS的状态(固定解，浮点解等) 33 /// 具体可以谷歌 34 // if(GPS_msg->status.status > 8) 35 // 向globalEstimator中输入GPS数据 36 globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy); 37 gpsQueue.pop(); 38 break; 39 } 40 else if(gps_t < t - 0.01) 41 gpsQueue.pop(); 42 else if(gps_t > t + 0.01) 43 break; 44 } 45 m_buf.unlock(); 46 ...... 47 // 广播轨迹（略）...... 48 pub_global_odometry.publish(odometry); 49 pub_global_path.publish(*global_path); 50 publish_car_model(t, global_t, global_q); 51 // 位姿写入文本文件（略）...... 52 }
可以看出，global Fusion的优化策略是收到一帧VIO数据，就寻找相应的GPS数据来进行优化。我们下面主要来看一下globalEstimator中的inputOdom()和inputGPS()这两个函数。
首先看下 inputGPS():
1 void GlobalOptimization::inputGPS(double t, double latitude, 2 double longitude, 3 double altitude, 4 double posAccuracy) 5 { 6 double xyz[3]; 7 // 因为经纬度表示的是地球上的坐标，而地球是一个球形， 8 // 需要首先把经纬度转化到平面坐标系上 9 // 值得一提的是，GPS2XYZ()并非把经纬度转化到世界坐标系下(以0经度，0纬度为原点)， 10 // 而是以第一帧GPS数据为坐标原点，这一点需要额外注意 11 GPS2XYZ(latitude, longitude, altitude, xyz); 12 // 存入经纬度计算出的平面坐标，存入GPSPositionMap中 13 vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy}; 14 GPSPositionMap[t] = tmp; 15 newGPS = true; 16 }
扫描二维码关注公众号，回复：
7922767 查看本文章
再看inputOdom()：
1 void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ) 2 { 3 mPoseMap.lock(); 4 // 把vio直接输出的位姿存入 localPoseMap 中 5 vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 6 OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()}; 7 localPoseMap[t] = localPose; 8 Eigen::Quaterniond globalQ; 9 /// 把VIO转换到GPS坐标系下，准确的说是转换到以第一帧GPS为原点的坐标系下 10 /// 转换之后的位姿插入到globalPoseMap 中 11 globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ; 12 Eigen::Vector3d globalP = 13 WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3); 14 vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(), 15 globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()}; 16 globalPoseMap[t] = globalPose; 17 lastP = globalP; 18 lastQ = globalQ; 19 // 把最新的全局姿态插入轨迹当中(过程略) 20 ...... 21 global_path.poses.push_back(pose_stamped); 22 mPoseMap.unlock(); 23 }
现在两种数据都收到以后，万事俱备，我们看一下 void GlobalOptimization::optimize()这个函数：
这个函数开了一个线程来做优化(这个代码太长了，贴一部分把)：
- 首先使用ceres构建最小二乘问题，这个没啥可说的
- 状态量赋初值，添加参数块。可以看出来，迭代的初始值是globalPoseMap中的值，也就是VIO转换到GPS坐标系下的值。
1 int length = localPoseMap.size(); 2 // w^t_i w^q_i 3 double t_array[length][3]; 4 double q_array[length][4]; 5 map<double, vector<double>>::iterator iter; 6 iter = globalPoseMap.begin(); 7 for (int i = 0; i < length; i++, iter++) 8 { 9 t_array[i][0] = iter->second[0]; 10 t_array[i][1] = iter->second[1]; 11 t_array[i][2] = iter->second[2]; 12 q_array[i][0] = iter->second[3]; 13 q_array[i][1] = iter->second[4]; 14 q_array[i][2] = iter->second[5]; 15 q_array[i][3] = iter->second[6]; 16 problem.AddParameterBlock(q_array[i], 4, local_parameterization); 17 problem.AddParameterBlock(t_array[i], 3); 18 }
3.然后添加残差：
1 for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++) { 2 //vio factor 3 // 添加VIO残差，观测量是两帧VIO数据之差，是相对的。而下面的GPS是绝对的 4 iterVIONext = iterVIO; 5 iterVIONext++; 6 if (iterVIONext != localPoseMap.end()) { 7 /// 计算两帧VIO之间的相对差（略）...... 8 ceres::CostFunction *vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(), 9 iQj.w(), iQj.x(), iQj.y(), iQj.z(), 10 0.1, 0.01); 11 problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i + 1], t_array[i + 1]); 12 } 13 // gps factor 14 // GPS残差，这个观测量直接就是GPS的测量数据， 15 // 残差计算的是GPS和优化变量的差，这个是绝对的差。 16 double t = iterVIO->first; 17 iterGPS = GPSPositionMap.find(t); 18 if (iterGPS != GPSPositionMap.end()) { 19 ceres::CostFunction *gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1], 20 iterGPS->second[2], iterGPS->second[3]); 21 problem.AddResidualBlock(gps_function, loss_function, t_array[i]); 22 } 23 24 }
优化完成后，再根据优化结果更新姿态就ok啦。为了防止VIO漂移过大，每次优化完成还需要计算一下VIO到GPS坐标系的变换。
2.GPS与VIO融合策略
（知乎这个文章编辑器是真的卡........）
根据上文的分析，我们可以看出整体的优化策略和位姿图非常相似，因为观测量是相邻两帧VIO之间的差和GPS坐标，所以global Fusion 节点相当于把对应时间戳的GPS位姿和VIO位姿的差，均匀分布到每两个相邻的VIO之间。使得整体的误差和最小化。
3.总结与讨论
1.思考
根据上文中分析的优化策略，global fusion的应用场景应该是GPS频率较低，VIO频率较高的系统。fusion 默认发布频率位10hz，而现在的GPS可以达到20hz，如果在这种系统上使用，你可能还需要修改下VIO或者GPS频率。
2.GPS与VIO时间不同步
上文提到了，在多传感器融合系统中，传感器往往需要做时钟同步，那么global Fusion需要么？GPS分为为很多种，我们常见的GPS模块精度较低，十几米甚至几十米的误差，这种情况下，同不同步没那么重要了，因为GPS方差太大。另外一种比较常见的是RTK-GPS ，在无遮挡的情况下，室外精度可以达到 2cm之内，输出频率可以达到20hz，这种情况下，不同步时间戳会对系统产生影响，如果VIO要和RTK做松耦合，这点还需要注意。