ORB-SLAM2的布置（五）使用 intel D435i 构建SLAM点云地图

Intel RealSense SDK 2.0 是跨平台的开发套装，包含了基本的相机使用工具如 realsense-viewer，也为二次开发提供了丰富的接口，包括 ROS，python , Matlab, node.js, LabVIEW, OpenCV, PCL, .NET 等。

这次使用的摄像头是D435i
它可以提供深度和 RGB 图像，而且带有 IMU
本次流程就是想使用D435i摄像头进行SLAM的点云建图

进行安装Intel RealSense SDK，查看摄像头的数据
Linux/Ubuntu - 实感 SDK 2.0 构建指南

安装完成后，查看相机的深度和 RGB 图像

realsense-viewer

然后这里下载驱动 intel 的各个摄像头的 ROS 框架的驱动功能包
适用于英特尔®实感™设备的 ROS 封装器

当安装完成后便可以启动看看摄像头的数据

我们这里继续配置SMAL
在启动相机之前，我们需要设置一下 realsense2_camera rospack 中的 rs_camera.launch 的文件

前者是让不同传感器数据（depth, RGB, IMU）实现时间同步，即具有相同的 timestamp;
后者会增加若干 rostopic，其中我们比较关心的是/camera/aligned_depth_to_color/image_raw，这里的 depth 图像与 RGB 图像是对齐的

  <arg name="enable_sync"               default="true"/><arg name="align_depth"               default="true"/>

完成后，就可以用如下命令启动相机

roslaunch realsense2_camera rs_camera.launch

其中关键的是 /camera/color/image_raw 和/camera/aligned_depth_to_color/image_raw
分别对应 RGB 图像和深度图像

然后在Examples/RGB-D中，新建一个名为RealSense.yaml的参数文件，

然后进行编辑

#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 909.3504638671875
Camera.fy: 908.5739135742188
Camera.cx: 645.5443725585938
Camera.cy: 369.3554992675781Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0Camera.width: 1280
Camera.height: 720#Camera frames per second
Camera.fps: 30.0#IR projector baseline times fx (aprox.)
Camera.bf: 46.01#Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1#Close/Far threshold. Baseline times.
ThDepth: 40.0#Deptmap values factor，将深度像素值转化为实际距离，原来单位是 mm，转化成 m
DepthMapFactor: 1000.0#ORB Parameters
#--------------------------------------------------------------------------------------------#ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000#ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2#ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8#ORB Extractor: Fast threshold
#Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
#Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
#You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7#--------------------------------------------------------------------------------------------
#Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

其中，里面包含了相机内参，可以通过以下获取，每个相机的参数可能会有差别

rostopic echo /camera/color/camera_info

这里主要在yaml中注意的是

# 相机标定和畸变参数（OpenCV）

要根据话题【/camera/color/camera_info】的数据进行修改
参数可以对照网址查看具体的各参数信息
http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html

也就是以下的参数

Camera.fx: 909.3504638671875
Camera.fy: 908.5739135742188
Camera.cx: 645.5443725585938
Camera.cy: 369.3554992675781Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0Camera.width: 1280
Camera.height: 720

完成后保存并退出

然后修改src/pointcloudmapping.cc文件

1.第128行附近

 voxel.setLeafSize (0.02f, 0.02f, 0.02f);        //Modified place 1 调节点云密度

2，第75行左右

            p.y = ( m - kf->cy) * p.z / kf->fy;// p.y = - ( m - kf->cy) * p.z / kf->fy;   //Modified place2 与原先添加了负号,将原本颠倒的点云地图上下翻转，方便观察 【后续测试显示的pcd完全无法使用。不进行上下翻转】p.r = color.ptr<uchar>(m)[n*3];         //Modified place3 修改颜色显示p.g = color.ptr<uchar>(m)[n*3+1];p.b = color.ptr<uchar>(m)[n*3+2];

然后编译ORB_SLAM2_modified

./build.sh
./build_ros.sh

完成编译后进行测试

启动主节点

roscore

启动摄像头

roslaunch realsense2_camera rs_camera.launch

进行建图,发现报错

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.bin Examples/RGB-D/RealSense.yaml /camera/rgb/image_raw:=/camera/color/image_raw /camera/depth_registered/image_raw:=/camera/aligned_depth_to_color/image_raw

经过查找可能是yaml文件还需要修改
opencv4.0读取yaml文件错误 error: (-49:Unknown error code -49) Input file is empty in function ‘open’

需要在yaml文件顶端添添加

%YAML:1.0

重新运行

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.bin Examples/RGB-D/RealSense.yaml /camera/rgb/image_raw:=/camera/color/image_raw /camera/depth_registered/image_raw:=/camera/aligned_depth_to_color/image_raw

当CTRL+C后,可以看到输出的pcd文件

查看保存的pcd文件

pcl_viewer vslam.pcd

至此,基于深度相机 Intel RealSense D435i 实现 ORB SLAM 2 的流程成功

不过由于启动SLAM的指令有点复杂,使用roslauch进行优化
首先是离线包的启动

当启动的命令没有输全的时候,终端会输出使用的用法

Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings

首先建立一个功能包，命名为 slam

cd ~/catkin_ws/src
catkin_create_pkg slam std_msgs rospy roscpp

然后在这个功能包中创建launch文件，我命名为rgbd.launch
进行编辑

<launch><!--定义全局参数--><arg name="rgb_image" default="/camera/rgb/image_raw"/><arg name="path_to_vacabulary" default="/home/heying/ORB_SLAM2_modified/Vocabulary/ORBvoc.bin"/><arg name="path_to_settings" default="/home/heying/ORB_SLAM2_modified/Examples/RGB-D/TUM1_ROS.yaml"/><!--播放离线包--><!-- 注意这里bag文件的路径必须为绝对路径--><node pkg="rosbag" type="play" name="player" output="screen"args=" /home/heying/ORB_SLAM2_modified/Examples/datasets/rgbd_dataset_freiburg1_xyz.bag"><remap from="/camera/rgb/image_color" to="/camera/rgb/image_raw" /><remap from="/camera/depth/image" to="/camera/depth_registered/image_raw" /></node><!--启动ORB-SLAM2 RGBD--><node name ="RGBD" pkg="ORB_SLAM2" type="RGBD"args="$(arg path_to_vacabulary) $(arg path_to_settings)" respawn="true" output="screen"><!--remap from="/camera/image_raw" to="$(arg rgb_image)"/--></node>
</launch>

完成后测试

roslaunch slam rgbd.launch

正常启动

实时摄像头

再新建一个launch文件，用于摄像头的启动于建图

touch  camera.launch
gedit camera.launch

然后进行编写

首先是全局参数，先不要打全指令，可以看到输出了使用方法，出来的path_to_vocabulary和path_to_settings便是需要设置的全局参数

rosrun ORB_SLAM2 RGBD

<launch><!--定义全局参数--><arg name = "path_to_vocabulary" default = "/home/heying/ORB_SLAM2_modified/Vocabulary/ORBvoc.bin"/><arg name = "path_to_settings" default = "/home/heying/ORB_SLAM2_modified/Examples/RGB-D/RealSense.yaml"/><!--include file="$(find realsense2_camera)/launch/rs_camera.launch"/--><!--启动ORB-SLAM2 RGBD--><node pkg = "ORB_SLAM2" type ="RGBD" name = "RGBD_camera" args="$(arg path_to_vocabulary) $(arg path_to_settings)" respawn="true" output="screen"><remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" /><remap from="/camera/depth_registered/image_raw" to="/camera/aligned_depth_to_color/image_raw" /></node></launch>

测试
开启主节点

roscore

然后检查摄像头的硬件连接

启动摄像头

roslaunch realsense2_camera rs_camera.launch

启动建图

roslaunch slam camera.launch

启动正常，效果简直完美

在程序CTRL+C后会生成pcd文件

查看效果

pcl_viewer vslam.pcd

效果良好

至此，使用 intel D435i 构建SLAM点云地图与slam摄像头建图的启动指令优化流程结束

ORB-SLAM2的布置（五）使用 intel D435i 构建SLAM点云地图相关推荐

ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
orbslam2可视化_[Ubuntu] ORB SLAM2 编译调试
ORB SLAM2 是 2015年比较受到关注的一篇文章,它的主要思想是借助 ORB 描述子改进了 Sparse SLAM 的性能,使得其在稳定性和速度上都达到了比较好的程度.从创新性上来讲,它的主要 ...
Ubuntu下使用单目相机运行ORB SLAM2
环境:Ubuntu16.04+ROS Kinetic+USB单目摄像头虽然ORB SLAM2的官方说明中表示没有ROS也可以编译运行,但要实时的跑ORB SLAM2还是需要ROS平台的,所以之前没有 ...
ROS kinetic环境使用Realsense D435i获取三维点云并存为.pcd文件
ROS kinetic环境使用Realsense D435i获取三维点云并存为.pcd文件二进制安装D435的SDK 下载intel Realsense ROS工作空间 ROS下驱动D435i获得点 ...
Gradle 1.12用户指南翻译——第五十四章. 构建原生二进制文件
其他章节的翻译请参见: http://blog.csdn.net/column/details/gradle-translation.html 翻译项目请关注Github上的地址: https://g ...
【AngularJs学习笔记五】AngularJS从构建项目开始
为什么80%的码农都做不了架构师?>>> #0 系列目录# AngularJs学习笔记 [AngularJs学习笔记一]Bower解决js的依赖管理 [AngularJs学习笔 ...
多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...
realsense D435i gazebo slam(px4)仿真（转载）
转载自:https://blog.csdn.net/weixin_41469272/article/details/117919845?utm_medium=distribute.pc_relevan ...
学习 Intel 线程构建块开源库（TBB）
原文转载于:https://blog.csdn.net/zhu2695/article/details/51247267 学习 Intel 线程构建块开源库简介我们发现了 POSIX 线程和基于 ...

ORB-SLAM2的布置（五）使用 intel D435i 构建SLAM点云地图

ORB-SLAM2的布置（五）使用 intel D435i 构建SLAM点云地图相关推荐

最新文章

热门文章