pointcloud verifier

pointcloud_align_pair.py

from icecream import ic
import open3d as o3d
import sys
import os
import numpy as np
import pickle
from copy import deepcopy

if len(sys.argv) != 4:
    # base folder, image 1, image 2
    print("Usage: %s <base_folder> <image number 1> <image number 2>" %
          sys.argv[0])
    sys.exit(1)

traj_path = "/path/to/traj.pickle"
with open(traj_path, 'rb') as f:
    traj = pickle.load(f)

base_folder = sys.argv[1]
color_folder = os.path.join(base_folder, "color")
depth_folder = os.path.join(base_folder, "depth")

image_name_1 = sys.argv[2].zfill(6)+".png"
image_name_2 = sys.argv[3].zfill(6)+".png"

color_1 = o3d.io.read_image(os.path.join(color_folder, image_name_1))
color_2 = o3d.io.read_image(os.path.join(color_folder, image_name_2))
depth_1 = o3d.io.read_image(os.path.join(depth_folder, image_name_1))
depth_2 = o3d.io.read_image(os.path.join(depth_folder, image_name_2))

rgbd_1 = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_1, depth_1, convert_rgb_to_intensity=False)
rgbd_2 = o3d.geometry.RGBDImage.create_from_color_and_depth(
    color_2, depth_2, convert_rgb_to_intensity=False)

print('using aobi module camera intrinsics')
cam_param = o3d.camera.PinholeCameraIntrinsic()
# for aobi
# cam_param.set_intrinsics(640, 480, 514.243, 514.243, 326.907, 226.968)
# for himax
cam_param.set_intrinsics(800, 1280, 1027.857, 1026.579, 337.785, 557.996)

cloud_1 = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_1, cam_param)
cloud_2 = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_2, cam_param)

trans_1 = traj[image_name_1]
trans_1 = np.array(trans_1, dtype=np.float64)
inverse_trans_1 = np.linalg.inv(trans_1)

trans_2 = traj[image_name_2]
trans_2 = np.array(trans_2, dtype=np.float64)
inverse_trans_2 = np.linalg.inv(trans_2)

cloud_1_t = deepcopy(cloud_1).transform(inverse_trans_1)
cloud_2_t = deepcopy(cloud_2).transform(inverse_trans_2)
ic(trans_1, trans_2, inverse_trans_1, inverse_trans_2)
# cloud_1_t = deepcopy(cloud_1).transform(trans_2)
# cloud_2_t = deepcopy(cloud_2).transform(trans_1)

o3d.visualization.draw_geometries([cloud_1_t, cloud_2_t],
                                  zoom=0.7,
                                  front=[0.115, -0.086, -0.989],
                                  lookat=[0.022, 0.004, 0.630],
                                  up=[0.275, -0.954, 0.115])