转载自：https://blog.csdn.net/hltt3838/article/details/110148851
GPS/INS/视觉 融合 、 自己采集数据测试
贵在坚持，不忘初心 2020-11-25 22:13:37 376 收藏 4
分类专栏： VINS-Mono and Fusion 程序解读
版权
cd /VIO_GPS/MapVIG_cmake/build
cmake ..
make
./run_MapVIG //别忘了更新一下
一、运行程序
打开第一个终端
roscore
打开第二个终端
进入工作区间内，分别输入：
cd GPS_Stereo_Ins/catkin_ws
catkin_make //单独一个端口，编译后关闭，没有修改程序不用编译，可以用于检查程序问题
source devel/setup.bash
roslaunch vins vins_rviz.launch
打开第三个终端
进入工作区间内，分别输入：
cd GPS_Stereo_Ins/catkin_ws
source devel/setup.bash
rosrun vins wuda_imu_camera_gps src/VINS-Fusion/config/test_data/wuda_imu_camera_gps.yaml /media/hltt3838/DATA/wuda_data/
//数据放在了D盘，不是Linux下的文件
打开第四个终端
进入工作区间内，分别输入：
cd GPS_Stereo_Ins/catkin_ws
source devel/setup.bash
rosrun global_fusion global_fusion_node
opt_R_w2vio<< 0.561189,-0.823137, -0.606892,
0.677156, 0.003792, 0.796881,
-0.663336,-0.701721, 0.056534;
注意事项
任意两个坐标系之间的转换矩阵求解：
https://blog.csdn.net/thequitesunshine007/article/details/106123617/
（0）IMU坐标系和 载体坐标系的转换
假设小车的坐标系如下所示，是 前-左-上； 而IMU在 小车上的方向是：前-右-下；
那么，要IMU要转换成小车的 前-左-上，需要对原始数据做一下坐标转换，简单的说就是把 [x y z ] 三轴数据变为 [x -y -z]
记住啦，是IMU和小车之间的转换关系！ 别忘了，相机和IMU之间的外参也需要转换！
（1）相机的内参不准确，写函数进行优化；
（2）IMU的零偏参数不要用 原始数据给的，用KITTI .yaml 里面的IMU参数，或者加入优化俄参数
（3）KITTI 数据中，VIO和GPS融合后结果也很好，但是自己采集的数据结果不好，看看区别；
一方面是自己采集的数据的GPS数据是1HZ， 二方面，自己采集的GPS数据是纬度、经度、高度，看看与KITTI数据的区别！
问题出在，KITTI 数据中， GPS的频率和图像是一样的，而自己的采集的RTK数据是1HZ，图像数据为20HZ,
在gloal 的时候需要 调整一下 频率！
后续，自己的猜想是对的，把容忍时间 0.01s 改成 0.05s 即可！ 如下所示：
程序有很多要学习的地方，学习一下人家的这种思路！
m_buf.lock();
// 寻找与VIO时间戳相对应的GPS消息
while(!gpsQueue.empty())//有GPS数据时执行
{
// 获取最早的GPS数据和其时间
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);
// +- 50ms的时间偏差,根觉自己采集数据而定
if(gps_t >= t - 0.05 && gps_t <= t + 0.05)
{
printf("GPS VIO integetaed success: \n");
//gps的经纬度，海拔高度
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
//gps 数据的方差
double pos_accuracy = GPS_msg->position_covariance[0];
if(pos_accuracy <= 0)
pos_accuracy = 1;
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.05)
gpsQueue.pop();
else if(gps_t > t + 0.05)
break;
}
m_buf.unlock();注意：
我们要比较的数据是： GPS经纬高转换到 以第一个GPS数据为原点的局部坐标系的位置，和优化后的 global_t， 如下程序：
// 因为经纬度表示的是地球上的坐标，而地球是一个球形， 需要首先把经纬度转化到平面坐标系上
// 值得一提的是，GPS2XYZ()并非把经纬度转化到世界坐标系下(以0经度，0纬度为原点)，
// 而是以第一帧GPS数据为坐标原点，这一点需要额外注意
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy[3])
{
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
if(first_gps_flag == 1)
{
first_local_XYZ << xyz[0],xyz[1],xyz[2];
first_gps_flag = 0;
}
//打印出来看看，和优化后的p, 比较一下，注意啦，应该是这两个数据的比较！
local_XYZ << xyz[0],xyz[1],xyz[2];
//存入经纬度计算出的平面坐标，存入GPSPositionMap中
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy[0],posAccuracy[1],posAccuracy[2]};
GPSPositionMap[t] = tmp;
newGPS = true;
}这是globalOptNode.cpp 中显示GPS局部 XYZ 信息
// +- 时间偏差,根觉自己采集数据而定
if(gps_t >= t - 0.05 && gps_t <= t + 0.05)
{
printf("*****************GPS VIO integetaed success***************** \n");
//gps的经纬度，海拔高度
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
//gps 数据的方差
//double pos_accuracy = GPS_msg->position_covariance[0];
double pos_accuracy[3];
pos_accuracy[0] = GPS_msg->position_covariance[0];
pos_accuracy[1] = GPS_msg->position_covariance[1];
pos_accuracy[2] = GPS_msg->position_covariance[2];
if(pos_accuracy[0] <= 0 || pos_accuracy[1] <= 0 ||pos_accuracy[2] <= 0)
{
pos_accuracy[0] = 1;
pos_accuracy[1] = 1;
pos_accuracy[2] = 1;
}
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.05)
gpsQueue.pop();
else if(gps_t > t + 0.05)
break;
}
m_buf.unlock();
printf("gps_local_XYZ[0]: %10.3f \n", globalEstimator.local_XYZ[0]);
printf("gps_local_XYZ[1]: %10.3f \n", globalEstimator.local_XYZ[1]);
printf("gps_local_XYZ[2]: %10.3f \n", globalEstimator.local_XYZ[2]);显示优化后的 global_t 信息
//广播轨迹
pub_global_odometry.publish(odometry); // imu|视觉结果 转换到 世界坐标系
//这两个都是优化后世界坐标系下的pose
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
printf("global_t[0]: %10.3f \n", global_t[0]);
printf("global_t[1]: %10.3f \n", global_t[1]);
printf("global_t[2]: %10.3f \n", global_t[2]);发现：
每次GPS数据更新的时候，局部的GPS 的 xyz 坐标和 优化后的 global_t， 误差在厘米范围内（因为我用的是GPS数据是RTK），但是随着时间增加距离差达到 米级
分析：
VINS-fusion 的 VIO 和 GPS 组合是个 开环的松组合，并没有反馈 IMU的零篇，PVA的误差等信息，后面自己进行优化！
还有一个原因可能是时间没有对齐，VIO输出的结果与GPS进行优化的时候，他们的时间是不对齐的，需要用插值法进行对齐，但是为什么KITTI数据的结果那么好？ 那是因为KITTI 数据集的IMU数据和GPS时间是对齐过后的，特别的对齐，时间没有区别，而我们自己测量的数据，虽然硬件上对齐了，但是VIO输出的结果和GPS到来的时间还是有一定的偏差，形如下面：
(4) imu parameters
# Gyr unit: " rad/s"; Acc unit: " m/s^2"; mGal（毫加仑） = 0.001Gal（加仑）
# 加仑是重力加速度单位，Gal = =1厘米／(秒^2)
acc_n: 0.1 # acc_std = 200[mGal] -> ("imu.ba_std") * 1.0E-5 = 0.002 (效果不好)
gyr_n: 0.02 # gyr_std = 20[deg/h] -> ("imu.bg_std") * (D2R / 3600) = 0.00009691
acc_w: 0.002 # vrw = 1.2[m/s/sqrt(h)] -> ("imu.vrw") / 60.0 = 0.02 速度随机游走
gyr_w: 0.0005815 # arw = 0.2[deg/sqrt(h)] -> ("imu.arw") * (D2R / 60) = 0.00005815 角度随即游走
g_norm: 9.81007 # gravity magnitude
（5）
- 使用场景
根据上文中分析的优化策略，global fusion的应用场景应该是GPS频率较低，VIO频率较高的系统。fusion 默认发布频率位10hz，而现在的GPS可以达到20hz，
如果在这种系统上使用，你可能还需要修改下VIO或者GPS频率。另外，使用的GPS是常见的误差较大的GPS，并不是RTK-GPS。
- GPS与VIO时间不同步
上文提到了，在多传感器融合系统中，传感器往往需要做时钟同步，那么global Fusion需要么？GPS分为为很多种，我们常见的GPS模块精度较低，十几米甚至几十米的误差，
这种情况下，同不同步没那么重要了，因为GPS方差太大。另外一种比较常见的是RTK-GPS ，在无遮挡的情况下，室外精度可以达到 2cm之内，输出频率可以达到20hz，
这种情况下，不同步时间戳会对系统产生影响，如果VIO要和RTK做松耦合，这点还需要注意。
本人用的数据是自己采集的，GPS 是 RTK，频率 1HZ, 和 VIO融合，效果很差，不知道是因为坐标系转换的问题，还是因为同步的问题，可是轨迹神似，个人感觉和坐标系的转换有关，
后面如果解决这个问题，会公开处理的细节！
（6） VIO 输出频率问题
仔细研究程序的可以知道，在进行VIO和GPS融合时，VIO结果的发布频率是图像的频率，也就是20HZ；
如果我们想改变VIO的发布频率，其实是可以去改变的！
只需要在 globalOptnode.cpp 中的 maincpp 文件中的订阅消息改变，如下：
//ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/imu_propagate", 100, vio_callback);为什么？ 解释如下：
信息的发布函数在 void Estimator::processMeasurements() 里面，如下：
*/
void Estimator::processMeasurements()
{
while (1)
{
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。
//需要显示的
pubOdometry(*this, header);
pubKeyPoses(*this, header);
pubCameraPose(*this, header);
pubPointCloud(*this, header);
pubKeyframe(*this);
pubTF(*this, header);
mProcess.unlock();
}
}而怎么发布这些信息，你就要研究一下 visualization.cpp 这个函数, 我仅仅展示相关的内容，如下：
void registerPub(ros::NodeHandle &n)
{
pub_latest_odometry = n.advertise<nav_msgs::Odometry>("imu_propagate", 1000);
pub_path = n.advertise<nav_msgs::Path>("path", 1000);
pub_odometry = n.advertise<nav_msgs::Odometry>("odometry", 1000);
pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud", 1000);
pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("margin_cloud", 1000);
pub_key_poses = n.advertise<visualization_msgs::Marker>("key_poses", 1000);
pub_camera_pose = n.advertise<nav_msgs::Odometry>("camera_pose", 1000);
pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
pub_keyframe_pose = n.advertise<nav_msgs::Odometry>("keyframe_pose", 1000);
pub_keyframe_point = n.advertise<sensor_msgs::PointCloud>("keyframe_point", 1000);
pub_extrinsic = n.advertise<nav_msgs::Odometry>("extrinsic", 1000);
pub_image_track = n.advertise<sensor_msgs::Image>("image_track", 1000);
cameraposevisual.setScale(0.1);
cameraposevisual.setLineWidth(0.01);
}
void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, double t)
{
nav_msgs::Odometry odometry;
odometry.header.stamp = ros::Time(t);
odometry.header.frame_id = "world";
odometry.pose.pose.position.x = P.x();
odometry.pose.pose.position.y = P.y();
odometry.pose.pose.position.z = P.z();
odometry.pose.pose.orientation.x = Q.x();
odometry.pose.pose.orientation.y = Q.y();
odometry.pose.pose.orientation.z = Q.z();
odometry.pose.pose.orientation.w = Q.w();
odometry.twist.twist.linear.x = V.x();
odometry.twist.twist.linear.y = V.y();
odometry.twist.twist.linear.z = V.z();
pub_latest_odometry.publish(odometry);
}
void pubOdometry(const Estimator &estimator, const std_msgs::Header &header)
{
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
{
nav_msgs::Odometry odometry;
odometry.header = header;
odometry.header.frame_id = "world";
odometry.child_frame_id = "world";
Quaterniond tmp_Q;
tmp_Q = Quaterniond(estimator.Rs[WINDOW_SIZE]);
odometry.pose.pose.position.x = estimator.Ps[WINDOW_SIZE].x();
odometry.pose.pose.position.y = estimator.Ps[WINDOW_SIZE].y();
odometry.pose.pose.position.z = estimator.Ps[WINDOW_SIZE].z();
odometry.pose.pose.orientation.x = tmp_Q.x();
odometry.pose.pose.orientation.y = tmp_Q.y();
odometry.pose.pose.orientation.z = tmp_Q.z();
odometry.pose.pose.orientation.w = tmp_Q.w();
odometry.twist.twist.linear.x = estimator.Vs[WINDOW_SIZE].x();
odometry.twist.twist.linear.y = estimator.Vs[WINDOW_SIZE].y();
odometry.twist.twist.linear.z = estimator.Vs[WINDOW_SIZE].z();
pub_odometry.publish(odometry);
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = header;
pose_stamped.header.frame_id = "world";
pose_stamped.pose = odometry.pose.pose;
path.header = header;
path.header.frame_id = "world";
path.poses.push_back(pose_stamped);
pub_path.publish(path);
// write result to file
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << header.stamp.toSec() << " ";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << " "
<< estimator.Ps[WINDOW_SIZE].y() << " "
<< estimator.Ps[WINDOW_SIZE].z() << " "
<< tmp_Q.x() << " "
<< tmp_Q.y() << " "
<< tmp_Q.z() << " "
<< tmp_Q.w() << endl;
foutC.close();
Eigen::Vector3d tmp_T = estimator.Ps[WINDOW_SIZE];
printf("time: %f, t: %f %f %f q: %f %f %f %f \n", header.stamp.toSec(), tmp_T.x(), tmp_T.y(), tmp_T.z(),
tmp_Q.w(), tmp_Q.x(), tmp_Q.y(), tmp_Q.z());
}
}你应该很熟悉 registerPub(ros::NodeHandle &n) 这个函数，在主函数里面！
而 IMU 推导出来的结果，然后发布PVQ 在下面的程序里面，当一有 IMU数据便开始 发布 latest_PVQ
//IMU积分，并且上传IMU的状态信息
void Estimator::inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
{
mBuf.lock();
accBuf.push(make_pair(t, linearAcceleration));//时间和加速度信息放在 accBuf 里面
gyrBuf.push(make_pair(t, angularVelocity));
mBuf.unlock();
if (solver_flag == NON_LINEAR)//每次非线性，都进行积分一次
{
mPropagate.lock();
fastPredictIMU(t, linearAcceleration, angularVelocity);
pubLatestOdometry(latest_P, latest_Q, latest_V, t);
mPropagate.unlock();
}
}
//快速IMU积分
void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)
{
double dt = t - latest_time;
latest_time = t;//时间更新处理
Eigen::Vector3d un_acc_0 = latest_Q * (latest_acc_0 - latest_Ba) - g;
Eigen::Vector3d un_gyr = 0.5 * (latest_gyr_0 + angular_velocity) - latest_Bg;
latest_Q = latest_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = latest_Q * (linear_acceleration - latest_Ba) - g;
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
latest_P = latest_P + dt * latest_V + 0.5 * dt * dt * un_acc;
latest_V = latest_V + dt * un_acc;
latest_acc_0 = linear_acceleration;
latest_gyr_0 = angular_velocity;
}理解：
先不说 VIO怎么和GPS结合， 先说一下 VIO的结果是怎么来的！ 如果我们发布的是 Ps[]、Rs[]、Vs[], 那么这个结果是20HZ，
他是IMU与图像结合后，滑动窗口内最新优化后 载体相对 第一帧图像的 信息，我们的IMU这时候用的是 预积分功能，然后
一些限制条件和视觉信息结合啦，求出IMU的零偏和图像的尺度之类的事情； 但是我们如果发布的是 latest_PVQ，这个时候
我们发布的 PVQ 就不同于 Ps[]、Rs[]、Vs[] 了， 这时候我们可以看作当没有视觉时，我们发布的就是 IMU机械编排的结果，
当有图像信息的时候，我们再用图像 校正 IMU的偏差，求出 Ps[]、Rs[]、Vs[] ， 然后赋值 给 latest_PVQ，这个思想很容易理解吧！
所以对于 latest_PVQ， 他的结果频率就是IMU的 频率，200HZ； 相关程序如下：
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
}
。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。
slideWindow();//滑动窗口
f_manager.removeFailures();//去掉失败点
// prepare output of VINS
key_poses.clear();
for (int i = 0; i <= WINDOW_SIZE; i++)
key_poses.push_back(Ps[i]);
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
updateLatestStates();
}
（7）VINS-fusion的问题
1、
VINS-FUSION 中 GPS 与 INS 属于开环松组合，感觉这是精度上不来的根本原因，故本人想写一下闭环松组合的程序，参考程序如下
GPS_imu_视觉参考 https://blog.csdn.net/weixin_41843971/article/details/102976271
2、别人写好的多传感器融合程序 https://github.com/2013fangwentao/Multi_Sensor_Fusion
功能
- 支持GNSS/INS松组合解算
- 支持GNSS/INS/Camera融合解算
- 支持纯惯导推算
- 支持VIO解算，不过需要利用GNSS数据进行全局的初始化
4809
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
