VINS-FUSION GPS融合坐标转换细节分析

先贴两个对GPS融合的代码结构分析的博文，感谢博主们的分享。

https://zhuanlan.zhihu.com/p/75492883

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

希望读者先看看整体的代码结构，本文不再赘述，本文主要想把想到的GPS融合时坐标转换的一些细节分享给大家。

GPS坐标系和vio坐标系的转换。

在做多传感器融合的时候必然会涉及到传感器间坐标系的不同，需要一个外参，比如视觉和imu的外参。GPS和vio融合同样如此有一个GPS到vio结果的外参。构造函数里把GPS和vio的外参初始化为一个单位矩阵。在最初看代码的时候对其十分不解，单位矩阵也就意味两者间的坐标系完全重合，这怎么能保证。

GlobalOptimization::GlobalOptimization()
{initGPS = false;newGPS = false;WGPS_T_WVIO = Eigen::Matrix4d::Identity();threadOpt = std::thread(&GlobalOptimization::optimize, this);
}

外参初始化为单位矩阵

gps原始数据经纬高，转为xyz用的是第三方库函数GeographicLib里的LocalCartesian::Forward函数。

void GlobalOptimization::GPS2XYZ(double latitude, double longitude, double altitude, double* xyz)
{if(!initGPS){geoConverter.Reset(latitude, longitude, altitude);initGPS = true;}geoConverter.Forward(latitude, longitude, altitude, xyz[0], xyz[1], xyz[2]);//第三方库转换gps为xyz//printf("la: %f lo: %f al: %f\n", latitude, longitude, altitude);//printf("gps x: %f y: %f z: %f\n", xyz[0], xyz[1], xyz[2]);
}

那么转换后的xyz是在哪个坐标系下的xyz呢？查一下官方文档：

Local cartesian coordinates.

Convert between geodetic coordinates latitude = lat, longitude = lon, height = h (measured vertically from the surface of the ellipsoid) to local cartesian coordinates (x, y, z). The origin of local cartesian coordinate system is at lat = lat0, lon = lon0, h = h0. The z axis is normal to the ellipsoid; the y axis points due north. The plane z = - h0 is tangent to the ellipsoid.
是Y轴指向正北的局部笛卡尔坐标系。所以GPS坐标系是固定方向的，而vio的坐标系是和你的启动时设备朝向有关。所以它们间的外参WGPS_T_WVIO不是个固定值，是变化的，那么怎么解决的这个外参问题？

下面以实际vio和gps数据传入处理思路，来解释和分析这个问题。

我们先考虑情况：代码里gps数据和vio数据必须同步在10ms内，才认为两者同步，用vio的时间赋给gps数据。可是GPS频率一般10HZ，能和vio数据同步需要一定的偶然。所以我们考虑刚开始，gps数据一直无法同步时，代码是怎么样运行的。

此时的vio_callback后续的时间同步判断满足不了，只会运行如下代码，后续把同步的gps数据传入GPSbuffer：GPSPositionMap不会执行。

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);//咋叫vio_t，和last_vio_t（时间）有歧义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);//把VIO结果时间、位置，姿态输入，做坐标转换，数据存储进相应buffer

再看一下inputOdom函数，这个函数把vio的结果存储在localPoseMap中，然后使用外参 WGPS_T_WVIO把VIO的结果转换到GPS坐标系下存储在globalPoseMap中。注意，此时我们考虑刚开始gps没对齐时，此时外参WGPS_T_WVIO是单位矩阵，所以globalPoseMap里的位姿和VIO的结果一样。globalPoseMap是存储融合优化后的位姿的，这也符合逻辑：在没有gps数据时，融合结果完全和VIO一样。

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{mPoseMap.lock();vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};localPoseMap[t] = localPose;//一个buffer,存储时间和位姿，此用法又区别于数组，t为double类型。//把vio的结果用外参转换到GPS坐标系下。globalQ，globalP，并存储在globalPoseMap里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;lastP = globalP;//VIO结果经坐标转换给了lastPlastQ = globalQ;//把vio转换坐标系后的结果赋值给global_path，给最新传入的一个初始值。geometry_msgs::PoseStamped pose_stamped;pose_stamped.header.stamp = ros::Time(t);pose_stamped.header.frame_id = "world";pose_stamped.pose.position.x = lastP.x();pose_stamped.pose.position.y = lastP.y();pose_stamped.pose.position.z = lastP.z();pose_stamped.pose.orientation.x = lastQ.x();pose_stamped.pose.orientation.y = lastQ.y();pose_stamped.pose.orientation.z = lastQ.z();pose_stamped.pose.orientation.w = lastQ.w();global_path.header = pose_stamped.header;global_path.poses.push_back(pose_stamped);mPoseMap.unlock();
}

接下来看一下，第一个同步的gps传入是怎么融合vio结果的。此时，在vio_callback函数里如下代码执行，通过inputGPS函数把gps数据存储在GPSPositionMap中（input函数里面）。同时在inputGPS函数中把newGPS = true;

