使用D435i相机录制TUM格式的数据集

摆烂女侠

已于 2023-03-27 15:06:43 修改

阅读量3.2k

点赞数 7

文章标签：计算机视觉 opencv 数码相机

于 2023-03-23 16:25:26 首次发布

本文链接：https://blog.csdn.net/m0_60355964/article/details/129518283

版权

前言

本文写于2023年3月14日。D435i相机的rgb图像与depth图像的像素没有对齐，在此记录一下如何像素对齐

系统版本

Ubuntu18.04 + ROS melodic

一、使用realsense SDK录制bag包的情况

1.录制视频

这一步需要使用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文件。
在这里插入图片描述

2.、提取rgb和depth图片

1.

首先，进入bag文件的存储路径并打开终端，通过rosbag info 20220723_202328.bag查看待提取的深度图及彩色图所对应的 topic，如下图所示：
在这里插入图片描述

新建文件夹room（此名称随意），在此文件夹下新建文件夹rgb和depth保存提取出来的深度图和彩色图，同时新建文件rgb.txt和depth.txt为对齐时间戳做准备。

2.

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

这部分在原有的提取图片的基础上添加了像素对齐的功能
参考链接：
Realsense相机在linux下的配置使用，RGB与depth图像对齐（此博客有对齐图像与非对齐图像的比较）
realsense深度图像读取对齐与保存（下面程序的框架来源）
realsense bag文件时间戳获取（下面程序时间戳获取来源）

#coding=utf-8
import pyrealsense2 as rs
import numpy as np
import cv2
import time
import os
#像素对齐了
pipeline = rs.pipeline()

#Create a config并配置要流式传输的管道
config = rs.config()
config.enable_device_from_file("/home/d/下载/20221128_161053.bag")#这是打开相机API录制的视频
# config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
# config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

profile = pipeline.start(config)

depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: " , depth_scale)

align_to = rs.stream.color
align = rs.align(align_to)

# 保存路径
save_path = '/home/d/文档/数据集/自己录的D435i/table/'

# 保存的图片和实时的图片界面
# cv2.namedWindow("live", cv2.WINDOW_AUTOSIZE)
# cv2.namedWindow("save", cv2.WINDOW_AUTOSIZE)
number = 0

file_handle1 = open('/home/d/文档/数据集/自己录的D435i/table/rgb.txt', 'w')
file_handle2 = open('/home/d/文档/数据集/自己录的D435i/table/depth.txt', 'w')

# 主循环
try:
    while True:
        #获得深度图的时间戳
        frames = pipeline.wait_for_frames()
        number = number + 1
        depth_timestamp = "%.6f" % (frames.timestamp / 1000)
        rgb_timestamp = "%.6f" % (frames.timestamp / 1000 + 0.000017)#对比了 提取图片.py 的时间戳，发现rgb比depth多0.000017

        aligned_frames = align.process(frames)
        #获取对齐后的深度图与彩色图
        aligned_depth_frame = aligned_frames.get_depth_frame()
        color_frame = aligned_frames.get_color_frame()

        if not aligned_depth_frame or not color_frame:
            continue
        
        depth_data = np.asanyarray(aligned_depth_frame.get_data(), dtype="float16")
        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())
        color_image = cv2.cvtColor(color_image,cv2.COLOR_BGR2RGB)
        depth_mapped_image = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.05), cv2.COLORMAP_JET)
        
        #下面两行是opencv显示部分
        # cv2.imshow("live", np.hstack((color_image, depth_mapped_image)))
        # key = cv2.waitKey(30)

        rgb_image_name = rgb_timestamp+ ".png"
        depth_image_name = depth_timestamp+ ".png"
        rgb_path = "rgb/" + rgb_image_name
        depth_path = "depth/" + depth_image_name
        # rgb图片路径及图片保存为png格式
        file_handle1.write(rgb_timestamp + " " + rgb_path + '\n')
        cv2.imwrite(save_path + rgb_path, color_image)
        # depth图片路径及图片保存为png格式
        file_handle2.write(depth_timestamp + " " + depth_path + '\n')
        cv2.imwrite(save_path + depth_path, depth_image)
        print(number, rgb_timestamp, depth_timestamp)
        #cv2.imshow("save", np.hstack((saved_color_image, saved_depth_mapped_image)))

        #查看话题包有多少帧图片，下面就改成多少
        if number == 2890:
            cv2.destroyAllWindows()
            break    
finally:
    pipeline.stop()

file_handle1.close()
file_handle2.close()

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

config.enable_device_from_file("/home/d/下载/20221128_161053.bag")

save_path = '/home/d/文档/数据集/自己录的D435i/table/'

file_handle1 = open('/home/d/文档/数据集/自己录的D435i/table/rgb.txt', 'w')
file_handle2 = open('/home/d/文档/数据集/自己录的D435i/table/depth.txt', 'w')

还有判断停止的条件

#查看话题包有多少帧图片，下面就改成多少
if number == 2890:

改好了之后打开终端输入以下指令执行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

至此，数据集制作完成。

二、用realsense-ros打开相机录制bag包

1.将深度图对齐到RGB

打开realsense-ros/realsense2_camera/launch/rs_d435_camera_with_model.launch文件，将align_depth的值改为true
在这里插入图片描述

2.使用realsense-ros打开相机

切换到安装realsense-ros的工作空间

source devel/setup.bash
roslaunch realsense2_camera rs_d435_camera_with_model.launch

之后新建终端查看rostopic list，/camera/aligned_depth_to_color/image_raw就是对齐后的深度图像话题
在这里插入图片描述

3.录制rosbag

参考链接：如何优雅的录制ROS的rosbag包？

1.直接使用命令

rosbag record -O xxx.bag topic-name

根据上面的格式，我把路径补全，并且选择监听的话题，因此我使用下面的命令

rosbag record -O /home/d/1.bag /camera/color/image_raw /camera/aligned_depth_to_color/image_raw

录制结束按Ctrl+C暂停

2.写一个launch文件

<launch>
	<node pkg="rosbag" type="record" name="bag_record" args="topic-name1 topic-name1 -O xxxx"/>                                                                                                
</launch>

根据上面的格式，我把路径补全，并且选择监听的话题，因此我launch文件改成下面的

<launch>
	<node pkg="rosbag" type="record" name="bag_record" args="/camera/color/image_raw /camera/aligned_depth_to_color/image_raw -O /home/d/1"/>                                                                                                            
</launch>

我把上面的文件命名为rosbag_record.launch，并且放在/home/d/catkin_ws/src/realsense-ros/realsense2_camera/launch/路径下
切换到安装realsense-ros的工作空间

source devel/setup.bash
roslaunch realsense2_camera rosbag_record.launch

录制结束按Ctrl+C暂停

4.提取图像以及对齐时间戳

新建文件夹room（此名称随意），在此文件夹下新建文件夹rgb和depth保存提取出来的深度图和彩色图，同时新建文件rgb.txt和depth.txt为对齐时间戳做准备
执行以下python脚本提取图像.py

注意：该脚本只能提取图像，并不能对齐像素（因为第二部分的第1步已经对齐过了）

# -*- 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 == "/camera/aligned_depth_to_color/image_raw":  #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 == "/camera/color/image_raw":   #rgb topic
            cv_image = bridge.imgmsg_to_cv2(msg,"bgr8")
            timestr = "%.6f" %  msg.header.stamp.to_sec()   #rgb time stamp
            image_name = timestr+ ".png"
            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: