VINS - Fusion GPS/INS/视觉融合 0、 Kitti数据测试

最新推荐文章于 2025-03-07 15:35:46 发布

他人是一面镜子，保持谦虚的态度

最新推荐文章于 2025-03-07 15:35:46 发布

阅读量6k

点赞数 22

分类专栏： VINS-Mono and Fusion 程序解读

本文链接：https://blog.csdn.net/hltt3838/article/details/109739046

版权

VINS-Mono and Fusion 程序解读专栏收录该内容

20 篇文章

订阅专栏

本文围绕VINS-Fusion程序展开，介绍了运行前的准备工作，包括指定输出路径、修改代码输出轨迹文件。详细说明了运行步骤，需打开多个终端执行不同命令。还阐述了处理结果的方法，以及运行中出现问题的解答。最后尝试实现视觉+IMU+GPS组合，分享了数据预处理、提取数据、运行程序等步骤及效果。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

放两张图片,至于为什么？后面会解释！

程序下载：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

数据集制作：https://zhuanlan.zhihu.com/p/115562083

测试：https://blog.csdn.net/weixin_39702448/article/details/107545488

数据评估：https://blog.csdn.net/weixin_47074246/article/details/109134740

KITTI数据集00序列times.txt文件 https://download.csdn.net/download/haolele3587/12262651

KITTI 数据跑双目+IMU的问题 https://zhuanlan.zhihu.com/p/75672946

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

https://www.it610.com/article/1282169330733170688.htm

一、准备

步骤一、指定输出路径:打开vins-fusion/config/kitti_raw/kitti_10_03_config.yaml 或 kitti_raw/kitti_09_30_config.yaml

(根据数据集选择具体参数文件),第十行 output_path: "/home/tony-ws1/output/"
这里指明自己需要保存到的地址，本人：output_path: "/home/hltt3838/GPS_Stereo_Ins/output/"

步骤二、修改代码使其输出轨迹文件,原本的vins_fusion是无轨迹文件输出的 globalOptNode.cpp中vio_callback函数

    std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);//这里还是文件输出位置
   foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
    foutC.precision(5);
    foutC << global_t.x() << ","
            << global_t.y() << ","
            << global_t.z() << ","
            << global_q.w() << ","
            << global_q.x() << ","
            << global_q.y() << ","
            << global_q.z() << endl;
    foutC.close();

这部分改成如下:

     //这里还是文件输出位置,最好和前面位置一样
    std::ofstream foutC("/home/hltt3838/GPS_Stereo_Ins/output/vio_GPS_global.txt", ios::app);

   foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() << " ";
    foutC.precision(5);
    foutC << global_t.x() << " "
            << global_t.y() << " "
            << global_t.z() << " "
            << global_q.x() << " "
            << global_q.y() << " "
            << global_q.z() << " "
            << global_q.w() << endl;
    foutC.close();

注意：

这里不仅把数据的保存地址改了，而且把数据的顺序也改了！ 1e9 和 “ ，”也改了

二、运行

打开第一个终端

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 kitti_gps_test src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml /home/hltt3838/kitti_data/2011_10_03_drive_0027_sync/

//rosun 包名执行文件 argv[1] argv[2]

% /home/hltt3838/ kitti_data/2011_10_03_drive_0027_sync/ 是数据的位置

打开第四个终端

进入工作区间内，分别输入：

cd GPS_Stereo_Ins/catkin_ws

source devel/setup.bash

rosrun global_fusion global_fusion_node

三、处理结果

(1) 进入输出结果的文件夹：

cd GPS_Stereo_Ins/output

(2) 使用evo分析结果,这里是tum数据格式,故使用

evo_traj tum vio_GPS_global.txt -p

绘图结果如下：

（3）对比真值

真值是12列的kitti轨迹格式,所以要将真值转换成8列格式的tum轨迹格式

python kitti_poses_and_timestamps_to_trajectory.py 00.txt times.txt kitti_00_gt.txt