if(gps_t >= t - 0.01 && gps_t <= t + 0.01)//因为VIO的输出频率，对这个函数考虑到20hz的运行频率，此处的0.01s意味着只取到了vio结果附近0.01s的GPS结果，其他的GPS数据还在原始buff里面。而vio附近的结果存储在了GPSPositionMap里面{//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);//input后，优化线程因为有NEW_GPS就开始优化了gpsQueue.pop();break;//此处break,意味只存储了一个GPS数据后就break了。后来想明白了GPS不同于imu，是绝对位置，只需要某一时刻的数据。}

而newGPS置为true后会激活optimize函数里面的优化。而ceres优化里给GPS数据和vio数据定义的残差对于理解坐标系转换至关重要。

void GlobalOptimization::optimize()
{while(true){if(newGPS)//必须得有符合要求的gps数据传入

在factor.h文件中，首先是GPS的数据和状态量定义的残差：状态量位置-gps算出来的位置。

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;}

而VIO的数据和状态量的残差定义为：（j时刻的状态量-i时刻的状态量）得到的增量-vio增量。其意味着融合后的位置必须和GPS位置尽可能重合，而两帧间的增量要和VIO尽可能相等。这对理解坐标转换至关重要，这样的处理意味着vio的数据并不对融合后的绝对位置做约束，只要求融合后的位置增量和vio的增量误差尽可能小。所以融合后的位置会在GPS坐标系下。

bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const//输入的状态量两个时刻的LIO的位姿{T t_w_ij[3];//世界坐标系下ij帧的位置增量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);T t_i_ij[3];//i帧坐标系下，ij帧的位置增量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];//传入观测的四元数增量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;}

在优化求解完成以后，会遍历globalPoseMap,对其更新。即globalPoseMap里面存储着优化后的位姿。同时，之后会用在VIO坐标系下的位置（localPoseMap里）和优化后在GPS坐标系下的位置（globalPose）对外参WGPS_T_WVIO进行更新。还是考虑第一次同步的gps数据传入，优化完成后。外参WGPS_T_WVIO之前是单位矩阵，而第一次更新，GPS传入的经纬高会被转化为局部坐标系下的xyz，因为是第一个GPS所以一定是转化为（0,0,0），而此时的vio坐标已经跑了一会：例如为（2,0,0）第一次更新会修正WGPS_T_WVIO的距离，旋转则要第二个同步的gps数据传入才会修正。

 ceres::Solve(options, &problem, &summary);//std::cout << summary.BriefReport() << "\n";// update global pose//mPoseMap.lock();//再次遍历全局地图，用优化后的量对齐进行更新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)//用最新的位姿对 WGPS_T_WVIO更新{Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity(); Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();double t = iter->first;WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4], localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4], globalPose[5], globalPose[6]).toRotationMatrix();WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();}}updateGlobalPath();

值得一提的是获取WGPS_T_WVIO的公式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()使得误差包含在里面了。

VINS-FUSION GPS融合坐标转换细节分析相关推荐

VINS Fusion GPS融合部分
概述 VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 局部传感器(如相机,IMU,激光雷达 ...
VINS - Fusion GPS/VIO 融合二、数据融合
https://zhuanlan.zhihu.com/p/75492883 一.简介源代码:VINS - Fusion 数据集:KITTI 数据程序入口:globalOptNode.cpp 二.程 ...
vins中imu融合_VINS-Mono代码分析与总结(最终版)
VINS-Mono代码分析总结参考文献前言 ??视觉与IMU融合的分类: 松耦合和紧耦合:按照是否把图像的Feature加入到状态向量区分,换句话说就是松耦合是在视觉和IMU各自求出的位姿的基础上 ...
vins中imu融合_双目版 VINS 项目发布，小觅双目摄像头作为双目惯导相机被推荐...
继 VINS-Mono 和 VINS-Mobile 之后,香港科技大学沈劭劼老师实验室正式发布了双目版 VINS 开源项目 VINS-Fusion .项目推荐了小觅双目摄像头标准版(MYNT EYE ...
vins中imu融合_小觅智能 | VINS 学习笔记（持续更新中）
VINS 基本介绍 VINS-Mono 和 VINS-Mobile 是香港科技大学沈劭劼老师开源的单目视觉惯导 SLAM 方案.2017年发表于<IEEE Transactions on Rob ...
vins-fusion gps融合相关总结
1 . 简介: VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 相比于局部传感器,全局传感 ...
小觅相机D系列跑vins fusion
有幸使用一款D系列小觅相机.今天测试了下它跑vins fusion的表现.其中有一个注意点,官方文档没有说明.这里记录一下. D系列小觅相机添加了IR散斑投射器,用来恢复没有纹理细节时的深度.这与老版 ...
vins中imu融合_基于非线性优化算法—当视觉SLAM遇到VINS会碰撞出怎样的火花？
今天来给大家分享一个视觉SLAM中比较综合且复杂的系统-VINS.VINS旨在通过融合两个传感器测量数据获得移动机器人的位姿和特征点在空间中的位置,在现代控制理论学科中属于最优估计问题.在移动智能机器 ...
GPS接收机的灵敏度分析
转自 http://blog.sina.com.cn/s/blog_4cd5dc1c0100yw2l.html GPS接收机的灵敏度分析 (2011-12-13 12:27:44) 转载▼ 摘要:GP ...

VINS-FUSION GPS融合坐标转换细节分析

VINS-FUSION GPS融合坐标转换细节分析相关推荐

最新文章

热门文章