使用D435i相机录制数据集跑ORB-SLAM2稠密建图_m0_60355964_d435i制作tum数据集

未知 02-07 2197

目录前言系统版本一、录制视频二、提取rgb和depth图片1.2.3. 三、测试数据集

前言

本文写于2022年7月24日。

系统版本

Ubuntu18.04 + ROS melodic

一、录制视频

这一步需要使用InterRealSenseD435i SDK2，可以参考此链接安装。打开Intel RealSense Viewer。设置Depth Stream以及Color Stream的图像分辨率为640 × 480，设置采集帧率为30 fps。点击左上角的Record按钮即可进行录制，开始录制后，点击左上角的Stop按钮即可结束录制并保存录制结果。

若点击Record按钮后出现以下报错，则更改一下保存路径。

UNKNOWN in rs2_create_record_device_ex(device:0x7f151c000b20, file:/): Error opening file: /home/d/Documents/20220723_223742.bag

点击右上角的齿轮图标，选择Settings，然后改变存储路径，之后点击Apply，OK。

结束录制后，在相应存储路径下即生成.bag文件。

二、提取rgb和depth图片 1.

首先，进入bag文件的存储路径并打开终端，通过rosbag info 20220723_202328.bag查看待提取的深度图及彩色图所对应的 topic，如下图所示：新建文件夹room（此名称随意），在此文件夹下新建文件夹rgb和depth保存提取出来的深度图和彩色图，同时新建文件rgb.txt和depth.txt为对齐时间戳做准备。

执行以下python脚本提取图像.py 注：我保存的路径出现了中文，所以在第一行加入了# -*- coding: utf-8 -*-，否则会报错SyntaxError: Non-ASCII character '\xe8' in file 参考链接

# -*- coding: utf-8 -*- import roslib import rosbag import rospy import cv2 import os from sensor_msgs.msg import Image from cv_bridge import CvBridge from cv_bridge import CvBridgeError rgb = '/home/d/视频/room/rgb/' #rgb path depth = '/home/d/视频/room/depth/' #depth path bridge = CvBridge() file_handle1 = open('/home/d/视频/room/depth.txt', 'w') file_handle2 = open('/home/d/视频/room/rgb.txt', 'w') with rosbag.Bag('/home/d/视频/20220723_202328.bag', 'r') as bag: for topic,msg,t in bag.read_messages(): if topic == "/device_0/sensor_0/Depth_0/image/data": #depth topic cv_image = bridge.imgmsg_to_cv2(msg) timestr = "%.6f" % msg.header.stamp.to_sec() #depth time stamp image_name = timestr+ ".png" path = "depth/" + image_name file_handle1.write(timestr + " " + path + '\n') cv2.imwrite(depth + image_name, cv_image) if topic == "/device_0/sensor_1/Color_0/image/data": #rgb topic cv_image = bridge.imgmsg_to_cv2(msg,"bgr8") timestr = "%.6f" % msg.header.stamp.to_sec() #rgb time stamp image_name = timestr+ ".jpg" path = "rgb/" + image_name file_handle2.write(timestr + " " + path + '\n') cv2.imwrite(rgb + image_name, cv_image) file_handle1.close() file_handle2.close()

需要将以下几行路径改成自己对应的路径

rgb = '/home/d/视频/room/rgb/' #rgb path depth = '/home/d/视频/room/depth/' #depth path bridge = CvBridge() file_handle1 = open('/home/d/视频/room/depth.txt', 'w') file_handle2 = open('/home/d/视频/room/rgb.txt', 'w') with rosbag.Bag('/home/d/视频/20220723_202328.bag', 'r') as bag:

改好了之后打开终端输入以下指令执行python脚本

python 提取图像.py 3.

由于深度图及彩色图的时间戳并非严格一一对齐，存在一定的时间差，因此需将深度图及彩色图按照时间戳最接近原则进行两两配对。将associate.py脚本文件存储至room文件夹下，如图所示：

associate.py脚本文件：

""" The RealSense provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images. For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches. """ import argparse import sys import os import numpy def read_file_list(filename): """ Reads a trajectory from a text file. File format: The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. Input: filename -- File name Output: dict -- dictionary of (stamp,data) tuples """ file = open(filename) data = file.read() lines = data.replace(",", " ").replace("\t", " ").split("\n") list = [[v.strip() for v in line.split(" ") if v.strip() != ""] for line in lines if len(line) > 0 and line[0] != "#"] list = [(float(l[0]), l[1:]) for l in list if len(l) > 1] return dict(list) def associate(first_list, second_list, offset, max_difference): """ Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim to find the closest match for every input tuple. Input: first_list -- first dictionary of (stamp,data) tuples second_list -- second dictionary of (stamp,data) tuples offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) max_difference -- search radius for candidate generation Output: matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) """ first_keys = first_list.keys() second_keys = second_list.keys() potential_matches = [(abs(a - (b + offset)), a, b) for a in first_keys for b in second_keys if abs(a - (b + offset)) < max_difference] potential_matches.sort() matches = [] for diff, a, b in potential_matches: if a in first_keys and b in second_keys: first_keys.remove(a) second_keys.remove(b) matches.append((a, b)) matches.sort() return matches if __name__ == '__main__': # parse command line parser = argparse.ArgumentParser(description=''' This script takes two data files with timestamps and associates them ''') parser.add_argument('first_file', help='first text file (format: timestamp data)') parser.add_argument('second_file', help='second text file (format: timestamp data)') parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true') parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)', default=0.0) parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)', default=0.02) args = parser.parse_args() first_list = read_file_list(args.first_file) second_list = read_file_list(args.second_file) matches = associate(first_list, second_list, float(args.offset), float(args.max_difference)) if args.first_only: for a, b in matches: print("%f %s" % (a, " ".join(first_list[a]))) else: for a, b in matches: print("%f %s %f %s" % (a, " ".join(first_list[a]), b - float(args.offset), " ".join(second_list[b])))

在该路径下打开终端并通过执行如下命令生成配对结果associate.txt

python associate.py rgb.txt depth.txt > associate.txt

至此，数据集制作完成。

三、测试数据集

使用高翔的ORB-SLAM2稠密点云的代码，高翔ORB-SLAM2稠密建图编译参考链接

./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/MyD435i.yaml /home/d/视频/room /home/d/视频/room/associate.txt

结果如下图： MyD435i.yaml文件

%YAML:1.0 #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- # Camera calibration and distortion parameters (OpenCV) Camera.fx: 910.099731 Camera.fy: 909.994873 Camera.cx: 639.493347 Camera.cy: 359.377410 Camera.k1: 0.0 Camera.k2: 0.0 Camera.p1: 0.0 Camera.p2: 0.0 Camera.p3: 0.0 Camera.width: 640 Camera.height: 480 # Camera frames per second Camera.fps: 30.0 # IR projector baseline times fx (aprox.) # bf = baseline (in meters) * fx, D435i的 baseline = 50 mm Camera.bf: 50.0 # 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 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 PointCloudMapping.Resolution: 0.01 meank: 50 thresh: 2.0