注释代码
(visualization、pose_grapf、pose_grapf_node)
编译：
cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
运行代码
source ~/vins/devel/setup.bash
roslaunch vins vins_rviz.launch
source ~/vins/devel/setup.bash
rosrun vins vins_node ~/vins/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
source ~/vins/devel/setup.bash
(optional) rosrun loop_fusion loop_fusion_node ~/vins/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
source ~/vins/devel/setup.bash
rosbag play /home/dyt/compare/MH_01_easy.bag
运行VINS希望输出tum型数据从而使用evo工具进行比较：
1、visualization.cpp中pubOdometry()函数
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << ","
<< estimator.Ps[WINDOW_SIZE].y() << ","
<< estimator.Ps[WINDOW_SIZE].z() << ","
<< tmp_Q.w() << ","
<< tmp_Q.x() << ","
<< tmp_Q.y() << ","
<< tmp_Q.z() << ","
<< estimator.Vs[WINDOW_SIZE].x() << ","
<< estimator.Vs[WINDOW_SIZE].y() << ","
<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;
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;
2、pose_graph.cpp中的updatePath()函数
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << (*it)->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
修改为：
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << (*it)->time_stamp << " ";
loop_path_file.precision(5);
loop_path_file << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
3、pose_graph.cpp文件中addKeyFrame()函数
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","
<< P.y() << ","
<< P.z() << ","
<< Q.w() << ","
<< Q.x() << ","
<< Q.y() << ","
<< Q.z() << ","
<< endl;
修改为：
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp << " ";
loop_path_file.precision(5);
loop_path_file << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
4、pose_graph_node.cpp中的main()函数
修改为txt文件：
VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";
最后编译。
遇到问题：
在运行vins输出轨迹与真值比较时，会发现对于有的数据来说无法对齐。需要查看输出函数时间精度，因为evo是根据时间对齐，当时间精度较低，甚至数个数据对应同一时间时，对齐精度较差。
代码解析：
1、KITTIGPSTest.cpp （KITTI数据集 中 双目 + GPS）
读取GPS的数据，并以 相应的 topic 发出去，为后面的 global_fusion作准备，实现 数据的融合
pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);
2、 KITTIOdomTest.cpp（KITTI数据集 中 双目）
3、rosNodeTest.cpp （EUROC数据集 单目 + IMU）
运行车载KITTI数据：
（修改重力信息msckf_vio.cpp，配置信息）
先提取IMU信息
roscore
source devel/setup.bash
roslaunch vins vins_rviz.launch
source devel/setup.bash
rosrun vins kitti /home/dyt/VINS/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config1.yaml /home/dyt/2011_10_03/2011_10_03_drive_0042_sync/
运行KITTI：
添加kitti_vio.cpp
#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 "kitti_vio.h"
using namespace std;
using namespace Eigen;
Estimator estimator;
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);
}
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;
}
void readIMUdata(const std::string &line, IMU_MSG &imuObs);
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);
ros::Publisher pubLeftImage = n.advertise<sensor_msgs::Image>("/leftImage",1000);
ros::Publisher pubRightImage = n.advertise<sensor_msgs::Image>("/rightImage",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/dyt/2011_10_03_drive_0042_sync/ \n");
return 1;
}
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
string sequence = argv[2];
printf("read sequence: %s\n", argv[2]);
string dataPath = sequence + "/";
double init_imu_time;
IMU_MSG imuObs;
std::string line_imu;
std::string imuPath = dataPath + "imu_data/imu.txt";
std::ifstream csv_IMUfile(imuPath);
if (!csv_IMUfile)
{
printf("cannot find imu Path \n" );
return 0;
}
std::getline(csv_IMUfile, line_imu);
readIMUdata(line_imu, imuObs);
init_imu_time = imuObs.time;
printf("init_imu_time： %10.5f \n", init_imu_time);
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_cam_time);
printf("baseTime: %10.5f \n", baseTime);
readParameters(config_file);
estimator.setParameter();
registerPub(n);
FILE* outFile;
FILE* outFile1;
outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
outFile1 = fopen((OUTPUT_FOLDER + "/vio_kitti.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;
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);
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)
{
break;
}
}
if(STEREO)
{
estimator.inputImage(imgTime,imLeft, imRight);
}
else
estimator.inputImage(imgTime, imLeft);
Eigen::Matrix<double, 4, 4> pose;
Eigen::Vector3d P;
Eigen::Quaterniond Q;
double T;
estimator.getPoseInWorldFrame(pose);
estimator.getPoseInWorldFrame1(T,P,Q);
if(outFile1 != NULL){
fprintf (outFile1, "%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));
ofstream foutC("/home/dyt/VINS/src/VINS-Fusion/output_kitti/kitti_text.txt", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(15);
foutC << T << " ";
foutC.precision(5);
foutC << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
foutC.close();
fprintf(outFile, "%f %f %f %f %f %f %f %f\n", T, P.x(), P.y(), P.z(),
Q.x(), Q.y(), Q.z(), Q.w());
}
}
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]);
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]);
}
添加kitti_vio.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;
}
};
double min(double x,double y,double z )
{
return x < y ? (x < z ? x : z) : (y < z ? y : z);
}
KITTI
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
