ORB-SLAM2(3) ROS下实时跑ORB_SLAM2

Step1 : 运行内核

roscore

Step2 : 启动相机

cd catkin_ws/src/usb_cam/launch  #进入usb_cam驱动的安装目录
roslaunch my_cam.launch 
#roslaunch usb_cam usb_cam-test.launch

my_cam.launch文件

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video1" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

• 自己的路径,自己的launch文件名
•       

Step3 ： 运行orb_slam2 

运行官方的

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

其中包含两个参数：
PATH_TO_VOCABULARY：视觉词典，一般可以直接使用catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt
PATH_TO_SETTINGS_FILE：摄像头内参，可以使用路径catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml，但需要将Asus.yaml中的参数改成自己使用摄像头标定后的参数，摄像头标定可以使用opencv或matlab工具箱。

运行自己的

#如果路径还没配置需要以下两行添加ROS环境
cd /home/hm/catkin_ws/src/ORB_SLAM2  #进入ORB_SLAM2的主目录
#source Examples/ROS/ORB_SLAM2/build/devel/setup.bash
cd Examples/ROS/
rosrun ORB_SLAM2 Mono ../../Vocabulary/ORBvoc.txt ../../my_camera.yaml #自己的相机标定文件

my_camera.yaml #自己的相机标定文件

image_width: 640
image_height: 480
camera_name: head_camera
camera_matrix:
  rows: 3
  cols: 3
  data: [684.9063320280035, 0, 333.8552962590653, 0, 685.8214305739915, 258.3133970146105, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.08540487526140211, -0.4947463318653558, 0.002070145615834643, 1.739354152579926e-05, 0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 4
  data: [679.8218383789062, 0, 334.2231118845648, 0, 0, 686.0590209960938, 258.4570282812347, 0, 0, 0, 1, 0]

修改话题

usb_cam默认话题 为usb_cam/image_raw

ros订阅的图像默认为 image/image_raw

进到catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src 打开ros_mono.cc
将subscribe的话题改为/usb_cam/image_raw,将话题接上

  ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

重新编译ROS的example

 cd ~/catkin_ws/src/ORB_SLAM2
    chmod +x build_ros.sh
    ./build_ros.sh

重新编译工作空间

cd ~/catkin_ws
catkin_make

再来一次：

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE

问题:

权限问题: 一直提示无法打开参数文件

解决方法: 直接将两个参数文件,复制到同目录下,还不行需要分别给权限

 sudo chmod 777 Asus.yaml 

 sudo chmod 777 ORBvoc.txt

最后出图: