【SLAM】点线特征的VINS-Mono：PL-VINS算法测试-CSDN博客

本文链接：https://blog.csdn.net/qq_45306739/article/details/144727485

0.前言

感谢作者开源：https://github.com/cnqiangfu/PL-VINS

之前已经安装过VIns-Mono和Vins-Fusion。所以基于Vins-Mono改进的PL-VINS与前者所需环境一致，不需要额外配置。

测试系统环境为Ubuntu18.04，ROS1 Melodic，Eigen-3.3.3，Opencv-3.2.0，Ceresolver-1.14.0。

1.工程编译

基本流程跟VINS-Mono一样，非常熟悉直接写命令了：

mkdir -p ~/origin_pl_vins/src
cd ..
catkin_make
source devel/setup.bash
echo $ROS_PACKAGE_PATH
cd src
git clone https://github.com/cnqiangfu/PL-VINS.git

到这里先停一下，直接编译会feature_tracker节点会报错，显示找不到OpenCV的一些相关方法和头文件。返回到Github中发现是作者也进行了提醒，原因是在feature_tracker文件夹下Cmakelist里面将OpenCV路径写成绝对路径给写死了，所以需要改成自己的环境路径。

如果是手动编译安装的OpenCV，去找类似这个路径：opencv-3.2.0/build

如果是安装ROS Melodic自带的OpenCV3.2.0，去找这个路径：/usr/share/OpenCV，其中有OpenCVConfig.cmake和OpenCVConfig-version.cmake是确保find_package这个命令能找到包的。要么直接注释掉。

一共要改动的是三处位置：

第17行OpenCV路径：# Set(OpenCV_DIR “/usr/share/OpenCV”)
第27行头文件路径：include_directories(…)
第58行链接库命令：target_link_libraries(…)

catkin_make

上面配置文件都是测试过100%通过的。

如果你是OpenCV4系列的用户，要么重新配置环境，要么修改源代码，注意cv_bridge和一些头文件的问题。

2.EuRoc数据集测试

github上作者十分自信自己的算法，建议选择难度数据集来测试。

source ~/origin_pl_vins/devel/setup.bash
roslaunch plvins_estimator euroc_fix_extrinsic.launch

上面的指令会打开节点和rviz。

rosbag play V2_03_difficult.bag

延迟有点高啊…，前端feature_tracker呢？？

在这里插入图片描述

问题1：运行roslaunch plvins_estimator plvins-show-linepoint.launch节点进程died

这个问题是源代码中加载相机模型的yaml配置文件使用了相对路径，需要改成绝对路径：

  m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile("/home/h/origin_pl_vins/src/PL-VINS/config/euroc/euroc_config_fix_extrinsic.yaml");

这个问题会解决节点挂掉的问题。但是在rviz中不显示tracker image话题的问题还是没法解决。

问题2：rviz订阅不到tracker image，无法显示图像

这个问题比较复杂，作者好像自己的代码版本有出入，所以在坐标转换的时候发布到了其他没有的channels中，在github的一大半issue中都在提问和探讨这个话题，参考#issue8和#issue16中讨论的：

加入投影函数：

void project(cv::Point2f& pt, cv::Mat const& K)                                                                                                                                                        
 {                                                                                                                                                                                                      
     pt.x = K.at<float>(0, 0) * pt.x + K.at<float>(0, 2);                                                                                                                                               
     pt.y = K.at<float>(1, 1) * pt.y + K.at<float>(1, 2);                                                                                                                                               
 }

重新定义发布：

    cv::remap(img1, show_img, undist_map1_, undist_map2_, CV_INTER_LINEAR);
    for(int i=0; i<line_feature_msg->points.size(); i++)
    {
        // cv::Point startPoint = cv::Point(line_feature_msg->channels[3].values[i], line_feature_msg->channels[4].values[i]);
        // cv::Point endPoint = cv::Point(line_feature_msg->channels[5].values[i], line_feature_msg->channels[6].values[i]);
        cv::Point2f startPoint = cv::Point2f(line_feature_msg->points[i].x, line_feature_msg->points[i].y);
        project(startPoint, K_);
        cv::Point2f endPoint = cv::Point2f(line_feature_msg->channels[1].values[i], line_feature_msg->channels[2].values[i]);
        project(endPoint, K_);   
        cv::line(show_img, startPoint, endPoint, cv::Scalar(0, 0, 255),2 ,8);
    }

问题解决。

在这里插入图片描述

3.VECtor数据集测试

修改config和launch文件过程跟Vins-Mono一样，但是在传入话题的时候会出现问题，在image_node_b这个节点的cpp文件中，收发话题的格式被作者写死。如果不是/cam0/image_raw话题模式是接收不到的。之前的文件流输入很方便，不懂为什么要写死成固定的，是为了减少算法时间吗?-.-

  //message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/cam0/image_raw", 1000);
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/camera/left/image_mono", 1000);