转换得到tum格式的真值后,我们可以进行评定,将我们得到的vio_GPS_global.txt 与这里的真值进行对比

evo_ape tum vio_GPS_global.txt kitti_00_gt.txt -a -p

注意：右图点击上面的map才可以出来这个对比图！

四、出现的问题

问题1：

真值 kitti_00_gt.txt 怎么生成的？

答：

将KITTI的 pose 里面的 00.txt 加上时间戳 times.txt，转换成TUM格式的; 需要注意的是：pose 是个单独的文件，

不在 2011_10_03_drive_0042_sync 数据里面，自己从网上下载吧！

在evo文件夹下有一个contrib文件夹，里面有一个kitti_poses_and_timestamps_to_trajectory.py文件。

把KITTI数据集下的 00.txt 和 times.txt 文件拷贝到该目录，运行如下命令：

cd evo-master/contrib

python kitti_poses_and_timestamps_to_trajectory.py 00.txt times.txt kitti_00_gt.txt

通过该指令完成转换, 转换得到tum格式的真值后 kitti_00_gt.txt

我们可以进行评定,将我们得到的 vio_GPS_global.txt 与这里的真值进行对比

evo_ape tum vio_GPS_global.txt kitti_00_gt -a -p

问题2：

rviz 显示小车的轨迹和GPS数据垂直！

答：

虽然，轨迹和GPS数据垂直！但在（3）对比真值时，两个轨迹是对齐的！

而且去看看程序你就会发现，两条线垂直，一条是 vio的结果，一条是vio转换到世界坐标系下的结果，

转换矩阵是根据每次的优化值重新求得的结果，本来人家就不应该一样，为啥非让人家重合呢!

问题3：

配置文件里面的IMU 变成1 ， rosrun global_fusion global_fusion_node 运行不了！为什么？

答：

需要改程序！2011_10_03_drive_0027_sync 文件里面虽然有IMU数据，但是我们仅仅是读取出来了，

并没有用它！GPS和IMU数据是放在一起的，具体的意思如下：

问题4：

为什么很少见 “视觉+IMU+GPS的组合”呢？是因为他们没有必要嘛？

答：

其实VINS-fusion已经实现了，只是需要进行一下组合罢了！也可能做这方面的人少，个人感觉还是有价值的，

对于大场景来说很有必要的！后面自己会尝试组合一下，感觉更多是数据的处理！

五、视觉 + IMU + GPS

前面我们跑的程序其实是：双目+GPS； VINS-Fusion 里面并没有双目+IMU+GPS！

但是本人想把这个组合跑起来，所以自己动手写了一下，具体步骤如下，如果效果好，会公开细节！

数据预处理

以 2011_10_03_drive_0042_sync 为例，这个数据是图片和激光雷达对齐后的数据；

此数据中GPS和IMU数据在一起，但是IMU的时间是和图片的时间一样的，10HZ！

因此我们如果想实现 IMU+图像+GPS的数据，必须把100HZ的IMU数据和时间提取出来；

还有对应的 2011_10_03_drive_0042_extract, 这是没有对齐的数据，其中IMU的时间为100HZ，因此

我们要从这个文件里面把IMU数据提取出来！值的注意的是，KITTI数据里面的图像、激光雷达、GPS的数据是10HZ ！

下面是我提取数据的程序！

#include <iostream>
#include <stdio.h>
#include <cmath>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include "estimator/estimator.h"
 

