使用Intel RealSense D435i自制离线数据集跑通BundleFusion

摘要

本文主要讲述如何使用 Intel RealSense D435i 深度相机录制 Rosbag 离线数据集，并基于此数据集跑通 BundleFusion。关于如何基于 BundleFusion 使用 Intel RealSense D435i 深度相机实现实时三维建图可参照 《Intel RealSense D435i + BundleFusion 实现实时三维建图》。

操作环境

环境项	版本/型号
操作系统	Win10_x64、Ubuntu 18.04
开发工具集	Visual Studio 2013、Pycharm Professional 2019.1.3
计算架构	CUDA 8.0
深度相机	Intel RealSense D435i
显卡	NVIDIA GeForce 940MX

Ubuntu 18.04 系统中需完成 ROS 配置，具体配置过程可参考 《ROS开发之环境搭建:Ubuntu 18.04安装ROS Melodic》。ROS 配置完成后，需为 Pycharm 设定环境变量，从而实现在 Pycharm 中编写 ROS 代码。通过 sudo gedit /usr/share/applications/pycharm.desktop 命令编辑 “ pycharm.desktop ” 文件，路径取决于安装 Pycharm 时的选项，选择为所有用户安装（/usr/share/applications），选择为当前用户安装（~/.local/share/applications）。将该文件中的 Exec = /... 修改为 Exec = bash -i -c /... ，从而实现在通过快捷方式启动 Pycharm 的同时加载 “ ~/.bashrc ” 中的 ROS环境变量，修改后如下图所示：

大致流程

1. 录制数据集
2. 解析 bag 文件，提取深度图和彩色图并制作成源格式
3. 将源格式封装为 sens 格式
4. 基于 BundleFusion 进行离线三维重建

录制数据集

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

若点击 Record 按钮后出现以下报错，说明默认存储路径为 “ C:\Users\ ”，软件无权在该路径下创建文件。通过点击右上角的齿轮图标，选择 Settings 并改变存储路径即可解决此问题，具体如下图所示：

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

格式转换

此步骤的目的在于将录制好的数据集转换为 BundleFusion 所要求的离线输入格式，即 .sens 格式。BundleFusion 提供了将源格式封装成 .sens 格式的实现，因此只需将录制好的数据集存储为源格式即可。从 BundleFusion 官网下载源格式数据集，查看其存储格式如下图所示：

BundleFusion 官网对于数据存储格式也有相应说明：

解析.bag文件

.bag 是 ROS 常用的数据存储格式，由于 ROS 的配置在 Ubuntu 18.04 系统中，因此切换至 Ubuntu 系统完成 .bag 文件的解析并从中提取出深度图和彩色图。首先，进入 bag 文件的存储路径并打开终端，通过 rosbag info ***.bag 查看待提取的深度图及彩色图所对应的 topic，如下图所示：

Intel RealSense D435i 深度相机所录制的数据包括 Depth、RGB 以及 IMU，使用 BundleFusion 进行三维重建只需使用 Depth 及 RGB，因此新建两个文件夹分别用于存放此两种类型的数据。

执行以下 Python 脚本，提取深度图及彩色图。其中，深度图的命名格式为 时间戳.png，彩色图的命名格式为 时间戳.jpg。同时将深度图与彩色图的时间戳及相对路径按照 （时间戳 相对路径） 的形式分别存储至两个文本文件中。

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/magus/jizongxing-workspace/slam/rosImage/rgb/'  #rgb path
depth = '/home/magus/jizongxing-workspace/slam/rosImage/depth/'   #depth path
bridge = CvBridge()

file_handle1 = open('/home/magus/jizongxing-workspace/slam/rosImage/depth-stamp.txt', 'w')
file_handle2 = open('/home/magus/jizongxing-workspace/slam/rosImage/rgb-stamp.txt', 'w')

