目录
一、相关概念
二、程序解读
2.1 参数读取
解析：
2.2 获取图像时间信息
解析：
2.3 获取图像时间信息
解析：
2.4 定义VIO结果输出路径和读取图像信息
解析：
2.5 读取GPS位置速度等信息并通过节点发布
解析：
2.6 获取VIO的位姿态 pose
理解：
源代码：VINS - Fusion
数据集：KITTI 数据
程序入口：KITTIGPSTest.cpp 中
#include <iostream>
#include <stdio.h>
#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"
using namespace std;
using namespace Eigen;
Estimator estimator;
ros::Publisher pubGPS;
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);
if(argc != 3) //argc:终端输入参数的个数
{
printf("please intput: rosrun vins kitti_gps_test [config file] [data folder] \n"
"for example: rosrun vins kitti_gps_test "
"~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml "
"/media/tony-ws1/disk_D/kitti/2011_10_03/2011_10_03_drive_0027_sync/ \n");
return 1;
}
//config_file = vins_fusion/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
//sequence = vins_fusion/catkin_ws/Dates/2011_10_03_drive_0027_sync/
string sequence = argv[2];
printf("read sequence: %s\n", argv[2]);
string dataPath = sequence + "/";这里的参数读取和 VINS-Mono 里面的参数读取是不一样的，请注意体会和学习！这里涉及到 C++ 中的小知识！
我们在运行 VINS-Fusion 程序的时候，步骤如下：
cd vins_fusion/catkin_ws //程序 VINS-Fusion 放在了 vins_fusion/catkin_ws/src 里面
source devel/setup.bash
rosrun vins kitti_gps_test src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml Dates/2011_10_03_drive_0027_sync/
// rosrun 包名 执行程序名 argv[1] argv[2]
//数据 2011_10_03_drive_0027_sync 放在了 vins_fusion/catkin_ws/Dates 里面， 仔细体会这个意思，文件截图如下：
注意：pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000); 这个程序主要是GPS数据的发布，
为GPS和VIO结果的融合作准备，后面读取文件中GPS的数据 给 pubGPS
// c_str()返回一个客户程序可读不可改的指向字符数组的指针，不需要手动释放或删除这个指针
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> imageTimeList;
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)
{
imageTimeList.push_back(hour * 60 * 60 + minute * 60 + second);
}
std::fclose(file);这个应该很容易看懂，都是C++的知识；本人想提醒的是，一般多传感器的数据融合最重要的是时间同步，
这里的图像时间应该是通过 GNSS 板卡或POS系统获取的时间
vector<double> GPSTimeList;
{
FILE* file;
file = std::fopen((dataPath + "oxts/timestamps.txt").c_str() , "r");
if(file == NULL){
printf("cannot find file: %soxts/timestamps.txt \n", dataPath.c_str());
ROS_BREAK();
return 0;
}
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)
{
GPSTimeList.push_back(hour * 60 * 60 + minute * 60 + second);
}
std::fclose(file);
}
readParameters(config_file); //参数读取和设置
estimator.setParameter(); //此时开启了线程estimate，把这个弄明白
registerPub(n); //发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出既然时间是同步的，那么GPS时间和视觉的时间应该相差无几，小的差值应该是硬件延迟原因造成的！
文件存放的位置 如下所示：
需要注意的是 estimator.setParameter() 程序，它的作用是 processMeasurements() 线程的开启，程序如下：
void Estimator::setParameter()
{
mProcess.lock();
for (int i = 0; i < NUM_OF_CAM; i++)
{
tic[i] = TIC[i];
ric[i] = RIC[i];
cout << " exitrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl;
}
f_manager.setRic(ric);
ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
g = G;
cout << "set g " << g.transpose() << endl;
featureTracker.readIntrinsicParameter(CAM_NAMES);
std::cout << "MULTIPLE_THREAD is " << MULTIPLE_THREAD << '\n';
if (MULTIPLE_THREAD && !initThreadFlag)
{
initThreadFlag = true; //线程开启
processThread = std::thread(&Estimator::processMeasurements, this);
}
mProcess.unlock();
}设置参数，此处与重投影误差的协方差有关，FOCAL_LENGTH / 1.5 * Matrix2d::Identity() = 公式；
参数是从 readParameters(config_file) 获取的！
FILE* outFile;
outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
if(outFile == NULL)
printf("Output path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());
string leftImagePath, rightImagePath;
cv::Mat imLeft, imRight;
double baseTime;//基准时间
for (size_t i = 0; i < imageTimeList.size(); i++)
{
if(ros::ok())
{
if(imageTimeList[0] < GPSTimeList[0])
baseTime = imageTimeList[0];
else
baseTime = GPSTimeList[0];
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() );
imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE ) ;//返回灰色图像
imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );
double imgTime = imageTimeList[i] - baseTime;//程序都不难理解，先是定义了后面VIO输出数据结果所在的文件夹，再者是读取图像信息转化成 cv::Mat 的形式；
需要注意的是：参数 OUTPUT_FOLDER 是从 readParameters(config_file) 读出来的！
FILE* GPSFile;
string GPSFilePath = dataPath + "oxts/data/" + ss.str() + ".txt";
GPSFile = std::fopen(GPSFilePath.c_str() , "r");
if(GPSFile == NULL)
{
printf("cannot find file: %s\n", GPSFilePath.c_str());
ROS_BREAK();
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;
//这是按照GPS文件中数据的顺序读取的，好好学习数据读取的程序
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 ", &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);
//发布GPS信息，为数据融合作准备
sensor_msgs::NavSatFix gps_position;//传感器的一种数据结构
gps_position.header.frame_id = "NED";
gps_position.header.stamp = ros::Time(imgTime);
gps_position.status.status = navstat;
gps_position.status.service = numsats;
gps_position.latitude = lat;
gps_position.longitude = lon;
gps_position.altitude = alt;
gps_position.position_covariance[0] = pos_accuracy;
pubGPS.publish(gps_position);整个程序目的是读取文件夹中GPS的数据，并通过之前我们说的发布出去，用于VIO、GPS的融合！
estimator.inputImage(imgTime, imLeft, imRight);
Eigen::Matrix<double, 4, 4> pose;
estimator.getPoseInWorldFrame(pose);
//输出文件，内容是：VIO结果
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;
}注意 estimator.inputImage(imgTime, imLeft, imRight)， 其实这段语句是VIO的开启，读取图片信息、提取特征点...输出结果！
下面我们看一下这个程序：
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
inputImageCnt++; //记录图像的个数
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
TicToc featureTrackerTime;
//featureFrame = 每帧图像，左目，右目的：归一化坐标、像素坐标、速度
if(_img1.empty())
featureFrame = featureTracker.trackImage(t, _img); //图像信息获取，位置，速度。。。
else
featureFrame = featureTracker.trackImage(t, _img, _img1);//追踪双目
//然后，getTrackImage 对特征到跟踪的图像进行一些处理。并把追踪的图片imgTrack发布出去.
if (SHOW_TRACK)
{
cv::Mat imgTrack = featureTracker.getTrackImage();
pubTrackImage(imgTrack, t);
}
//然后，填充 featureBuf，最后执行processMeasurements，之后会进行详细讲解
if(MULTIPLE_THREAD)
{
if(inputImageCnt % 2 == 0)
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));//当前之间，左右相机，所有特征点信息
mBuf.unlock();
//queue<pair<时间, map< 特征点id, vector<pair< 相机id , 特征点信息 > > > > > featureBuf;
}
}
else
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
TicToc processTime;
//这是处理全部量测的线程,IMU的预积分,特征点的处理等等都在这里进行.
processMeasurements();
printf("process time: %f\n", processTime.toc());
}
}有没有很熟悉 processMeasurements()， 在前面的2.3 中因为有了对应线程的开启，才会有这个程序的执行！
具体的VIO代码我们就不具体分析了，想看的可以看我之前的博客！ 到此为止，数据的读取完成，下一步看GPS与VIO的融合！