int main(int argc, char** argv)
{
	ros::init(argc, argv, "vins_estimator");
	ros::NodeHandle n("~");
    
	
    string sequence1 = "/home/hltt3838/kitti_data/2011_10_03_drive_0042_extract/";
	string dataPath1 = sequence1 + "/";
   
    string sequence2 = "/home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/";
	string dataPath2 = sequence2 + "/";
	
     /*----------------------------IMU读取数据----------------------------------*/    
    //读取 100HZ的IMU时间
	FILE* file1;
	file1 = std::fopen((dataPath1 + "oxts/timestamps.txt").c_str() , "r");
	if(file1 == NULL){
	    printf("cannot find file: %soxts/timestamps.txt \n", dataPath1.c_str());
	    return 0;          
	}
	vector<double> imuTimeList;
	int year1, month1, day1;
	int hour1, minute1;
	double second1;
   while (fscanf(file1, "%d-%d-%d %d:%d:%lf", &year1, &month1, &day1, &hour1, &minute1, &second1) != EOF)
	{
	    imuTimeList.push_back(hour1 * 60 * 60 + minute1 * 60 + second1);
	}
	std::fclose(file1);
    /*------------------------------IMU读取数据-------------------------------*/
     
    
    /*----------------------------GPS读取数据----------------------------------*/    
    //读取 100HZ的IMU时间
	FILE* file2;
	file2 = std::fopen((dataPath2 + "oxts/timestamps.txt").c_str() , "r");
	if(file2 == NULL){
	    printf("cannot find file: %soxts/timestamps.txt \n", dataPath2.c_str());
	    return 0;          
	}
	vector<double> gpsTimeList;
	int year2, month2, day2;
	int hour2, minute2;
	double second2;
   while (fscanf(file2, "%d-%d-%d %d:%d:%lf", &year2, &month2, &day2, &hour2, &minute2, &second2) != EOF)
	{
	    gpsTimeList.push_back(hour2 * 60 * 60 + minute2 * 60 + second2);
	}
	std::fclose(file2);
    /*------------------------------GPS读取数据-------------------------------*/
    
    string OUTPUT_imu = "/home/hltt3838/kitti_data/imu_data_100hz/"; //不能有空格
    FILE* outFile_imu;
	outFile_imu = fopen((OUTPUT_imu + "/imu.txt").c_str(),"w");
 
    string OUTPUT_gps = "/home/hltt3838/kitti_data/gps_data_10hz/"; //不能有空格
    FILE* outFile_gps;
	outFile_gps = fopen((OUTPUT_gps + "/gps.txt").c_str(),"w");
    
    
    for (size_t i = 0; i < imuTimeList.size(); i++)
	{	
            stringstream ss;
			ss << setfill('0') << setw(10) << i;
        
            //读取GPS信息
			FILE* imuFile;
			string imuFilePath = dataPath1 + "oxts/data/" + ss.str() + ".txt";
			imuFile = std::fopen(imuFilePath.c_str() , "r");
			if(imuFile == NULL){
			    printf("cannot find file: %s\n", imuFilePath.c_str());
			    return 0;          
			}
			double lat, lon, alt, roll, pitch, yaw;
			double vn, ve, vf, vl, vu;
			double ax, ay, az, af, al, au;
			double wx, wy, wz, wf, wl, wu;
			double pos_accuracy, vel_accuracy;
			double navstat, numsats;
			double velmode, orimode;
			
			fscanf(imuFile, "%lf %lf %lf %lf %lf %lf ", &lat, &lon, &alt, &roll, &pitch, &yaw);
			fscanf(imuFile, "%lf %lf %lf %lf %lf ", &vn, &ve, &vf, &vl, &vu);
			fscanf(imuFile, "%lf %lf %lf %lf %lf %lf ", &ax, &ay, &az, &af, &al, &au);
			fscanf(imuFile, "%lf %lf %lf %lf %lf %lf ", &wx, &wy, &wz, &wf, &wl, &wu);
			fscanf(imuFile, "%lf %lf %lf %lf %lf %lf ", &pos_accuracy, &vel_accuracy, &navstat, &numsats, &velmode, &orimode);

			std::fclose(imuFile);
        
        fprintf (outFile_imu, "%f %f %f %f %f %f %f \n",imuTimeList[i],ax,ay,az,wx,wy,wz);
       
    }
    
    for (size_t i = 0; i < gpsTimeList.size(); i++)
	{	
            stringstream ss;
			ss << setfill('0') << setw(10) << i;
        
            //读取GPS 信息
			FILE* gpsFile;
			string gpsFilePath = dataPath2 + "oxts/data/" + ss.str() + ".txt";
			gpsFile = std::fopen(gpsFilePath.c_str() , "r");
			if(gpsFile == NULL){
			    printf("cannot find file: %s\n", gpsFilePath.c_str());
			    return 0;          
			}
			double lat, lon, alt, roll, pitch, yaw;
			double vn, ve, vf, vl, vu;
			double ax, ay, az, af, al, au;
			double wx, wy, wz, wf, wl, wu;
			double pos_accuracy, vel_accuracy;
			double navstat, numsats;
			double velmode, orimode;
			
			fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &lat, &lon, &alt, &roll, &pitch, &yaw);
			fscanf(gpsFile, "%lf %lf %lf %lf %lf ", &vn, &ve, &vf, &vl, &vu);
			fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &ax, &ay, &az, &af, &al, &au);
			fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &wx, &wy, &wz, &wf, &wl, &wu);
			fscanf(gpsFile, "%lf %lf %lf %lf %lf %lf ", &pos_accuracy, &vel_accuracy, &navstat, &numsats, &velmode, &orimode);

			std::fclose(gpsFile);
        
        fprintf (outFile_gps, "%f %f %f %f %f \n",gpsTimeList[i],lat,lon,alt,pos_accuracy);
       
    }
    
	if(outFile_imu != NULL)
		fclose (outFile_imu);
    
    if(outFile_gps != NULL)
		fclose (outFile_gps);
    
	return 0;
}

