intel RealSense D435i自制数据集跑SLAM

---

提示：该篇文章在Ubuntu22.04系统上进行

1.安装RealSense Viewer

1.下载realsense

git clone https://github.com/IntelRealSense/librealsense 

如果clone不下来，可以试试用手机流量。
如果流量也不行可以科学上网至今进入github网址下载下来，然后利用WinSCP或者X-Ftps等软件传输即可。

2.安装依赖项

cd librealsense
sudo apt-get install libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libusb-1.0-0-dev pkg-config
sudo apt-get install libglfw3-dev
sudo apt-get install libssl-dev

3.编译与安装

sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && udevadm trigger 
mkdir build
cd build

//从这里开始用手机流量，否则你会遇到网络问题无法继续下去！！！！
cmake ../ -DBUILD_EXAMPLES=true
make
sudo make install

4.测试

realsense-viewer 

2.录制数据集

使用 USB 线连接好深度相机和电脑，打开 Intel RealSense Viewer 。设置 Depth Stream 以及 Color Stream 的图像分辨率为 640 × 480 ，设置采集帧率为 30 fps 。点击左上角的 Record 按钮即可进行录制，开始录制后，点击左上角的 Stop 按钮即可结束录制并保存录制结果，如下图所示：
提示：开始录制时如果报错，在setting里修改一下保存位置即可！

结束录制后，在相应存储路径下即生成 .bag 文件，按需重命名该文件。至此，完成离线数据集的录制过程。

3.格式转换

此步骤的目的在于将录制好的数据集转换为 BundleFusion 所要求的离线输入格式，即 .sens 格式。BundleFusion 提供了将源格式封装成 .sens 格式的实现，因此只需将录制好的数据集存储为源格式即可。

3.1 解析.bag文件

.bag 是 ROS 常用的数据存储格式。我们需要对.bag 文件的解析，并从中提取出深度图和彩色图。首先，进入 bag 文件的存储路径并打开终端，通过 rosbag info ***.bag 查看待提取的深度图及彩色图所对应的 topic，如下图所示：

rosbag info myslam.bag 

提示：这里一定要用ROS1，ROS2不能识别.bag的文件，我之前安装ROS看别人说ROS1已经不再维护更新了，所以我装的ROS2，这可把我恶心坏了，搞了半天才知道ROS2不能识别.bag文件，浪费好多时间！

3.2 提取rgb和depth图片和信息

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

Python脚本(ass.py)如下：

如果你保存的路径出现了中文，要在第一行加入

coding: utf-8
否则会报错SyntaxError: Non-ASCII character ‘\xe8’ in file

#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/lvslam/myslam.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/lvslam/SLAM_Picture/'

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

file_handle1 = open('/home/lvslam/SLAM_Picture/rgb.txt', 'w')
file_handle2 = open('/home/lvslam/SLAM_Picture/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/lvslam/myslam.bag")#这是打开相机API录制的视频
save_path = '/home/lvslam/SLAM_Picture/'
file_handle1 = open('/home/lvslam/SLAM_Picture/rgb.txt', 'w')
file_handle2 = open('/home/lvslam/SLAM_Picture/depth.txt', 'w')

还有判断停止的条件

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

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

python ass.py
//由于我之前跑别的版本的SLAM电脑上装了几个不同版本的python，所以我要制定目录的python来跑！
/usr/bin/python3.8 ass.py

此时我们的rgb文件夹，depth文件夹，rgb.txt，depth.txt里面都写入了东西

3.3 对齐时间戳

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

associate.py：

import argparse
import sys
import os
import numpy


def read_file_list(filename, remove_bounds):
    """
    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")
    if remove_bounds:
        lines = lines[100:-100]
    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 = list(first_list.keys())
    second_keys = list(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)
    parser.add_argument('--remove_bounds', help='remove first and last 100 entries', action='store_true')
    args = parser.parse_args()

    first_list = read_file_list(args.first_file, args.remove_bounds)
    second_list = read_file_list(args.second_file, args.remove_bounds)

    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

至此，数据集制作完成。

4.用我们做的这个数据集跑跑SLAM算法

我还是以之前跑过的YOLO_ORB_SLAM3_with_pointcloud_map算法来做实验，这个算法的复现之前也写过博客。

运行成功！！
自制数据集完美运行！