本人用的是Ubuntu18.04版本系统,Eigen3.3.4 版本,ROS版本好像影响不大,和官方要求的不一样,但是结果还是成功的,若有失败的可以找相关博客。下面的运行的基础是保证系统安装成功ceres ,opencv , eigen等需要的库,网上方法很多!
一、过程理解
把程序跑完了,程序看了几遍,公式也推导了,论文看了很多,但是总感觉没有掌握这个程序。师兄问我,在这程序上加GPS可以吗?。。。。。因为我发现这这个程序弄懂不仅需要把公式、论文搞清楚,还需要把ROS学习一下,因此,本部分要讲的内容是如何理解整体的程序运行过程,对于公式的推导,程序的解读我就不说啦
第一步:运行 euroc.launch
roslaunch vins_estimator euroc.launch
我们可以打开 euroc.launch 里面的程序,其实就是运行三个节点,分别是: feature_tracker vins_estimator pose_graph
这也是 roslaunch 方便的地方,不用一个一个节点去运行。 其实我的理解是:每个节点都是独立的一部分,分工明确,他们之间的联系是用 ROS之间的通信 方式来完成的,就是所谓的 发布和订阅, 只要节点订阅到 对应的主题 就能接收到对应的数据或者图片, 要看 feature_track_node/main.cpp 中返回函数的形式。
feature_tracker : 用于图像的跟踪和特征点的读取等
vins_estimator:优化部分,当然还有其他很多的部分
pose_graph: 位姿图优化
我们首先看 feature_tracker, 当我们运行 roslaunch vins_estimator euroc.launch 时, feature_tracker节点启动,进入对应的 feature_tracker_node.cpp 程序, 我们看一下他的main.cpp (程序不就是这样看麻)
//完成视觉的跟踪
int main(int argc, char **argv)
{
ros::init(argc, argv, "feature_tracker"); //ros初始化和设置句柄
ros::NodeHandle n("~"); //命名空间为/node_namespace/node_name
//设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
//1.读取euroc_config.yaml中的一些配置参数-具体看这个参数
readParameters(n);
// 2.读取每个相机实例对应的相机内参,NUM_OF_CAM 经常为1,单目
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
// 3.判断是否加入鱼眼mask来去除边缘噪声
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
// 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时,执行回调函数img_callback
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
// 5.发布feature点云,实例feature_points,跟踪的特征点,给后端优化用
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
//发布feature_img,实例ptr,跟踪的特征点图,给RVIZ用和调试用
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
//发布restart,estimator_node.cpp订阅使用
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
/*
if (SHOW_TRACK)
cv::namedWindow("vis", cv::WINDOW_NORMAL);
*/
ros::spin();
return 0;
}
我们需要注意两个地方,
第一个地方就是:ros::init(argc, argv, "feature_tracker"); //这里的节点 feature_tracker 因该和我们euroc.launch 中的节点 feature_tracker 一致。
第二个地方就是://订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时,执行回调函数img_callback
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
话题 IMAGE_TOPIC 的订阅, 这个是怎么和我们后面的 rosbag play vins/Dates/MH_01_easy.bag 产生联系的呢?
可以参考我的这个博客:https://blog.csdn.net/hltt3838/article/details/107652555
既然是订阅话题 IMAGE_TOPIC, 那么这个话题代表的信息是什么,怎么得到?
答案是:读取euroc_config.yaml中的一些配置参数 得到的
readParameters(n);
我们进入这个程序就可以看到
/*
bool getParam (const std::string& key, parameter_type& output_value) const
key : 是参数名,命名方法参考
output_value: 用来保持参数的值
*/
template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
T ans;
if (n.getParam(name, ans))
{
ROS_INFO_STREAM("Loaded " << name << ": " << ans);
}
else
{
ROS_ERROR_STREAM("Failed to load " << name);
n.shutdown();
}
return ans;
}
/*
流程搞明白:
1、程序第一步,启动 roslaunch vins_estimator euroc.launch, 进入feature_tracker_node.cpp main()函数
2、节点名字feature_tracker, 参数<param name="config_file" = config/euroc/euroc_config.yaml, 可以看
对应的launch
3、readParam 读取对应文件的参数
*/
void readParameters(ros::NodeHandle &n)
{
std::string config_file;
config_file = readParam<std::string>(n, "config_file");
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
}
std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");
fsSettings["image_topic"] >> IMAGE_TOPIC;
fsSettings["imu_topic"] >> IMU_TOPIC;
MAX_CNT = fsSettings["max_cnt"];
MIN_DIST = fsSettings["min_dist"];
ROW = fsSettings["image_height"];
COL = fsSettings["image_width"];
FREQ = fsSettings["freq"];
F_THRESHOLD = fsSettings["F_threshold"];
SHOW_TRACK = fsSettings["show_track"];
EQUALIZE = fsSettings["equalize"];
FISHEYE = fsSettings["fisheye"];
if (FISHEYE == 1)
FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";
CAM_NAMES.push_back(config_file);
WINDOW_SIZE = 20;
STEREO_TRACK = false;
FOCAL_LENGTH = 460;
PUB_THIS_FRAME = false;
if (FREQ == 0)
FREQ = 100;
fsSettings.release();
}
结合 euroc.lanch 里面具体的内容,我们可以知道的是 fsSettings["image_topic"] >> IMAGE_TOPIC;
IMAGE_TOPIC 是从 程序中 配置文件 config/euroc/euroc_config.yaml 读出来的, 那我们需要看看 euroc_config.yaml
对应的 "image_topic" 是什么? 我们发现它是: /cam0/image_raw
%YAML:1.0
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
output_path: "/home/hltt3838/vins/out_result/"
...................................................
....................................................
....................................................
但是正巧的是,我们在下面的 rosbag play vins/Dates/MH_01_easy.bag 中,其实这个语句, 它发布的 topic 正好是 /cam0/image_raw。 我们怎么知道 人家发布的topic 是 /cam0/image_raw ? 看一下信息呀,怎么看? 用下面的语句 !
cd my_c++/VINS_test/ROS_bag // 首先进入.bag 的目录: 我把 MH_01_easy.bag 放在了 my_c++/VINS_test/ROS_bag
rosbag info MH_01_easy.bag //查看信息,具体如下
path: MH_01_easy.bag
version: 2.0
duration: 3:06s (186s)
start: Jun 25 2014 03:02:59.81 (1403636579.81)
end: Jun 25 2014 03:06:06.70 (1403636766.70)
size: 2.5 GB
messages: 47283
compression: none [2456/2456 chunks]
types: geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
topics: /cam0/image_raw 3682 msgs : sensor_msgs/Image
/cam1/image_raw 3682 msgs : sensor_msgs/Image
/imu0 36820 msgs : sensor_msgs/Imu
/leica/position 3099 msgs : geometry_msgs/PointStamped
我们可以知道他的 topic 是 /cam0/image_raw
人家是双目的数据,但是 VINS-MONO 是单目+IMU, 故我们用了 一个相机而已。
这里需要说的是:MH_01_easy.bag 这个包 是用原始数据生成的,topic 应该 是人为定义的,这个比我更清楚的可以指定一下错误哈!
因此,我们可以总结:运行 rosbag play vins/Dates/MH_01_easy.bag 时,这个包会出来图像数据, 主题topic 是 /cam0/image_raw 运行 roslaunch vins_estimator euroc.launch 时,节点feature_track_node.cpp 订阅 /cam0/image_raw 这个主题, 并通过 返回函数 img_callback 这个程序 读取 主题对应的图片。 OVER
其他的节点也是这样的思路,可以参考理解一下!也是我想了几天的结果!
二、运行
步骤一、创建ROS工作空间
mkdir -p ~/vins/src //其中vins是自己起的-可以改变,src不能更改,就算建立文件夹
cd ~/vins/src //进入建立的文件夹
catkin_init_workspace //创建工作空间
步骤二、编译VINS
cd ~/vins/src // 进入之前建立的文件夹
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git //下载代码到文件夹
cd ../ //返回上一级文件夹
catkin_make //编译
【注意】如果我们事先有代码,把代码放进建立的文件夹即可
步骤三、运行VINS
打开三个终端,
第一个终端:
//配置环境变量 ,vins 是之前创建的文件夹,devel/setup.bash是catkin_make 编译后的内容
source ~/vins/devel/setup.bash
roslaunch vins_estimator euroc.launch //这是运行euroc数据集的三个启动节点.
<launch>
<arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" />
<arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
<node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
<node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="vins_folder" type="string" value="$(arg vins_path)" />
</node>
<node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
<param name="config_file" type="string" value="$(arg config_path)" />
<param name="visualization_shift_x" type="int" value="0" />
<param name="visualization_shift_y" type="int" value="0" />
<param name="skip_cnt" type="int" value="0" />
<param name="skip_dis" type="double" value="0" />
</node>
</launch>
理解:运行.launch 文件 = 运行节点 = 运行对应的main.c文件 = 运行一个相对独立的工程,包括很多.c 和.h文件, 独立的工程中包括独立的CMakeLists.txt 和 package.xml 文件。 各个节点之间通过 通信方式连接
第二个终端:
source ~/vins/devel/setup.bash //配置环境变量 ,,注意是vins ,这与之前创建的工作空间名字有关
roslaunch vins_estimator vins_rviz.launch //启动rviz
<launch>
<node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins_estimator)/../config/vins_rviz_config.rviz" />
</launch>
其中启动了rviz,对应的配置文件为vins_rviz_config.rviz。
rviz启动界面如下图所示.
第三个终端:
source ~/vins/devel/setup.bash //配置环境变量, vins 是之前创建文件夹
rosbag play vins/Dates/MH_01_easy.bag //我的bag数据集放在vins/Dates文件夹下面了
运行结果
若想看到真实的轨迹,可以打开第四个终端:
roslaunch benchmark_publisher publish.launch sequence_name:=MH_01_easy //没有效果,不知道为啥
解决方案一:
回去再次编译:
catkin_make
添加环境变量:
source ./devel/setup.bash
解决方案二:
打开这个文件:
sudo gedit ~/.bashrc
在该文件的最后添加如下代码
vins为我的ROS工作路径。
source ~/vins/devel/setup.bash
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/vins/
然后重启bashrc
source ~/.bashrc
查看ROS路径是否添加上
echo $ROS_PACKAGE_PATH
设置输出文件保存路径
因为我们现在跑的是euroc数据集,所以我们要修改的地方有两处。
euroc_config.yaml中的pose_graph_save_path项
pose_graph_save_path: "/home/kk/自己的路径/"
euroc_config.yaml中的output_path项
output_path: "/home/kk/自己的路径/"
上述""里填写自己的路径,先创建好该路径,注意最后面有个/,代表着是一个文件夹。
修改完之后,记得在ros工作空间中重新编译一下catkin_make。---记住!!!
保存地图---这一步骤有问题,会是Eigen3版本的问题吗?
没有问题,是我自己的路径写错了!
重新运行程序,待地图跑完之后,在运行 roslaunch vins_estimator euroc.launch 的terminal中,输入 “s” ,并按下回车键Enter,等待地图保存,我电脑花了20秒左右的时间。然后会出现下列信息:
pose graph path: /home/kk/happy/pose_graph_map/
pose graph saving...
save pose graph time: 22.858210 s
save pose graph finish
you can set 'load_previous_pose_graph' to 1 in the config file to reuse it next time
三、evo测评
evo工具
evo工具用过没?一个评测SLAM的工具,可以比较不同SLAM的算法精度,轨迹等等。evo支持好几种数据集的格式,tum、euroc等等 怎么装? 。安装1 测评
常见的参数如下:
- evo_config:用于保存配置文件,把自己常用的参数保存为.json文件,避免每次输入。
- evo_traj:用于绘制轨迹,支持的格式有kitti,eurco,tum 数据集等,也可以用于验证数据是否有效,导出为其他格式等。
- evo_res:可用于比较指标中的多个结果文件(打印消息和统计消息,绘制结果,将统计信息保存在表内)
- evo_ape :计算绝对位姿误差
- evo_rpe:计算相对位姿误差
修改数据格式
这边我只测试了evo_traj,也就是画出它的轨迹。问题来了,vins-mono保存的轨迹没法直接用,因为它既不符合tum数据集的格式,又不符合euroc数据集的格式。那怎么办,改呗。
修改以下文件:
- visualization.cpp中pubOdometry()函数
// write result to file
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()函数
原本是csv文件,改成txt。
VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";
好了,修改完成,重新编译catkin_make。
使用evo绘制轨迹
重新运行程序,会发现在刚刚保存地图的路径,生成了一个文件:
vins_result_loop.txt
经过我们上面的修改,该文件是符合tum格式的,虽然我们使用的是euroc数据集,但evo只支持tum格式的绘制,它提供了euroc格式转tum格式的工具。
首先我们打开数据集的state_groundtruth_estimate0/文件夹,会发现有一个文件:
data.csv。这是一个euroc格式的文件,我们首先要把他转成tum格式。输入以下命令:
evo_traj euroc data.csv --save_as_tum
生成data.tum
好了,接下来就可以绘制轨迹了!在当前目录下输入://当前目录的理解是 包含vins_result_loop.txt 和 data.tum的目录
evo_traj tum vins_result_loop.txt --ref=data.tum -p --plot_mode=xyz --align --correct_scale
真是beautiful。
其中虚线代表ground truth,蓝线代表vins的轨迹。
感谢下面连接作者提供的思路
https://blog.csdn.net/guaijiaodie2064/article/details/83041721
https://blog.csdn.net/learning_tortosie/article/details/83182258
https://blog.csdn.net/tanyong_98/article/details/103073812
https://blog.csdn.net/nannan7777/article/details/102915699
https://blog.csdn.net/Hanghang_/article/details/104535370
很好