提取数据

cd GPS_Stereo_Ins/catkin_ws

catkin_make

source devel/setup.bash

rosrun vins change_data

获得数据后，需要计算IMU和相机之间的外参！具体的参数介绍看下面的连接！

https://blog.csdn.net/cuichuanchen3307/article/details/80596689

代码如下：

import numpy as np;
from numpy import *
  

IMU_2_V =np.mat([[9.999976e-01, 7.553071e-04, -2.035826e-03,-8.086759e-01],
                   [-7.854027e-04, 9.998898e-01, -1.482298e-02, 3.195559e-01],
                   [2.024406e-03, 1.482454e-02, 9.998881e-01,-7.997231e-01],
                   [0,0,0,1]])

V_2_C = np.mat([[7.967514e-03, -9.999679e-01, -8.462264e-04, -1.377769e-02],
                    [-2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02],
                    [9.999644e-01, 7.969825e-03, -2.764397e-03, -2.918589e-01],
                    [0,0,0,1]])

  

C_2_C1 = np.mat([[9.993440e-01, 1.814887e-02, -3.134011e-02, -5.370000e-01],
                    [-1.842595e-02, 9.997935e-01, -8.575221e-03, 5.964270e-03],
                    [3.117801e-02, 9.147067e-03, 9.994720e-01, -1.274584e-02],
                    [0,0,0,1]])


C_2_C1_ = np.mat([[1, 0, 0, 0.537150653267924],
                   [0, 1, 0, 0,],
                   [0, 0, 1, 0,],
                   [0, 0, 0, 1]])



IMU_2_C0 = V_2_C*IMU_2_V
IMU_2_C0 = IMU_2_C0.I

#####################################

IMU_2_C1 = C_2_C1_.I*V_2_C*IMU_2_V
IMU_2_C1 = IMU_2_C1.I


print("Tb_2_c0:")
print(mat(IMU_2_C0))
 
print("Tb_2_c1:")
print(mat(IMU_2_C1))

可能有人对公式有点迷茫！这里说一下转换矩阵的 Ra_2_b;

Ra_2_b 可以是 a坐标系到b坐标系之间的转换；也是 b坐标系上的点，经过 Ra_2_b矩阵 投影到 a 坐标系上的点；

