在ROS上发布ORB_SLAM2的位姿信息过程及遇到的问题

因为我的实验中要把ORB_SLAM2估计出的位姿信息提供给无人机使用，因此要创建一个ROS发布者实时发布ORB_SLAM2的位姿信息。发布位姿的源码我们已经共享到了github上：
https://github.com/TGUMobileVision/MonocularUAV

写好相应的发布位姿的源文件之后，编译时遇到错误：

问题1.运行build_ros.sh文件时出错：

chmod +x build_ros.sh
./build_ros.sh

错误信息如下：

/home/acl/SLAM/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/lib/SlamData.cc: In constructor ‘ORB_SLAM2::SlamData::SlamData(ORB_SLAM2::System*, ros::NodeHandle*, bool)’:
/home/acl/SLAM/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/lib/SlamData.cc:14:29: error: ‘class ORB_SLAM2::System’ has no member named ‘GetpFrameDrawer’
     mpFrameDrawer = mpSLAM->GetpFrameDrawer();
                             ^
CMakeFiles/Mono.dir/build.make:119: recipe for target 'CMakeFiles/Mono.dir/lib/SlamData.cc.o' failed
make[2]: *** [CMakeFiles/Mono.dir/lib/SlamData.cc.o] Error 1
CMakeFiles/Makefile2:718: recipe for target 'CMakeFiles/Mono.dir/all' failed
make[1]: *** [CMakeFiles/Mono.dir/all] Error 2
Makefile:127: recipe for target 'all' failed
make: *** [all] Error 2

解决：打开SlamData.cc文件，注释掉第十四行，如下：

然后再运行

./build_ros.sh

编译完成。

问题2：ROS上运行ORB时出错：
运行以下指令来启动ORB_SLAM2：

rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml true

错误信息如下：

[e-mail]:~/SLAM/src/ORB_SLAM2$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml true
[ WARN] [1568696794.593679960]: bEnablePublishROSTopic not set, use default value: Publishing ROS topics
terminate called after throwing an instance of 'std::logic_error'
  what():  basic_string::_M_construct null not valid
已放弃 (核心已转储)

解决：看代码，打开ros_mono.cc文件，发现里面的第74-78行说的是这个错误，如下：

所以，将指令改为

rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml true true

成功运行。

此时经过我修改的ORB_SLAM2已经可以实时发布位姿信息了，但是源码里ORB_SLAM2默认订阅的图像信息话题并非我所使用过的D435i的话题，所以要修改订阅的话题，使ORB_SLAM2使用我自己相机来运行，即订阅D435i的图像话题：
ORB_SLAM2订阅D435i发布的图片信息话题
先运行相机，然后查看相机发布的所有话题，话题列表如下图：

可以看到有个话题名称为/camera/color/image_raw
然后修改ros_mono.cc文件中订阅图片话题的代码，即第99行，把/camera/image_raw改为/camera/color/image_raw，保存，然后在ORB_SLAM2目录下./build_ros.sh，完成后运行指令

rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml true true

然后打开相机：

roslaunch realsense2_camera rs_rgbd.launch

完成。

此时是已经在使用我自己的D435i相机运行ORB_SLAM2并实时发布位姿信息了，下面我们来查看一下发布的位姿信息内容：
查看ORB_SLAM2发布的位姿信息：
在输出位姿之前，topic list如下

[e-mail]:~/SLAM/src/ORB_SLAM2$ rostopic list
/camera/accel/imu_info
/camera/accel/sample
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/aligned_depth_to_infra1/camera_info
/camera/aligned_depth_to_infra1/image_raw
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_rect_color
/camera/color_rectify_color/parameter_descriptions
/camera/color_rectify_color/parameter_updates
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth_registered/points
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/extrinsics/depth_to_infra2
/camera/gyro/imu_info
/camera/gyro/sample
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/rosout
/rosout_agg
/tf_static

发布信息之后的话题：另外打开一个终端，输入topic list，显示的topic如下

[e-mail]:~$ rostopic list
/camera/accel/imu_info
/camera/accel/sample
/camera/aligned_depth_to_color/camera_info
/camera/aligned_depth_to_color/image_raw
/camera/aligned_depth_to_infra1/camera_info
/camera/aligned_depth_to_infra1/image_raw
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_rect_color
/camera/color_rectify_color/parameter_descriptions
/camera/color_rectify_color/parameter_updates
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth_registered/points
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/gyro/imu_info
/camera/gyro/sample
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/current_frame
/diagnostics
/incremental_pose_cov
/point_cloud_all
/point_cloud_ref
/posestamped
/rosout
/rosout_agg
/tf
/tf_static

可以发现多了/current_frame、/incremental_pose_cov、/point_cloud_all、/point_cloud_ref、/posestamped、/tf 这几个话题。其中/posestamped就是刚刚写的实时发布ORB_SLAM2位姿的话题。
然后用rostopic echo +话题 来查看具体某一话题的具体信息

比如

rostopic echo /posestamped

显示的信息如下：

header: 
  seq: 21117
  stamp: 
    secs: 1568856069
    nsecs: 795999328
  frame_id: "world"
pose: 
  position: 
    x: 0.0836435705423
    y: 0.00201921607368
    z: 0.0229973811656
  orientation: 
    x: 0.00288535521108
    y: -0.169365454415
    z: 0.00576285751004
    w: 0.985532245565
---

用rostopic echo /incremental_pose_cov 显示的消息如下

header: 
  seq: 21229
  stamp: 
    secs: 1568856075
    nsecs:  68009445
  frame_id: "keyframe_21229"
pose: 
  pose: 
    position: 
      x: -0.000474977126805
      y: 0.000119834308721
      z: 0.00053258004294
    orientation: 
      x: 6.81013483689e-05
      y: 0.000157509826337
      z: 1.28372972071e-06
      w: 0.999999985276
  covariance: [0.0005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
---