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

卑微小熊

已于 2023-04-17 08:57:00 修改

阅读量4.3k

点赞数 6

文章标签： ubuntu 计算机视觉 linux

于 2022-12-29 09:39:40 首次发布

本文链接：https://blog.csdn.net/Xiong2840/article/details/128470582

版权

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.3554992675781

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0

Camera.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.3554992675781

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0

Camera.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];

#########2023年04月17日 08:54:17 UTC+8:00#########
#########由于原先的截图有歧义，更换新的图片#########
在这里插入图片描述

然后编译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摄像头建图的启动指令优化流程结束

卑微小熊

关注

6
点赞
踩
78

收藏

觉得还不错? 一键收藏
5
评论
ORB-SLAM2的布置（五）使用 intel D435i 构建SLAM点云地图

Intel RealSense SDK 2.0 是跨平台的开发套装，包含了基本的相机使用工具如 realsense-viewer，也为二次开发提供了丰富的接口，包括 ROS，python , Matlab, node.js, LabVIEW, OpenCV, PCL, .NET 等。在启动相机之前，我们需要设置一下 realsense2_camera rospack 中的 rs_camera.launch 的文件。其中，里面包含了相机内参，可以通过以下获取，每个相机的参数可能会有差别。
复制链接

扫一扫