Realsense D435基于ROS跑通ORBSLAM2

Realsense D435基于ROS跑通ORBSLAM2

系统ubuntu16.04 ROS Kinetic
相机RealsenseD435
SLAM系统：ORBSLAM2
一.安装Realsense的SDK
官方链接:https://github.com/intel-ros/realsense
步骤就按照这个步骤来就可以了，下面会说一些每一步可能遇到的坑和注意点
第1步：安装最新的英特尔®实感™SDK 2.0
SDK源码地址：https://github.com/IntelRealSense/librealsense/releases/tag/v2.16.0
按照：https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md 操作
这里说几个我自己运行时候遇到的坑
1.关于系统内核问题
确保自己的内核是4.[4,8,10,13,15,16]否则无法运行，uname -r 进行查看
我自己电脑原先是4.8内核 但是运行realsense-viewer检测不到，后来升级到4.13可以检测到摄像头，所以少走坑的话，把内核升级到4.13比较合适
2.关于无法下载libgtk-3-dev
你会发现需要很多依赖，一环套一环
打开软件和更新把更新中的重要安全更新和推荐更新点上就可以啦
第2步：安装ROS
这里就不一一说步骤啦
直接给链接：https://www.cnblogs.com/liu-fa/p/5779206.html
如果你是学生，建议你别使用校园网，教育网太坑，就好像你玩LOL应该不会去教育网专区吧？使用教育网总有些东西下不下来，安装OpenCV也是一样。如果你手头没别的网，建议你手机开热点吧。
第3步：安装reansense最新的ROS包
这里按照官网步骤一般没什么问题，在你完成上述步骤后。你应该是完成啦retalsense_ros的SDK安装，那就让我们来测试一下吧
进入到自己的工作空间，因为你已经配置环境了，可以执行以下代码启动相机节点：
roslaunch realsense2_camera rs_rgbd.launch
然后再打开一个终端，运行rviz，没有安装的话就安装一下
sudo apt-get install rviz
打开rviz
Fixed Frame 左边勾选camera link 也就是最下面的一个
再点击左下角add 增加pointcloud2 结果如图

你会发现点云图出现啦，那么这一步就已经完成啦
二.安装ORBSLAM2
源码链接： https://github.com/raulmur/ORB_SLAM2
依赖请按照GitHub上自行安装
依赖安装好后可执行指令：

git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS
chmod +x build_ros.sh
./build_ros.sh

上面的PATH为你自己存放ORBSLAM2的路径，自行修改
必须先执行./build.sh，才能执行./build_ros.sh，两者存在依赖关系
运行过程中可能会出现OpenCV库不匹配之类的，在Cmakelists.txt中进行修改，缺少库或者其他编译错误，看看有没有相应的issue或者直接百度google都可以，一般你碰到的问题，那么别人也会碰到啦，在你现在运行的时候肯定已经被解决了。
三.realsense发布topic，ORBSLAM2接收topic
在ORBSLAM2中打开ros_rgbd.cc 找到以下语句

 message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1);

 message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1);

/camera/rgb/image_raw ，camera/depth_registered/image_raw为需接收的topic名
下面是关于realsense D435 发布的相机topic
在realsense的工作空间下找到rs_rgbd.launch，在里面第109,110行加入以下两句
< remap from="/camera/aligned_depth_to_color/image_raw"to="/camera/depth_registered/image_raw" />
< remap from="/camera/color/image_raw" to="/camera/rgb/image_raw" />
运行 rs_rgbd.launch 再运行rosrun ORB_SLAM2 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE RGBD PATH_TO_VOCABULARY 是你ORB字典的路径，PATH_TO_SETTINGS_FILE是你参数的路径即Asus.yaml 里面写上你的内参和深度信息的缩放比例
那么问题来了 如何获取内参呢，realsense的api已经写好啦 需要自己获取下
简单的方法就是去下官网的realsense官网的非ROS版本的SDK在/librealsense-2.16.0/tools/data-collect/rs-data-collect.cpp语句的return前加入下述语句：

    rs2::pipeline pipe;
    rs2::pipeline_profile selection = pipe.start();
    auto depth_stream = selection.get_stream(RS2_STREAM_COLOR)
            .as();
    auto resolution = std::make_pair(depth_stream.width(), depth_stream.height());
    auto i = depth_stream.get_intrinsics();
    auto principal_point = std::make_pair(i.ppx, i.ppy);
    auto focal_length = std::make_pair(i.fx, i.fy);
    rs2_distortion model = i.model;
    cout<<"FX "<

再重新make一下，然后在build中找到./rs-data-collect.cpp用于获取内参
同理可以用realsense的双目运行ORBSLAM
以下为你可能用到语句
39 < remap from="/camera/infra1/image_rect_raw" to="/camera/left/image_raw" />
40 < remap from="/camera/infra2/image_rect_raw" to="/camera/right/image_raw" />
具体怎么做，你看了上面之后应该知道怎么做了。下面就放一下我用双目跑的ORBSLAM的部分截图吧。

再附带上我实测ORBSLAM2的轨迹效果图
下面为RGBD轨迹实测图

ORBSLAM2本身没有写上保护轨迹的代码，所以自己写了个画轨迹的代码，有需求的话自取
链接： https://download.csdn.net/download/lzw_hdu/11081474