放两张图片,至于为什么？后面会解释！
程序下载：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
cd GPS_Stereo_Ins/output
evo_traj tum vio_GPS_global.txt -p
绘图结果如下：
真值是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才可以出来这个对比图！
真值 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
rviz 显示 小车的轨迹和GPS数据垂直！
虽然，轨迹和GPS数据垂直！但在 （3）对比真值 时，两个轨迹是对齐的！
而且去看看程序你就会发现，两条线垂直，一条是 vio的结果，一条是vio转换到世界坐标系下的结果，
转换矩阵 是根据每次的优化值重新求得的结果，本来人家就不应该一样，为啥非让人家重合呢!
配置文件里面的IMU 变成1 ， rosrun global_fusion global_fusion_node 运行不了！为什么？
需要改程序！2011_10_03_drive_0027_sync 文件里面虽然有IMU数据，但是我们仅仅是读取出来了，
并没有用它！GPS和IMU数据是放在一起的，具体的意思如下：
为什么很少见 “视觉+IMU+GPS的组合”呢？是因为他们没有必要嘛？
其实VINS-fusion已经实现了，只是需要进行一下组合罢了！也可能做这方面的人少，个人感觉还是有价值的，
对于大场景来说很有必要的！后面自己会尝试组合一下，感觉更多是数据的处理！
前面我们跑的程序其实是：双目+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 跑kitti数据可以，为什么吗 双目+IMU 就不行？
你要是说是程序有问题，但是 程序跑RUROC的双目+IMU时，也是好的呀！
而且跑kitti数据的双目程序也是好的呀，这样一排除，个人感觉还是出在了外参上面，如果发现我错误的小伙伴别忘了留言提醒我！
对了，我看了相关数据介绍的论文了，好像GPS/IMU是与相机不同步，可是不同步为什么 单目+IMU效果是好的？
要么都不同步呗， 这个我无法理解！注意咱的思路，后面验证我是对的！
上面问题已经解决了！过程很艰辛，但是我想说的是我的思路很好，不然估计还是解决不了！处理的方式上面已经给了！
修正后，双目+IMU+GPS 的效果如下：
完美！
/*
说明：
本程序尝试：相机+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]);
}
#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);
}
#######################-----自己加的程序-----#############################
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)