还可以是 a 坐标系下， b坐标系的姿态！

外参定义：b坐标系下的点经过 T 后，转换到 C坐标系下的矩阵！故 外参 = Tc_2_b (这个写法可能每个人不一样！)

运行程序：

cd kitti_data/2011_10_03_calib

python maxtrix_4_4.py

结果如下：

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.00875116, -0.00479609,  0.99995027,  1.10224312,
          -0.99986428, -0.01400249,  0.00868325, -0.31907194,
           0.01396015, -0.99989044, -0.00491798,  0.74606588,
           0, 0, 0, 1]
 
 
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.00875116, -0.00479609,  0.99995027,  1.10694381,
          -0.99986428, -0.01400249,  0.00868325, -0.8561497 ,
           0.01396015, -0.99989044, -0.00491798,  0.75356458,
           0, 0, 0, 1]

得到外参后，我们把参数放在自己写的配置文件中即可运行程序

注意啦、注意啦！注意啦！

程序中有两个C0到_C1 的转换矩阵，其中，C_2_C1是下面这个文件给的，但是这个是不准确的，不要相信呀，我被坑残了！

而 C_2_C1_ 是 VINS-fusion 跑纯双目时给的矩阵 body_T_cam1 （因为没有IMU的时候，body = C0）

这个才是对的，不然的话你们跑 IMU+双目+GPS 用 KITTI数据效果特别差的！自己可以体验一下！

其实我们也可以分析出C_2_C1是有问题的，你看看平移量t就明白了，再看看最前面我们放的两张图片中相机之间的位置，只有x方向上有区别！

我看了博客上也有人遇到这个问题，和他们交流他们也不知道问题所在，好在还是弄出来了！对后面的同学一定很有帮助！

运行程序

打开第一个终端

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 camera_IMU_GPS_test src/VINS-Fusion/config/test_data/stereo_imu_gps_config.yaml /home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/

单目

rosrun vins camera_IMU_GPS_test src/VINS-Fusion/config/test_data/mono_imu_gps_config.yaml /home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/

打开第四个终端

进入工作区间内，分别输入：

cd GPS_Stereo_Ins/catkin_ws

source devel/setup.bash

rosrun global_fusion global_fusion_node

情况一、单目+IMU

个人感觉效果还是可以的！我们再来看看 双目+IMU

情况二、双目+IMU

评价：一个字，惨不忍睹！

我有点想不明白了，单目+IMU 跑kitti数据可以，为什么吗双目+IMU 就不行？

你要是说是程序有问题，但是程序跑RUROC的双目+IMU时，也是好的呀！

而且跑kitti数据的双目程序也是好的呀，这样一排除，个人感觉还是出在了外参上面，如果发现我错误的小伙伴别忘了留言提醒我！

对了，我看了相关数据介绍的论文了，好像GPS/IMU是与相机不同步，可是不同步为什么单目+IMU效果是好的？

要么都不同步呗，这个我无法理解！注意咱的思路，后面验证我是对的！

上面问题已经解决了！过程很艰辛，但是我想说的是我的思路很好，不然估计还是解决不了！处理的方式上面已经给了！

修正后，双目+IMU+GPS 的效果如下：

完美！

对应的程序如下：

添加的主程序： camera_IMU_GPS_test.cpp

/*
说明：
本程序尝试：相机+IMU+GPS， 其实VINS-fusion 已经具备这个条件了，主要在于结合；
本次程序是rosNodeTest.cpp 和 KITTIGPS.cpp 的结合；
通过读取不同的配置文件，可以决定是单目相机还是双目相机；
先尝试用Kitti的数据，在尝试用自己采集的数据！
*/
#include <iomanip>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <map>
#include <thread>
#include <mutex>
#include <cmath>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include "estimator/estimator.h"
#include "utility/visualization.h"
#include "camera_imu_gps.h"
 

using namespace std;
using namespace Eigen;

Estimator estimator;
ros::Publisher pubGPS;

