VINS-Mono Rviz测试，详细而正确

最新推荐文章于 2024-06-07 18:40:28 发布

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

最新推荐文章于 2024-06-07 18:40:28 发布

阅读量2.9k

点赞数 11

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

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

版权

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

20 篇文章 108 订阅

订阅专栏

本人用的是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

很好