with rosbag.Bag('/home/magus/jizongxing-workspace/bedroom.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()

提取结果如下：

时间戳对齐

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

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 depth-stamp.txt rgb-stamp.txt > associate.txt

制作源格式

根据配对结果，将深度图及彩色图按对重命名。相机位姿是在重建过程中估计所得，作为输入无需精确的位姿数据，因此将源格式中的相机位姿统一预设为：

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

将相机位姿文件按格式命名并与深度图、彩色图组成序列，执行如下 Python 脚本文件实现上述需求。

import shutil
import os

bp='/home/magus/jizongxing-workspace/slam/rosImage/'  # base path
rp='/home/magus/jizongxing-workspace/slam/rosImage/result/'  # result path
file_handle1 = open('/home/magus/jizongxing-workspace/slam/rosImage/associate.txt', 'r')

count = 0
for line in file_handle1.readlines():
    line = line.strip()
    # relative path of depth and color
    path_d = line.split()[1]   # relative path of depth
    path_c = line.split()[3]   # relative path of rgb

    if (os.path.exists(bp+path_d)==True) and (os.path.exists(bp+path_c)==True):
        rName = "frame-" + str(count).zfill(6) + ".color.jpg"   # rename rgb
        dName = "frame-" + str(count).zfill(6) + ".depth.png"   # rename depth
        pName = "frame-" + str(count).zfill(6) + ".pose.txt"   # name pose
        # create pose file
        file_handle2 = open(rp+pName, 'w')
        file_handle2.write("1 0 0 0\n0 1 0 0\n0 0 1 0\n0 0 0 1")   # initialize pose.txt
        file_handle2.close()
        shutil.copy(bp+path_d,rp+dName)
        shutil.copy(bp+path_c,rp+rName)
        count=count+1
file_handle1.close()

执行结果如下：

最终，将 info.txt 文件拷贝至 result 文件夹中，将其中的 m_frames.size 按照实际大小进行设置。

m_versionNumber = 4
m_sensorName = StructureSensor
m_colorWidth = 640
m_colorHeight = 480
m_depthWidth = 640
m_depthHeight = 480
m_depthShift = 1000
m_calibrationColorIntrinsic = 582.871 0 320 0 0 582.871 240 0 0 0 1 0 0 0 0 1 
m_calibrationColorExtrinsic = 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 
m_calibrationDepthIntrinsic = 583 0 320 0 0 583 240 0 0 0 1 0 0 0 0 1 
m_calibrationDepthExtrinsic = 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 
m_frames.size = 2016

至此，成功生成与源格式相同的数据格式。

封装为.sens格式

在 BundleFusion 工程的 sensorData.h 文件中提供了将源格式封装为 .sens 格式的相关实现 loadFromImages 。先拷贝两份 BundleFusion 工程代码，一份用于将源格式封装为 .sens 格式，另一份用于运行 sens 生成 ply 模型。

因此，将其中一份 BundleFusion 工程中原来的主函数全部注释并替换为：

int main()
{
	ml::SensorData sd;
	sd.loadFromImages("H:/ROSImage/result", "frame-", "jpg");
	sd.saveToFile("H:/ROSImage/test.sens");
}

同时在 sensorData.h 文件的第812与813行对深度图及彩色图的压缩类型进行初始化，从而使代码能够正常运行，生成可用的sens文件。

生成解决方案后，点击 FriedLiver.exe 即可对源格式进行封装，在相应路径下生成 sens 文件。

离线三维重建

将生成的 sens 文件重命名后拷贝至另一份 BundleFusion 工程的 “ BundleFusion-master\FriedLiver\x64\data ” 目录下。

同时将 GlobalAppState.h 文件中的深度相机全部注释掉。

生成解决方案后，将 “ BundleFusion-master\FriedLiver ” 下的 zParametersBundlingDefault.txt 以及 zParametersDefault.txt 文件拷贝至 “ BundleFusion-master\FriedLiver\x64\Release ” 路径下，并作如下修改：

修改 zParametersDefault.txt

修改 zParametersBundlingDefault.txt

重建效果

点击 FriedLiver.exe 即可进行离线三维重建，重建过程如下所示：

运行结束后，“ BundleFusion-master\FriedLiver\x64\data ” 路径下将生成 ply 模型，如下图所示：

使用 Meshlab 查看该 ply 文件，重建效果如下所示：

结语

重建结果来看，整体具备辨识度，但细节处理欠缺，这是由于在录制数据集时，仅简单环绕一周，未对细节部分进行局部录制。在之后的工作中，我们将使用小车搭载 Intel RealSense D435i 深度相机，进行平稳匀速拍摄，并对细节部分的深度以及色彩信息进行采集。以上就是使用 Intel RealSense D435i 自制离线数据集跑通 BundleFusion 的全过程，相互学习，共克时艰，哈哈哈。