open3d学习笔记五【RGBD融合】

1.RGB图像+深度图像融合

先建立RGBD图像。

depth = o3d.io.read_image("path/to/depth.jpg")
color = o3d.io.read_image("path/to/color.jpg")
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
            color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)

根据RGBD图像以及相机内参生成点云数据

pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image,
                # 输入open3d能识别的相机内参，如果用自己的相机，则需要先做内参的转换
                o3d.camera.PinholeCameraIntrinsic(
                    o3d.camera.PinholeCameraIntrinsicParameters.
                    PrimeSenseDefault))
# 把生成的点云显示出来
o3d.visualization.draw_geometries([pcd])

2.RGBD融合

首先需要准备相机轨迹，文件名后缀.log

0 0 1
1.00000000 -0.00000000 -0.00000000 -0.00000000
-0.00000000 1.00000000 0.00000000 -0.00000000
0.00000000 -0.00000000 1.00000000 0.00000000
0.00000000 0.00000000 0.00000000 1.00000000
1 1 2
0.99999139 -0.00001393 0.00415030 0.00118646
-0.00003622 0.99992698 0.01208406 -0.02351636
-0.00415016 -0.01208411 0.99991837 -0.00144057
0.00000000 0.00000000 0.00000000 1.00000000

使用以下函数读取

class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)

def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj

读取图片文件函数

def sorted_alphanum(file_list_ordered):
    convert = lambda text: int(text) if text.isdigit() else text
    alphanum_key = lambda key: [convert(c) for c in re.split('([0-9]+)', key)]
    return sorted(file_list_ordered, key=alphanum_key)

def get_file_list(path, extension=None):
    if extension is None:
        file_list = [path + f for f in os.listdir(path) if os.path.isfile(join(path, f))]
    else:
        file_list = [
            path + f
            for f in os.listdir(path)
            if os.path.isfile(os.path.join(path, f)) and os.path.splitext(f)[1] == extension
        ]
    file_list = sorted_alphanum(file_list)
    return file_list

RGBD重建

数据集可以从github中获取，open3d_downloads。

def load_point_clouds(volume, voxel_size=0.0,debug_mode = True):
    path = "mode/livingroom1_clean_micro/"

    rgbd_images = []
    pcds = []
    depth_image_path = get_file_list(os.path.join(path, "depth/"),
                                    extension=".png")
    color_image_path = get_file_list(os.path.join(path, "image/"),
                                    extension=".jpg")
    assert (len(depth_image_path) == len(color_image_path))

    for i in range(len(depth_image_path)):
        depth = o3d.io.read_image(os.path.join(depth_image_path[i]))
        color = o3d.io.read_image(os.path.join(color_image_path[i]))
        rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
            color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
        if debug_mode:
            pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
                rgbd_image,
                o3d.camera.PinholeCameraIntrinsic(
                    o3d.camera.PinholeCameraIntrinsicParameters.
                    PrimeSenseDefault))
            pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
            pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
            o3d.visualization.draw_geometries([pcd_down])
            pcds.append(pcd_down)
        else:
            camera_poses = read_trajectory("mode\\livingroom1_clean_micro\\test_scene\\trajectory.log")
            volume.integrate(rgbd_image,
            o3d.camera.PinholeCameraIntrinsic(
                o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault),
            np.linalg.inv(camera_poses[i].pose))
        rgbd_images.append(rgbd_image)
    if debug_mode:
        return pcds
    else:
        return volume

3.整体代码

数据集一共210张，全部读取。
下面开始调用以上函数，生成融合后的点云图。注意图片有点多，所需时间较久。

from ntpath import join
import open3d as o3d
import numpy as np
import os
import re

# 相机轨迹
class CameraPose:
    def __init__(self, meta, mat):
        self.metadata = meta
        self.pose = mat

    def __str__(self):
        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
            "Pose : " + "\n" + np.array_str(self.pose)

def read_trajectory(filename):
    traj = []
    with open(filename, 'r') as f:
        metastr = f.readline()
        while metastr:
            metadata = list(map(int, metastr.split()))
            mat = np.zeros(shape=(4, 4))
            for i in range(4):
                matstr = f.readline()
                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
            traj.append(CameraPose(metadata, mat))
            metastr = f.readline()
    return traj

# 读取图片文件
def sorted_alphanum(file_list_ordered):
    convert = lambda text: int(text) if text.isdigit() else text
    alphanum_key = lambda key: [convert(c) for c in re.split('([0-9]+)', key)]
    return sorted(file_list_ordered, key=alphanum_key)

def get_file_list(path, extension=None):
    if extension is None:
        file_list = [path + f for f in os.listdir(path) if os.path.isfile(join(path, f))]
    else:
        file_list = [
            path + f
            for f in os.listdir(path)
            if os.path.isfile(os.path.join(path, f)) and os.path.splitext(f)[1] == extension
        ]
    file_list = sorted_alphanum(file_list)
    return file_list
# RGBD重建
def load_point_clouds(volume, voxel_size=0.0,debug_mode = True):
    path = "mode/livingroom1_clean_micro/"

    rgbd_images = []
    pcds = []
    depth_image_path = get_file_list(os.path.join(path, "depth/"),
                                    extension=".png")
    color_image_path = get_file_list(os.path.join(path, "image/"),
                                    extension=".jpg")
    assert (len(depth_image_path) == len(color_image_path))

    for i in range(len(depth_image_path)):
    # for i in range(4):
        depth = o3d.io.read_image(os.path.join(depth_image_path[i]))
        color = o3d.io.read_image(os.path.join(color_image_path[i]))
        rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
            color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)
        if debug_mode:
            pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
                rgbd_image,
                o3d.camera.PinholeCameraIntrinsic(
                    o3d.camera.PinholeCameraIntrinsicParameters.
                    PrimeSenseDefault))
            pcd.transform([[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
            pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
            o3d.visualization.draw_geometries([pcd_down])
            pcds.append(pcd_down)
        else:
            camera_poses = read_trajectory("mode\\livingroom1_clean_micro\\test_scene\\trajectory.log")
            volume.integrate(rgbd_image,
            o3d.camera.PinholeCameraIntrinsic(
                o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault),
            np.linalg.inv(camera_poses[i].pose))
        rgbd_images.append(rgbd_image)
    if debug_mode:
        return pcds
    else:
        return volume
from time import time

start_time = time()
volume = o3d.pipelines.integration.ScalableTSDFVolume(
    voxel_length=4.0 / 512.0,
    sdf_trunc=0.04,
    color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)

voxel_size = 0.02
debug_mode=False
pcds_down = load_point_clouds(volume, voxel_size, debug_mode=debug_mode)
if not debug_mode:
    mesh = pcds_down.extract_triangle_mesh()
    mesh.compute_vertex_normals()
    mesh.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
    print("花费时间：", time() - start_time)
    o3d.visualization.draw_geometries([mesh])