PL-SLAM stvo-pl-ros 使用OAK双目相机实时运行

1.下载编译PL-StVO

GitHub - rubengooj/stvo-pl: Stereo Visual Odometry by combining point and line segment features

2.下载编译stvo-pl-ros

 下载完项目之后新建src文件夹，将stvopl_node.cpp、package.xml和CMakeLists.txt放到src文件夹中，之后

cd src     

catkin_make

 3.在编译之前需要打开 stvopl_node.cpp修改几个路径

// Initialize scene object

//------------------------------------------------------

Matrix4d Tcw, Tfw = Matrix4d::Identity(), Tfw_prev = Matrix4d::Identity(), T_inc = Matrix4d::Identity(), T_inc_l = Matrix4d::Identity();//初始化为单位阵

Vector6d cov_eig; //协方差矩阵的特征值向量

Matrix6d cov;

Tcw = Matrix4d::Identity();

Tcw << 1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 1;

XInitThreads();//这是一个 X Window System 库函数,用于初始化 X 窗口系统的线程支持

scene = sceneRepresentation("/home/lzd/study_example/PL-VO/stvo-pl/config/scene_config_indoor.ini");#160行，需要将此路径改为自己的

scene.initializeScene(Tcw,false);

mrpt::utils::CTicTac clock;

// Subscribe to input video feed and publish output video feed 订阅输入视频源，发布输出视频源

img_sub_l_ = std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>>(

new message_filters::Subscriber<sensor_msgs::Image>(

nh_, "/stereo_publisher/left/image", 1) );#167行，需要将此话题改为自己的

img_sub_r_ = std::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>>(

new message_filters::Subscriber<sensor_msgs::Image>(

nh_, "/stereo_publisher/right/image", 1) );#171行，需要将此话题改为自己的

 

4.修改完之后编译即可

5. 按照实际编写dataset_params.yaml文件，我是仿照kitti数据集格式编写的dataset_params.yaml文件

cam0:
  cam_cx: 666.816075048802      #按照相机标定结果填写
  cam_cy: 365.13825670241545  #按照相机标定结果填写
  cam_d0: 0.0
  cam_d1: 0.0
  cam_d2: 0.0
  cam_d3: 0.0
  cam_fx: 805.9707277371266        #按照相机标定结果填写
  cam_fy: 804.9165005674341        #按照相机标定结果填写
  cam_bl: 0.07383359                      #按照相机标定结果填写
  cam_height: 720
  cam_model: Pinhole
  cam_width: 2104
  rx: 0.0
  ry: 0.0
  rz: 0.0
  tx: 0.0
  ty: 0.0
  tz: 0.0
images_subfolder_l: image_2/
images_subfolder_r: image_3/

6. 进入OAK相机工作区（dai_ws）发布双目信息

7.进入build文件夹下

./vo /home/lzd/study_example/PL-VO/stvo-pl/config/dataset_params/dataset_params.yaml /home/lzd/study_example/PL-VO/stvo-pl/config/config/config_kitti.yaml

指令中的路径按照实际填写