intel RealSense D435自制数据集跑ORB-SLAM3

一.安装RealSense Viewer

1.下载realsense

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

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 ..
make
sudo make install

4.测试

realsense-viewer

二.录制数据集

打开 Intel RealSense Viewer 。设置 Depth Stream 以及 Color Stream 的图像分辨率为 640 × 480 ,设置采集帧率为 30 fps 。点击左上角的 Record 按钮即可进行录制,开始录制后,点击左上角的 Stop 按钮即可结束录制并保存录制结果,如下图所示:

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

三.用.bag文件提取rgb和depth图片和信息

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

tiqu.py内容如下

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/gzz/dataset/m/rgb/'  #rgb path
depth = '/home/gzz/dataset/m/depth/'   #depth path
bridge = CvBridge()

file_handle1 = open('/home/gzz/dataset/m/depth.txt', 'w')
file_handle2 = open('/home/gzz/dataset/m/rgb.txt', 'w')

with rosbag.Bag('/home/gzz/dataset/m.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+ ".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/gzz/dataset/m/rgb/'            #rgb path
depth = '/home/gzz/dataset/m/depth/'    #depth path

file_handle1 = open('/home/gzz/dataset/m/depth.txt', 'w')      #depth.txt文件位置
file_handle2 = open('/home/gzz/dataset/m/rgb.txt', 'w')          #rgb.txt文件位置

with rosbag.Bag('/home/gzz/dataset/m.bag', 'r') as bag:      #.bag文件的位置

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

python tiqu.py

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

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

四、参数配置文件TUM.yaml。

生成自己的参数配置文件TUM.yaml。复制TUM3.yaml,并修改参数

查看Intel RealSense D435相机的参数

rs-enumerate-devices -c

rs-enumerate-devices -c 
Device info: 
    Name                          : 	Intel RealSense D435
    Serial Number                 : 	138422072048
    Firmware Version              : 	05.12.01.00
    Recommended Firmware Version  : 	05.12.01.00
    Physical Port                 : 	/sys/devices/pci0000:00/0000:00:14.0/usb2/2-2/2-2:1.0/video4linux/video1
    Debug Op Code                 : 	15
    Advanced Mode                 : 	YES
    Product Id                    : 	0B07
    Camera Locked                 : 	YES
    Usb Type Descriptor           : 	3.2
    Product Line                  : 	D400
    Asic Serial Number            : 	130523024075
    Firmware Update Id            : 	130523024075

Stream Profiles supported by Stereo Module
 Supported modes:
    stream       resolution      fps       format   
    Infrared 1	  1280x800	@ 30Hz	   Y8
    Infrared 2	  1280x800	@ 30Hz	   Y8
    Infrared 1	  1280x800	@ 25Hz	   Y16
    Infrared 2	  1280x800	@ 25Hz	   Y16
    Infrared 1	  1280x800	@ 15Hz	   Y16
    Infrared 2	  1280x800	@ 15Hz	   Y16
    Infrared 2	  1280x800	@ 15Hz	   Y8
    Infrared 1	  1280x800	@ 15Hz	   Y8
    Infrared 1	  1280x720	@ 30Hz	   Y8
    Infrared 2	  1280x720	@ 30Hz	   Y8
    Infrared 2	  1280x720	@ 15Hz	   Y8
    Infrared 1	  1280x720	@ 15Hz	   Y8
    Infrared 1	  1280x720	@ 6Hz	   Y8
    Infrared 2	  1280x720	@ 6Hz	   Y8
    Infrared 2	  848x480	@ 90Hz	   Y8
    Infrared 1	  848x480	@ 90Hz	   Y8
    Infrared 1	  848x480	@ 60Hz	   Y8
    Infrared 2	  848x480	@ 60Hz	   Y8
    Infrared 1	  848x480	@ 30Hz	   Y8
    Infrared 2	  848x480	@ 30Hz	   Y8
    Infrared 2	  848x480	@ 15Hz	   Y8
    Infrared 1	  848x480	@ 15Hz	   Y8
    Infrared 1	  848x480	@ 6Hz	   Y8
    Infrared 2	  848x480	@ 6Hz	   Y8
    Infrared 2	  848x100	@ 100Hz	   Y8
    Infrared 1	  848x100	@ 100Hz	   Y8
    Infrared 1	  640x480	@ 90Hz	   Y8
    Infrared 2	  640x480	@ 90Hz	   Y8
    Infrared 1	  640x480	@ 60Hz	   Y8
    Infrared 2	  640x480	@ 60Hz	   Y8
    Infrared 1	  640x480	@ 30Hz	   Y8
    Infrared 2	  640x480	@ 30Hz	   Y8
    Infrared 1	  640x480	@ 15Hz	   Y8
    Infrared 2	  640x480	@ 15Hz	   Y8
    Infrared 2	  640x480	@ 6Hz	   Y8
    Infrared 1	  640x480	@ 6Hz	   Y8
    Infrared 1	  640x400	@ 25Hz	   Y16
    Infrared 2	  640x400	@ 25Hz	   Y16
    Infrared 1	  640x400	@ 15Hz	   Y16
    Infrared 2	  640x400	@ 15Hz	   Y16
    Infrared 2	  640x360	@ 90Hz	   Y8
    Infrared 1	  640x360	@ 90Hz	   Y8
    Infrared 1	  640x360	@ 60Hz	   Y8
    Infrared 2	  640x360	@ 60Hz	   Y8
    Infrared 1	  640x360	@ 30Hz	   Y8
    Infrared 2	  640x360	@ 30Hz	   Y8
    Infrared 1	  640x360	@ 15Hz	   Y8
    Infrared 2	  640x360	@ 15Hz	   Y8
    Infrared 2	  640x360	@ 6Hz	   Y8
    Infrared 1	  640x360	@ 6Hz	   Y8
    Infrared 1	  480x270	@ 90Hz	   Y8
    Infrared 2	  480x270	@ 90Hz	   Y8
    Infrared 1	  480x270	@ 60Hz	   Y8
    Infrared 2	  480x270	@ 60Hz	   Y8
    Infrared 1	  480x270	@ 30Hz	   Y8
    Infrared 2	  480x270	@ 30Hz	   Y8
    Infrared 2	  480x270	@ 15Hz	   Y8
    Infrared 1	  480x270	@ 15Hz	   Y8
    Infrared 1	  480x270	@ 6Hz	   Y8
    Infrared 2	  480x270	@ 6Hz	   Y8
    Infrared 2	  424x240	@ 90Hz	   Y8
    Infrared 1	  424x240	@ 90Hz	   Y8
    Infrared 2	  424x240	@ 60Hz	   Y8
    Infrared 1	  424x240	@ 60Hz	   Y8
    Infrared 2	  424x240	@ 30Hz	   Y8
    Infrared 1	  424x240	@ 30Hz	   Y8
    Infrared 2	  424x240	@ 15Hz	   Y8
    Infrared 1	  424x240	@ 15Hz	   Y8
    Infrared 1	  424x240	@ 6Hz	   Y8
    Infrared 2	  424x240	@ 6Hz	   Y8
    Infrared 1	  256x144	@ 90Hz	   Y8
    Infrared 2	  256x144	@ 90Hz	   Y8
    Depth	  1280x720	@ 30Hz	   Z16
    Depth	  1280x720	@ 15Hz	   Z16
    Depth	  1280x720	@ 6Hz	   Z16
    Depth	  848x480	@ 90Hz	   Z16
    Depth	  848x480	@ 60Hz	   Z16
    Depth	  848x480	@ 30Hz	   Z16
    Depth	  848x480	@ 15Hz	   Z16
    Depth	  848x480	@ 6Hz	   Z16
    Depth	  848x100	@ 100Hz	   Z16
    Depth	  640x480	@ 90Hz	   Z16
    Depth	  640x480	@ 60Hz	   Z16
    Depth	  640x480	@ 30Hz	   Z16
    Depth	  640x480	@ 15Hz	   Z16
    Depth	  640x480	@ 6Hz	   Z16
    Depth	  640x360	@ 90Hz	   Z16
    Depth	  640x360	@ 60Hz	   Z16
    Depth	  640x360	@ 30Hz	   Z16
    Depth	  640x360	@ 15Hz	   Z16
    Depth	  640x360	@ 6Hz	   Z16
    Depth	  480x270	@ 90Hz	   Z16
    Depth	  480x270	@ 60Hz	   Z16
    Depth	  480x270	@ 30Hz	   Z16
    Depth	  480x270	@ 15Hz	   Z16
    Depth	  480x270	@ 6Hz	   Z16
    Depth	  424x240	@ 90Hz	   Z16
    Depth	  424x240	@ 60Hz	   Z16
    Depth	  424x240	@ 30Hz	   Z16
    Depth	  424x240	@ 15Hz	   Z16
    Depth	  424x240	@ 6Hz	   Z16
    Depth	  256x144	@ 90Hz	   Z16

Stream Profiles supported by RGB Camera
 Supported modes:
    stream       resolution      fps       format   
    Color	  1920x1080	@ 30Hz	   RGB8
    Color	  1920x1080	@ 30Hz	   Y16
    Color	  1920x1080	@ 30Hz	   BGRA8
    Color	  1920x1080	@ 30Hz	   RGBA8
    Color	  1920x1080	@ 30Hz	   BGR8
    Color	  1920x1080	@ 30Hz	   YUYV
    Color	  1920x1080	@ 15Hz	   RGB8
    Color	  1920x1080	@ 15Hz	   Y16
    Color	  1920x1080	@ 15Hz	   BGRA8
    Color	  1920x1080	@ 15Hz	   RGBA8
    Color	  1920x1080	@ 15Hz	   BGR8
    Color	  1920x1080	@ 15Hz	   YUYV
    Color	  1920x1080	@ 6Hz	   RGB8
    Color	  1920x1080	@ 6Hz	   Y16
    Color	  1920x1080	@ 6Hz	   BGRA8
    Color	  1920x1080	@ 6Hz	   RGBA8
    Color	  1920x1080	@ 6Hz	   BGR8
    Color	  1920x1080	@ 6Hz	   YUYV
    Color	  1280x720	@ 30Hz	   RGB8
    Color	  1280x720	@ 30Hz	   Y16
    Color	  1280x720	@ 30Hz	   BGRA8
    Color	  1280x720	@ 30Hz	   RGBA8
    Color	  1280x720	@ 30Hz	   BGR8
    Color	  1280x720	@ 30Hz	   YUYV
    Color	  1280x720	@ 15Hz	   RGB8
    Color	  1280x720	@ 15Hz	   Y16
    Color	  1280x720	@ 15Hz	   BGRA8
    Color	  1280x720	@ 15Hz	   RGBA8
    Color	  1280x720	@ 15Hz	   BGR8
    Color	  1280x720	@ 15Hz	   YUYV
    Color	  1280x720	@ 6Hz	   RGB8
    Color	  1280x720	@ 6Hz	   Y16
    Color	  1280x720	@ 6Hz	   BGRA8
    Color	  1280x720	@ 6Hz	   RGBA8
    Color	  1280x720	@ 6Hz	   BGR8
    Color	  1280x720	@ 6Hz	   YUYV
    Color	  960x540	@ 60Hz	   RGB8
    Color	  960x540	@ 60Hz	   Y16
    Color	  960x540	@ 60Hz	   BGRA8
    Color	  960x540	@ 60Hz	   RGBA8
    Color	  960x540	@ 60Hz	   BGR8
    Color	  960x540	@ 60Hz	   YUYV
    Color	  960x540	@ 30Hz	   RGB8
    Color	  960x540	@ 30Hz	   Y16
    Color	  960x540	@ 30Hz	   BGRA8
    Color	  960x540	@ 30Hz	   RGBA8
    Color	  960x540	@ 30Hz	   BGR8
    Color	  960x540	@ 30Hz	   YUYV
    Color	  960x540	@ 15Hz	   RGB8
    Color	  960x540	@ 15Hz	   Y16
    Color	  960x540	@ 15Hz	   BGRA8
    Color	  960x540	@ 15Hz	   RGBA8
    Color	  960x540	@ 15Hz	   BGR8
    Color	  960x540	@ 15Hz	   YUYV
    Color	  960x540	@ 6Hz	   RGB8
    Color	  960x540	@ 6Hz	   Y16
    Color	  960x540	@ 6Hz	   BGRA8
    Color	  960x540	@ 6Hz	   RGBA8
    Color	  960x540	@ 6Hz	   BGR8
    Color	  960x540	@ 6Hz	   YUYV
    Color	  848x480	@ 60Hz	   RGB8
    Color	  848x480	@ 60Hz	   Y16
    Color	  848x480	@ 60Hz	   BGRA8
    Color	  848x480	@ 60Hz	   RGBA8
    Color	  848x480	@ 60Hz	   BGR8
    Color	  848x480	@ 60Hz	   YUYV
    Color	  848x480	@ 30Hz	   RGB8
    Color	  848x480	@ 30Hz	   Y16
    Color	  848x480	@ 30Hz	   BGRA8
    Color	  848x480	@ 30Hz	   RGBA8
    Color	  848x480	@ 30Hz	   BGR8
    Color	  848x480	@ 30Hz	   YUYV
    Color	  848x480	@ 15Hz	   RGB8
    Color	  848x480	@ 15Hz	   Y16
    Color	  848x480	@ 15Hz	   BGRA8
    Color	  848x480	@ 15Hz	   RGBA8
    Color	  848x480	@ 15Hz	   BGR8
    Color	  848x480	@ 15Hz	   YUYV
    Color	  848x480	@ 6Hz	   RGB8
    Color	  848x480	@ 6Hz	   Y16
    Color	  848x480	@ 6Hz	   BGRA8
    Color	  848x480	@ 6Hz	   RGBA8
    Color	  848x480	@ 6Hz	   BGR8
    Color	  848x480	@ 6Hz	   YUYV
    Color	  640x480	@ 60Hz	   RGB8
    Color	  640x480	@ 60Hz	   Y16
    Color	  640x480	@ 60Hz	   BGRA8
    Color	  640x480	@ 60Hz	   RGBA8
    Color	  640x480	@ 60Hz	   BGR8
    Color	  640x480	@ 60Hz	   YUYV
    Color	  640x480	@ 30Hz	   RGB8
    Color	  640x480	@ 30Hz	   Y16
    Color	  640x480	@ 30Hz	   BGRA8
    Color	  640x480	@ 30Hz	   RGBA8
    Color	  640x480	@ 30Hz	   BGR8
    Color	  640x480	@ 30Hz	   YUYV
    Color	  640x480	@ 15Hz	   RGB8
    Color	  640x480	@ 15Hz	   Y16
    Color	  640x480	@ 15Hz	   BGRA8
    Color	  640x480	@ 15Hz	   RGBA8
    Color	  640x480	@ 15Hz	   BGR8
    Color	  640x480	@ 15Hz	   YUYV
    Color	  640x480	@ 6Hz	   RGB8
    Color	  640x480	@ 6Hz	   Y16
    Color	  640x480	@ 6Hz	   BGRA8
    Color	  640x480	@ 6Hz	   RGBA8
    Color	  640x480	@ 6Hz	   BGR8
    Color	  640x480	@ 6Hz	   YUYV
    Color	  640x360	@ 60Hz	   RGB8
    Color	  640x360	@ 60Hz	   Y16
    Color	  640x360	@ 60Hz	   BGRA8
    Color	  640x360	@ 60Hz	   RGBA8
    Color	  640x360	@ 60Hz	   BGR8
    Color	  640x360	@ 60Hz	   YUYV
    Color	  640x360	@ 30Hz	   RGB8
    Color	  640x360	@ 30Hz	   Y16
    Color	  640x360	@ 30Hz	   BGRA8
    Color	  640x360	@ 30Hz	   RGBA8
    Color	  640x360	@ 30Hz	   BGR8
    Color	  640x360	@ 30Hz	   YUYV
    Color	  640x360	@ 15Hz	   RGB8
    Color	  640x360	@ 15Hz	   Y16
    Color	  640x360	@ 15Hz	   BGRA8
    Color	  640x360	@ 15Hz	   RGBA8
    Color	  640x360	@ 15Hz	   BGR8
    Color	  640x360	@ 15Hz	   YUYV
    Color	  640x360	@ 6Hz	   RGB8
    Color	  640x360	@ 6Hz	   Y16
    Color	  640x360	@ 6Hz	   BGRA8
    Color	  640x360	@ 6Hz	   RGBA8
    Color	  640x360	@ 6Hz	   BGR8
    Color	  640x360	@ 6Hz	   YUYV
    Color	  424x240	@ 60Hz	   RGB8
    Color	  424x240	@ 60Hz	   Y16
    Color	  424x240	@ 60Hz	   BGRA8
    Color	  424x240	@ 60Hz	   RGBA8
    Color	  424x240	@ 60Hz	   BGR8
    Color	  424x240	@ 60Hz	   YUYV
    Color	  424x240	@ 30Hz	   RGB8
    Color	  424x240	@ 30Hz	   Y16
    Color	  424x240	@ 30Hz	   BGRA8
    Color	  424x240	@ 30Hz	   RGBA8
    Color	  424x240	@ 30Hz	   BGR8
    Color	  424x240	@ 30Hz	   YUYV
    Color	  424x240	@ 15Hz	   RGB8
    Color	  424x240	@ 15Hz	   Y16
    Color	  424x240	@ 15Hz	   BGRA8
    Color	  424x240	@ 15Hz	   RGBA8
    Color	  424x240	@ 15Hz	   BGR8
    Color	  424x240	@ 15Hz	   YUYV
    Color	  424x240	@ 6Hz	   RGB8
    Color	  424x240	@ 6Hz	   Y16
    Color	  424x240	@ 6Hz	   BGRA8
    Color	  424x240	@ 6Hz	   RGBA8
    Color	  424x240	@ 6Hz	   BGR8
    Color	  424x240	@ 6Hz	   YUYV
    Color	  320x240	@ 60Hz	   RGB8
    Color	  320x240	@ 60Hz	   Y16
    Color	  320x240	@ 60Hz	   BGRA8
    Color	  320x240	@ 60Hz	   RGBA8
    Color	  320x240	@ 60Hz	   BGR8
    Color	  320x240	@ 60Hz	   YUYV
    Color	  320x240	@ 30Hz	   RGB8
    Color	  320x240	@ 30Hz	   Y16
    Color	  320x240	@ 30Hz	   BGRA8
    Color	  320x240	@ 30Hz	   RGBA8
    Color	  320x240	@ 30Hz	   BGR8
    Color	  320x240	@ 30Hz	   YUYV
    Color	  320x240	@ 6Hz	   RGB8
    Color	  320x240	@ 6Hz	   Y16
    Color	  320x240	@ 6Hz	   BGRA8
    Color	  320x240	@ 6Hz	   RGBA8
    Color	  320x240	@ 6Hz	   BGR8
    Color	  320x240	@ 6Hz	   YUYV
    Color	  320x180	@ 60Hz	   RGB8
    Color	  320x180	@ 60Hz	   Y16
    Color	  320x180	@ 60Hz	   BGRA8
    Color	  320x180	@ 60Hz	   RGBA8
    Color	  320x180	@ 60Hz	   BGR8
    Color	  320x180	@ 60Hz	   YUYV
    Color	  320x180	@ 30Hz	   RGB8
    Color	  320x180	@ 30Hz	   Y16
    Color	  320x180	@ 30Hz	   BGRA8
    Color	  320x180	@ 30Hz	   RGBA8
    Color	  320x180	@ 30Hz	   BGR8
    Color	  320x180	@ 30Hz	   YUYV
    Color	  320x180	@ 6Hz	   RGB8
    Color	  320x180	@ 6Hz	   Y16
    Color	  320x180	@ 6Hz	   BGRA8
    Color	  320x180	@ 6Hz	   RGBA8
    Color	  320x180	@ 6Hz	   BGR8
    Color	  320x180	@ 6Hz	   YUYV

Intrinsic Parameters:

 Intrinsic of "Depth" / 256x144 / {Z16}
  Width:      	256
  Height:     	144
  PPX:        	128.925750732422
  PPY:        	71.9685974121094
  Fx:         	126.430702209473
  Fy:         	126.430702209473
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 424x240 / {Z16}
  Width:      	424
  Height:     	240
  PPX:        	213.533279418945
  PPY:        	119.94799041748
  Fx:         	209.400848388672
  Fy:         	209.400848388672
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 480x270 / {Z16}
  Width:      	480
  Height:     	270
  PPX:        	241.735794067383
  PPY:        	134.941116333008
  Fx:         	237.057556152344
  Fy:         	237.057556152344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 640x360 / {Z16}
  Width:      	640
  Height:     	360
  PPX:        	322.314392089844
  PPY:        	179.921493530273
  Fx:         	316.076751708984
  Fy:         	316.076751708984
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 640x480 / {Z16}
  Width:      	640
  Height:     	480
  PPX:        	322.777282714844
  PPY:        	239.905792236328
  Fx:         	379.292114257812
  Fy:         	379.292114257812
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 848x100 / {Z16}
  Width:      	848
  Height:     	100
  PPX:        	427.066558837891
  PPY:        	49.8959808349609
  Fx:         	418.801696777344
  Fy:         	418.801696777344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 848x480 / {Z16}
  Width:      	848
  Height:     	480
  PPX:        	427.066558837891
  PPY:        	239.895980834961
  Fx:         	418.801696777344
  Fy:         	418.801696777344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Depth" / 1280x720 / {Z16}
  Width:      	1280
  Height:     	720
  PPX:        	644.628784179688
  PPY:        	359.842987060547
  Fx:         	632.153503417969
  Fy:         	632.153503417969
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 320x180 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	320
  Height:     	180
  PPX:        	159.733459472656
  PPY:        	89.1353607177734
  Fx:         	226.920501708984
  Fy:         	226.946411132812
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 320x240 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	320
  Height:     	240
  PPX:        	159.644622802734
  PPY:        	118.847145080566
  Fx:         	302.560699462891
  Fy:         	302.59521484375
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 424x240 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	424
  Height:     	240
  PPX:        	211.644622802734
  PPY:        	118.847145080566
  Fx:         	302.560668945312
  Fy:         	302.59521484375
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 640x360 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	640
  Height:     	360
  PPX:        	319.466918945312
  PPY:        	178.270721435547
  Fx:         	453.841003417969
  Fy:         	453.892822265625
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 640x480 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	640
  Height:     	480
  PPX:        	319.289245605469
  PPY:        	237.694290161133
  Fx:         	605.121398925781
  Fy:         	605.1904296875
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 848x480 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	848
  Height:     	480
  PPX:        	423.289245605469
  PPY:        	237.694290161133
  Fx:         	605.121337890625
  Fy:         	605.1904296875
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 960x540 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	960
  Height:     	540
  PPX:        	479.200408935547
  PPY:        	267.406066894531
  Fx:         	680.761535644531
  Fy:         	680.839233398438
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 1280x720 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	1280
  Height:     	720
  PPX:        	638.933837890625
  PPY:        	356.541442871094
  Fx:         	907.682006835938
  Fy:         	907.78564453125
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Color" / 1920x1080 / {YUYV/RGB8/BGR8/RGBA8/BGRA8/Y16}
  Width:      	1920
  Height:     	1080
  PPX:        	958.400817871094
  PPY:        	534.812133789062
  Fx:         	1361.52307128906
  Fy:         	1361.67846679688
  Distortion: 	Inverse Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 256x144 / {Y8}
  Width:      	256
  Height:     	144
  PPX:        	128.925750732422
  PPY:        	71.9685974121094
  Fx:         	126.430702209473
  Fy:         	126.430702209473
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 424x240 / {Y8}
  Width:      	424
  Height:     	240
  PPX:        	213.533279418945
  PPY:        	119.94799041748
  Fx:         	209.400848388672
  Fy:         	209.400848388672
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 480x270 / {Y8}
  Width:      	480
  Height:     	270
  PPX:        	241.735794067383
  PPY:        	134.941116333008
  Fx:         	237.057556152344
  Fy:         	237.057556152344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 640x360 / {Y8}
  Width:      	640
  Height:     	360
  PPX:        	322.314392089844
  PPY:        	179.921493530273
  Fx:         	316.076751708984
  Fy:         	316.076751708984
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 640x480 / {Y8}
  Width:      	640
  Height:     	480
  PPX:        	322.777282714844
  PPY:        	239.905792236328
  Fx:         	379.292114257812
  Fy:         	379.292114257812
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 848x100 / {Y8}
  Width:      	848
  Height:     	100
  PPX:        	427.066558837891
  PPY:        	49.8959808349609
  Fx:         	418.801696777344
  Fy:         	418.801696777344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 848x480 / {Y8}
  Width:      	848
  Height:     	480
  PPX:        	427.066558837891
  PPY:        	239.895980834961
  Fx:         	418.801696777344
  Fy:         	418.801696777344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 1280x720 / {Y8}
  Width:      	1280
  Height:     	720
  PPX:        	644.628784179688
  PPY:        	359.842987060547
  Fx:         	632.153503417969
  Fy:         	632.153503417969
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 1" / 1280x800 / {Y8}
  Width:      	1280
  Height:     	800
  PPX:        	644.628784179688
  PPY:        	399.842987060547
  Fx:         	632.153503417969
  Fy:         	632.153503417969
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 256x144 / {Y8}
  Width:      	256
  Height:     	144
  PPX:        	128.925750732422
  PPY:        	71.9685974121094
  Fx:         	126.430702209473
  Fy:         	126.430702209473
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 424x240 / {Y8}
  Width:      	424
  Height:     	240
  PPX:        	213.533279418945
  PPY:        	119.94799041748
  Fx:         	209.400848388672
  Fy:         	209.400848388672
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 480x270 / {Y8}
  Width:      	480
  Height:     	270
  PPX:        	241.735794067383
  PPY:        	134.941116333008
  Fx:         	237.057556152344
  Fy:         	237.057556152344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 640x360 / {Y8}
  Width:      	640
  Height:     	360
  PPX:        	322.314392089844
  PPY:        	179.921493530273
  Fx:         	316.076751708984
  Fy:         	316.076751708984
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 640x480 / {Y8}
  Width:      	640
  Height:     	480
  PPX:        	322.777282714844
  PPY:        	239.905792236328
  Fx:         	379.292114257812
  Fy:         	379.292114257812
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 848x100 / {Y8}
  Width:      	848
  Height:     	100
  PPX:        	427.066558837891
  PPY:        	49.8959808349609
  Fx:         	418.801696777344
  Fy:         	418.801696777344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 848x480 / {Y8}
  Width:      	848
  Height:     	480
  PPX:        	427.066558837891
  PPY:        	239.895980834961
  Fx:         	418.801696777344
  Fy:         	418.801696777344
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 1280x720 / {Y8}
  Width:      	1280
  Height:     	720
  PPX:        	644.628784179688
  PPY:        	359.842987060547
  Fx:         	632.153503417969
  Fy:         	632.153503417969
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  

 Intrinsic of "Infrared 2" / 1280x800 / {Y8}
  Width:      	1280
  Height:     	800
  PPX:        	644.628784179688
  PPY:        	399.842987060547
  Fx:         	632.153503417969
  Fy:         	632.153503417969
  Distortion: 	Brown Conrady
  Coeffs:     	0  	0  	0  	0  	0  


Extrinsic Parameters:
Extrinsic from "Depth"	  To	  "Depth" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

Extrinsic from "Depth"	  To	  "Color" :
 Rotation Matrix:
   0.999883         0.0145104        0.00479119    
  -0.0145062        0.999894        -0.000911268   
  -0.00480391       0.00084166       0.999988      

 Translation Vector: 0.0148128503933549  -0.000189567057532258  0.000381971505703405  

Extrinsic from "Depth"	  To	  "Infrared 1" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

Extrinsic from "Depth"	  To	  "Infrared 2" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

Extrinsic from "Color"	  To	  "Depth" :
 Rotation Matrix:
   0.999883        -0.0145062       -0.00480391    
   0.0145104        0.999894         0.00084166    
   0.00479119      -0.000911268      0.999988      

 Translation Vector: -0.0148120354861021  -2.57152587437304e-05  -0.000453110988019034  

Extrinsic from "Color"	  To	  "Color" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

Extrinsic from "Color"	  To	  "Infrared 1" :
 Rotation Matrix:
   0.999883        -0.0145062       -0.00480391    
   0.0145104        0.999894         0.00084166    
   0.00479119      -0.000911268      0.999988      

 Translation Vector: -0.0148120354861021  -2.57152587437304e-05  -0.000453110988019034  

Extrinsic from "Color"	  To	  "Infrared 2" :
 Rotation Matrix:
   0.999883        -0.0145062       -0.00480391    
   0.0145104        0.999894         0.00084166    
   0.00479119      -0.000911268      0.999988      

 Translation Vector: -0.0148120354861021  -2.57152587437304e-05  -0.000453110988019034  

Extrinsic from "Infrared 1"	  To	  "Depth" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

Extrinsic from "Infrared 1"	  To	  "Color" :
 Rotation Matrix:
   0.999883         0.0145104        0.00479119    
  -0.0145062        0.999894        -0.000911268   
  -0.00480391       0.00084166       0.999988      

 Translation Vector: 0.0148128503933549  -0.000189567057532258  0.000381971505703405  

Extrinsic from "Infrared 1"	  To	  "Infrared 1" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

Extrinsic from "Infrared 1"	  To	  "Infrared 2" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: -0.0499339736998081  0  0  

Extrinsic from "Infrared 2"	  To	  "Depth" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0.0499339736998081  0  0  

Extrinsic from "Infrared 2"	  To	  "Color" :
 Rotation Matrix:
   0.999883         0.0145104        0.00479119    
  -0.0145062        0.999894        -0.000911268   
  -0.00480391       0.00084166       0.999988      

 Translation Vector: 0.0647409930825233  -0.000913920404855162  0.000142093122121878  

Extrinsic from "Infrared 2"	  To	  "Infrared 1" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0.0499339736998081  0  0  

Extrinsic from "Infrared 2"	  To	  "Infrared 2" :
 Rotation Matrix:
   1   0   0
   0   1   0
   0   0   1

 Translation Vector: 0  0  0  

D435.yaml

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"

Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera1.fx: 616.9
Camera1.fy: 616.9
Camera1.cx: 320.0
Camera1.cy: 240.0

Camera1.k1: -0.066
Camera1.k2: 0.027
Camera1.p1: 0.000
Camera1.p2: 0.000

Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
Stereo.ThDepth: 40.0
Stereo.b: 0.075

# Depth map values factor
RGBD.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.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500.0

五、自制数据集跑SLAM算法

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值