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数据进行全局的初始化