Creating point clouds using perspective RGBD images

[thomasmeeus2]

Hi,

I'm trying to create full point clouds from the RGB and depth images from the full perspective folders. I'm using Open3D to first create RGBD images and then use the intrinsics and extrinsics derived from the "parse_camera_info" function in utils to create the point clouds. However, when I try to merge two point clouds from different images of the same room they don't align properly.

The colors also don't seem to work properly, but this is not as much of a problem than the two point clouds not aligning. I was wondering if I could get some help with my problem. I'll attach my code so it's easier to see where it could go wrong.

Thanks!

import open3d as o3d
import numpy as np
import cv2
import torch
import matplotlib.pyplot as plt

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)

    print("W", W)
    print("U", U)
    print("V", V)

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

    trans = camera_info[:3]
    # myorder = [1, 2, 0]
    # trans = [trans[i] for i in myorder]

    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_raw = o3d.io.read_image(rgb_image_path)
    depth_raw = o3d.io.read_image(depth_image_path)
    rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        color_raw, depth_raw, depth_scale=10000, convert_rgb_to_intensity=False)

    with open(camera_path) as f:
        lines = f.readlines()[0].split(" ")

    # plt.subplot(1,2,1)
    # plt.imshow(np.array(rgbd_image.color))
    # plt.subplot(1,2,2)
    # plt.imshow(np.array(rgbd_image.depth))
    # plt.show()

    camera_info = [float(x) for x in lines]

    rot, trans, K = parse_camera_info(camera_info, np.array(color_raw).shape[0], np.array(color_raw).shape[1])
    print("rot", rot)
    print("trans", np.array(trans))
    print("K", K)

    trans = np.array(trans) / 10000

    w = np.array(color_raw).shape[0]
    h = np.array(color_raw).shape[1]
    fx = K[0][0]
    fy = K[1][1]
    cx = K[0][2]
    cy = K[1][2]

    extrinsic = np.vstack((np.hstack((np.array(rot), np.array([trans]).T)), np.array([0, 0, 0, 1])))
    # extrinsic = np.linalg.inv(extrinsic)
    print("ex", extrinsic)

    intrinsic = o3d.camera.PinholeCameraIntrinsic(w, h, fx,fy, cx, cy)
    cam = o3d.camera.PinholeCameraParameters()
    cam.intrinsic = intrinsic
    cam.extrinsic = extrinsic
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        rgbd_image, cam.intrinsic, cam.extrinsic)
    pcd.transform([[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
    print("--------")
    return pcd

if __name__ == '__main__':
    rgb_image_path = "Structured3D_perspective_full_00/scene_00000/2D_rendering/485142/perspective/full/0/rgb_rawlight.png"
    depth_image_path = "Structured3D_perspective_full_00/scene_00000/2D_rendering/485142/perspective/full/0/depth.png"
    camera_path = "Structured3D_perspective_full_00/scene_00000/2D_rendering/485142/perspective/full/0/camera_pose.txt"

    rgb_image_path_1 = "Structured3D_perspective_full_00/scene_00000/2D_rendering/485142/perspective/full/1/rgb_rawlight.png"
    depth_image_path_1 = "Structured3D_perspective_full_00/scene_00000/2D_rendering/485142/perspective/full/1/depth.png"
    camera_path_1 = "Structured3D_perspective_full_00/scene_00000/2D_rendering/485142/perspective/full/1/camera_pose.txt"

    pcd = create_pcd_perspective(rgb_image_path, depth_image_path, camera_path)
    # o3d.visualization.draw_geometries([pcd])

    pcd_1 = create_pcd_perspective(rgb_image_path_1, depth_image_path_1, camera_path_1)
    # o3d.visualization.draw_geometries([pcd_1])

    pcd_combined = pcd + pcd_1

    o3d.visualization.draw_geometries([pcd_combined])

    o3d.io.write_point_cloud("test.ply", pcd_combined)

[bertjiazheng]

Hi,

It seems that the extrinsic matrix is not correct. Try the following codes:

extrinsic = np.vstack((np.hstack((np.array(rot).T, np.array([trans]).T)), np.array([0, 0, 0, 1])))
extrinsic = np.linalg.inv(extrinsic)

For color issues, try to use cv2 instead.

Best, Jia

[thomasmeeus2]

Dear Jia,

Thank you for the very quick respons. Using cv2 to load in the image does indeed work! However, using the extrinsic matrix that you proposed unfortunately still doesn't align the two point clouds perfectly.

I'm still using the same code as before, only with the change that you proposed. I'm guessing the axis direction of the dataset and the axis direction that Open3D works with are not the same, resulting in the point clouds not aligning. Is there something else I could try?

I'm looking forward to your responds.

Best,

Thomas

[bertjiazheng]

Hi Thomas,

Yes, I guess the camera coordinate system in open3d is different from ours.

Another solution may directly convert the RGB-D image to the point clouds by yourself, instead of using built-in functions in open3d.

Best, Jia

[thomasmeeus2]

Hi Jia,

I accidentally closed the issue. I am indeed now trying to create the point cloud myself. Unfortunately, I'm not very knowledgeable in this. So if you got any tips where I should start, I'll be happy to hear it. Thanks for the help you have given me!

Best, Thomas

[bertjiazheng]

Hi @thomasmeeus2 ,

Could you please try the following code?

Best, Jia

[thomasmeeus2]

Hi @thomasmeeus2 ,

Could you please try the following code?

Best, Jia

Hi Jia,

The code that you have send me does indeed work! Thanks a lot for helping me with my problem.

Kind regards,

Thomas