Fusion the multi-view RGB-D perspective data in Structured3D dataset

fusion.py

import cv2
import numpy as np
import open3d as o3d


def normalize(vector):
    return vector / np.linalg.norm(vector)
  

def parse_camera_info(camera_info, height, width):
    """ extract intrinsic and extrinsic matrix
    """
    lookat = normalize(camera_info[3:6])
    up = normalize(camera_info[6:9])

    W = lookat
    U = np.cross(W, up)
    V = np.cross(W, U)

    rot = np.vstack((U, V, W))

    trans = camera_info[:3]

    xfov = camera_info[9]
    yfov = camera_info[10]

    K = np.diag([1, 1, 1])

    K[0, 2] = width / 2
    K[1, 2] = height / 2

    K[0, 0] = K[0, 2] / np.tan(xfov)
    K[1, 1] = K[1, 2] / np.tan(yfov)

    return rot, trans, K

  
def create_pcd_perspective(rgb_image_path, depth_image_path, camera_path):    
    color = cv2.imread(rgb_image_path)
    depth = cv2.imread(depth_image_path, cv2.IMREAD_UNCHANGED).astype(np.float32) / 1000.0

    height, width = color.shape[:2]

    camera_info = np.loadtxt(camera_path)

    rot, trans, K = parse_camera_info(camera_info, height, width)

    trans = np.array(trans) / 1000

    color_o3d = o3d.geometry.Image(color)
    depth_o3d = o3d.geometry.Image(depth)

    rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        color_o3d, depth_o3d, depth_scale=1.0, depth_trunc=10.0, convert_rgb_to_intensity=False)

    extrinsic = np.ones(4)
    extrinsic = np.eye(4)
    extrinsic[:3, :3] = rot.T
    extrinsic[:3, -1] = trans
    extrinsic = np.linalg.inv(extrinsic)

    intrinsic = o3d.camera.PinholeCameraIntrinsic(width, height, K[0][0], K[1][1], K[0][2], K[1][2])
    pointcloud = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic, extrinsic)

    return pointcloud