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文件
查看效果文章来源:https://www.toymoban.com/news/detail-407594.html
pcl_viewer vslam.pcd
效果良好
至此,使用 intel D435i 构建SLAM点云地图与slam摄像头建图的启动指令优化流程结束文章来源地址https://www.toymoban.com/news/detail-407594.html
到了这里,关于ORB-SLAM2的布置(五)使用 intel D435i 构建SLAM点云地图的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!