std::mutex m_buf;
 

void readIMUdata(const std::string &line, IMU_MSG &imuObs);
void readGPSdata(const std::string &line, GPS_MSG &gpsObs);



void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    for (unsigned int i = 0; i < feature_msg->points.size(); i++)
    {
        int feature_id = feature_msg->channels[0].values[i];
        int camera_id = feature_msg->channels[1].values[i];
        double x = feature_msg->points[i].x;
        double y = feature_msg->points[i].y;
        double z = feature_msg->points[i].z;
        double p_u = feature_msg->channels[2].values[i];
        double p_v = feature_msg->channels[3].values[i];
        double velocity_x = feature_msg->channels[4].values[i];
        double velocity_y = feature_msg->channels[5].values[i];
        if(feature_msg->channels.size() > 5)
        {
            double gx = feature_msg->channels[6].values[i];
            double gy = feature_msg->channels[7].values[i];
            double gz = feature_msg->channels[8].values[i];
            pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);
            //printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
        }
        ROS_ASSERT(z == 1);
        Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
        xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
    }
    double t = feature_msg->header.stamp.toSec();
    estimator.inputFeature(t, featureFrame);
    return;
}


void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
    if (restart_msg->data == true)
    {
        ROS_WARN("restart the estimator!");
        estimator.clearState();
        estimator.setParameter();
    }
    return;
}

void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
{
    if (switch_msg->data == true)
    {
        ROS_WARN("use IMU!");
        estimator.changeSensorType(1, STEREO);
    }
    else
    {
        ROS_WARN("disable IMU!");
        estimator.changeSensorType(0, STEREO);
    }
    return;
}

void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
{
    if (switch_msg->data == true)
    {
        ROS_WARN("use stereo!");
        estimator.changeSensorType(USE_IMU, 1);
    }
    else
    {
        ROS_WARN("use mono camera (left)!");
        estimator.changeSensorType(USE_IMU, 0);
    }
    return;
}



int main(int argc, char** argv)
{
  	ros::init(argc, argv, "vins_estimator");
	ros::NodeHandle n("~");
	ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

	pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);
    ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
    ros::Subscriber sub_imu_switch = n.subscribe("/vins_imu_switch", 100, imu_switch_callback);
    ros::Subscriber sub_cam_switch = n.subscribe("/vins_cam_switch", 100, cam_switch_callback);

    
	if(argc != 3)
	{
		printf("please intput: rosrun vins camera_IMU_GPS_test [config file] [data folder] \n"
			   "for example: rosrun vins camera_IMU_GPS_test "
			   "~/catkin_ws/src/VINS-Fusion/config/test_data/stereo_imu_gps_config.yaml "
			   "/home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/ \n");
		return 1;
	}
    
	string config_file = argv[1];//stereo_imu_gps_config.yaml
	printf("config_file: %s\n", argv[1]);
    
	string sequence = argv[2];   //---/home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/
	printf("read sequence: %s\n", argv[2]);
	string dataPath = sequence + "/";  
    
    //1、读取imu的 txt 文件，一行一行读取
    double init_imu_time;
    IMU_MSG imuObs;
    std::string line_imu;
    std::string imuPath = dataPath + "imu_data_100hz/imu.txt";
    std::ifstream  csv_IMUfile(imuPath); 
    if (!csv_IMUfile)
    {
	    printf("cannot find imu Path \n" );
	    return 0;          
	}
    std::getline(csv_IMUfile, line_imu); //header, 获得的第一个IMU数据
    
    readIMUdata(line_imu, imuObs);
    init_imu_time = imuObs.time;
    printf("init_imu_time： %10.5f \n", init_imu_time);
	
  
    //2、读取GPS的 txt 文件，一行一行读取
    double init_gps_time;
    GPS_MSG gpsObs;
    std::string line_gps;
    std::string gpsPath = dataPath + "gps_data_10hz/gps.txt";
    std::ifstream  csv_GPSfile(gpsPath);
    if (!csv_GPSfile)
    {
	    printf("cannot find gps Path \n" );
	    return 0;          
	}
    std::getline(csv_GPSfile, line_gps); //header, 获得的第一个gps数据
    readGPSdata(line_gps, gpsObs);
    init_gps_time = gpsObs.time;
    printf("init_gps_time： %10.5f \n", init_gps_time); 
    
    
    //3、读取图像时间，整个文件读取，存放到 imageTimeList,两个相机对齐了，没有进行判断
    //Cam0
    double init_cam_time;
	FILE* file;
	file = std::fopen((dataPath + "image_00/timestamps.txt").c_str() , "r");
	if(file == NULL)
    {
	    printf("cannot find file: %simage_00/timestamps.txt \n", dataPath.c_str());
	    ROS_BREAK();
	    return 0;          
	}
	vector<double> image0TimeList;
	int year, month, day;
	int hour, minute;
	double second;
	while (fscanf(file, "%d-%d-%d %d:%d:%lf", &year, &month, &day, &hour, &minute, &second) != EOF)
	{
	    image0TimeList.push_back(hour * 60 * 60 + minute * 60 + second);
	}
	std::fclose(file);
    
    
    init_cam_time = image0TimeList[0]; 
    printf("init_cam_time: %10.5f \n", init_cam_time);
    
    double baseTime;
    baseTime = min(init_imu_time,init_gps_time,init_cam_time);
    printf("baseTime: %10.5f \n", baseTime);
    
    //4、读取配置参数和发布主题
    readParameters(config_file);
	estimator.setParameter();
	registerPub(n);
    
    //5、VIO的结果输出保存文件
    FILE* outFile;
	outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
	if(outFile == NULL)
    printf("Output vio path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());
	string leftImagePath, rightImagePath;
	cv::Mat imLeft, imRight;
    
    
    //6、遍历整个图像
    for (size_t i = 0; i < image0TimeList.size(); i++)
	{	
        int num_imu = 0;
		if(ros::ok())
		{
			printf("process image %d\n", (int)i);
			stringstream ss;
			ss << setfill('0') << setw(10) << i;
			leftImagePath = dataPath + "image_00/data/" + ss.str() + ".png";
			rightImagePath = dataPath + "image_01/data/" + ss.str() + ".png";
            
			printf("%s\n", leftImagePath.c_str() );
			printf("%s\n", rightImagePath.c_str() );
            double imgTime = 0; 
           
            imLeft  = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
            imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );
            
            imgTime = image0TimeList[i] - baseTime;
            printf("image time: %10.5f \n", imgTime);
           
            
			//读取GPS信息
            std::getline(csv_GPSfile, line_gps);  
            readGPSdata(line_gps, gpsObs);
            
            sensor_msgs::NavSatFix gps_position;
			gps_position.header.frame_id = "NED";
			gps_position.header.stamp = ros::Time(imgTime);
			gps_position.latitude  = gpsObs.position[0];
			gps_position.longitude = gpsObs.position[1];
			gps_position.altitude  = gpsObs.position[2];
			gps_position.position_covariance[0] = gpsObs.pos_accuracy;
    
			pubGPS.publish(gps_position);
            
   //---------------------加上IMU-------------------------//       
            //读取imu的信息
            while (std::getline(csv_IMUfile, line_imu))
            {
               num_imu++;
               printf("process imu %d\n",num_imu);
               readIMUdata(line_imu, imuObs);
               double imuTime = imuObs.time - baseTime;
               printf("imu time: %10.5f \n", imuTime);
                
               Vector3d acc = imuObs.acc;
               Vector3d gyr = imuObs.gyr;
                   
               estimator.inputIMU(imuTime, acc, gyr);
                   
               if (imuTime >= imgTime) //简单的时间同步,IMU的时间戳是不大于图像的
                {
                    break;
                }
            }
     //---------------------加上IMU-------------------------//     
     
            if(STEREO)//双目为1，否则为0
            {
              estimator.inputImage(imgTime,imLeft, imRight);
            }
            else
              estimator.inputImage(imgTime, imLeft);
 
        
			Eigen::Matrix<double, 4, 4> pose;
			estimator.getPoseInWorldFrame(pose);
			if(outFile != NULL)
				fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
																	       pose(1,0), pose(1,1), pose(1,2),pose(1,3),
																	       pose(2,0), pose(2,1), pose(2,2),pose(2,3));
			
			 //cv::imshow("leftImage", imLeft);
			 //cv::imshow("rightImage", imRight);
			 //cv::waitKey(2);
		}
		else
			break;
	}
	
	if(outFile != NULL)
		fclose (outFile);
    
    
    return 0;
}   






void readIMUdata(const std::string &line, IMU_MSG &imuObs)
{
    std::stringstream  lineStream(line);
    std::string        dataRecord[7];
    std::getline(lineStream, dataRecord[0], ' ');//这里的数据间是空格， 如果有逗号，用'，'
    std::getline(lineStream, dataRecord[1], ' ');
    std::getline(lineStream, dataRecord[2], ' ');
    std::getline(lineStream, dataRecord[3], ' ');
    std::getline(lineStream, dataRecord[4], ' ');
    std::getline(lineStream, dataRecord[5], ' ');
    std::getline(lineStream, dataRecord[6], ' ');
    
    imuObs.time = std::stod(dataRecord[0]); //时间：s;
             
    imuObs.acc[0] = std::stod(dataRecord[1]);
    imuObs.acc[1] = std::stod(dataRecord[2]);
    imuObs.acc[2] = std::stod(dataRecord[3]);

    imuObs.gyr[0] = std::stod(dataRecord[4]);
    imuObs.gyr[1] = std::stod(dataRecord[5]);
    imuObs.gyr[2] = std::stod(dataRecord[6]);
}


void readGPSdata(const std::string &line, GPS_MSG &gpsObs)
{
    std::stringstream  lineStream(line);
    std::string        dataRecord[7];
    std::getline(lineStream, dataRecord[0], ' ');//这里的数据间是空格， 如果有逗号，用'，'
    std::getline(lineStream, dataRecord[1], ' ');
    std::getline(lineStream, dataRecord[2], ' ');
    std::getline(lineStream, dataRecord[3], ' ');
    std::getline(lineStream, dataRecord[4], ' ');
 
    gpsObs.time = std::stod(dataRecord[0]); //时间：s;
             
    gpsObs.position[0] = std::stod(dataRecord[1]);
    gpsObs.position[1] = std::stod(dataRecord[2]);
    gpsObs.position[2] = std::stod(dataRecord[3]);

    gpsObs.pos_accuracy = std::stod(dataRecord[4]);
   
}

camera_imu_gps.h

#pragma once
#include<chrono>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>


struct IMU_MSG
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    double time;
    Eigen::Vector3d acc;
    Eigen::Vector3d gyr;
    IMU_MSG &operator =(const IMU_MSG &other)
    {
        time = other.time;
        acc = other.acc;
        gyr = other.gyr;
        return *this;
    }
};


struct GPS_MSG
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    double time;
    Eigen::Vector3d position;
    double pos_accuracy;
    GPS_MSG &operator =(const GPS_MSG &other)
    {
        time         = other.time;
        position     = other.position;
        pos_accuracy = other.pos_accuracy;
        return *this;
    }
};

 
double min(double x,double y,double z )
{
    return x < y ? (x < z ? x : z) : (y < z ? y : z);
}

CMakeLists.txt 中加如下两行

#######################-----自己加的程序-----#############################
add_executable(change_data src/change_data.cpp)
target_link_libraries(change_data vins_lib) 


add_executable(camera_IMU_GPS_test src/camera_IMU_GPS_test.cpp)
target_link_libraries(camera_IMU_GPS_test vins_lib)