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

 

最后出圖: