ubuntu16.04 orbslam2 realsensed415

参考博客：

ROS+ORB-SLAM2+RealsenseD415_Escape_x1n的博客-CSDN博客

1、配置ros环境

mkdir -p orbslam_ws/src

catkin_init_workspace

cd orbslam_ws

catkin_make

在主文件夹下ctrl+h后找到隐藏文件夹.bashrc，在.bashrc末尾添加：

source /opt/ros/kinetic/setup.bash

source /home/sun/orbslam_ws/devel/setup.bash

说明：该部分的作用是建立一个工作空间

2、安装ROS usb_cam

cd src

git clone https://github.com/bosch-ros-pkg/usb_cam

cd ..

catkin_make

source devel/setup.bash

测试流程

1）新开终端：roscore

2）运行：roslaunch usb_cam usb_cam-test.launch出现画面。

说明：测试阶段会用到电脑自带的摄像头，如果电脑没有自带的摄像头会报错，此时不用管，继续进行即可。

3、编译ORB-SLAM2

3.1

copy ORB_SLAM2-master

cd orbslam_ws/src/ORB_SLAM2

source /home/sun/orbslam_ws/devel/setup.bash

chmod +x build.sh

./build.sh

chmod +x build_ros.sh

./build_ros.sh

说明：此时只需要运行./build.sh和./build_ros.sh即可，不需要按照ROS机器人开发实践中的执行sudo make install,会报错误而且没用。

3.2

修改ros_mono.cc （或者ros_mono_ar.cc）

进入~/orbslam_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src

打开 ros_mono.cc 把程序里面的topic改为 /usb_cam/image_raw(也就是： ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);

具体还要查看自己的rviz（即上一步打开的摄像头窗口的标题）

然后进入~/orbslam_ws/src/ORB_SLAM，重新编译./build_ros.sh

3.3

修改ros_rgbd.cc

把rgb_topic和depth_topic订阅话题修改为

 "/camera/color/image_raw";

 "/camera/aligned_depth_to_color/image_raw";

重新编译:

source /home/sun/orbslam_ws/devel/setup.bash

chmod +x build.sh

./build.sh

chmod +x build_ros.sh

./build_ros.sh

4、启动RGB-D相机 realsense D415

4.1 获得相机内参矩阵

they’re published on a topic in the realsense ROS node. We can get those parameters by running rostopic echo /camera/color/camera_info

打开三个终端

roscore

roslaunch realsense2_camera rs_rgbd.launch

rostopic echo /camera/color/camera_info

运行之后的结构长这样

header:

  seq: 2053

  stamp:

    secs: 1561365943

    nsecs:  92858932

  frame_id: "camera_color_optical_frame"

height: 480

width: 640

distortion_model: "plumb_bob"

D: [0.0, 0.0, 0.0, 0.0, 0.0]

K: [616.58837890625, 0.0, 310.9554138183594, 0.0, 616.196044921875, 234.5266876220703, 0.0, 0.0, 1.0]

R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]

P: [616.58837890625, 0.0, 310.9554138183594, 0.0, 0.0, 616.196044921875, 234.5266876220703, 0.0, 0.0, 0.0, 1.0, 0.0]

binning_x: 0

binning_y: 0

roi:

  x_offset: 0

  y_offset: 0

  height: 0

  width: 0

  do_rectify: False

K就是我们得到的内参矩阵写成了向量模式：

K = [fx 0 cx 0 fy cy 0 0 1 ]

然后就是baseline，根据官方datasheet，D435 的baseline为55mm，bf的值为bf = baseline (in meters) * fx。

根据相机参数可以得到d415.yaml文件。

4.2运行SLAM

roscore

roslaunch realsense2_camera rs_rgbd.launch

rosrun ORB_SLAM2 RGBD /home/sun/orbslam_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/sun/orbslam_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/d415.yaml

5.标定环节，待更