''' @ Date: 2022-09-26 16:32:19 @ Author: Qing Shuai @ Mail: s_q@zju.edu.cn @ LastEditors: Qing Shuai @ LastEditTime: 2022-10-17 13:05:28 @ FilePath: /EasyMocapPublic/apps/calibration/vis_camera_by_open3d.py ''' import open3d as o3d import os import cv2 import numpy as np from easymocap.mytools.camera_utils import read_cameras from easymocap.visualize.o3dwrapper import Vector3dVector, create_pcd from easymocap.mytools.vis_base import generate_colorbar def transform_cameras(cameras): dims = {'x': 0, 'y': 1, 'z': 2} R_global = np.eye(3) T_global = np.zeros((3, 1)) # order: trans0, rot, trans if len(args.trans0) == 3: trans = np.array(args.trans0).reshape(3, 1) T_global += trans if len(args.rot) > 0: for i in range(len(args.rot)//2): dim = args.rot[2*i] val = float(args.rot[2*i+1]) rvec = np.zeros((3,)) rvec[dims[dim]] = np.deg2rad(val) R = cv2.Rodrigues(rvec)[0] R_global = R @ R_global T_global = R_global @ T_global # 平移相机 if len(args.trans) == 3: trans = np.array(args.trans).reshape(3, 1) T_global += trans trans = np.eye(4) trans[:3, :3] = R_global trans[:3, 3:] = T_global # apply the transformation of each camera for key, cam in cameras.items(): RT = np.eye(4) RT[:3, :3] = cam['R'] RT[:3, 3:] = cam['T'] RT = RT @ np.linalg.inv(trans) cam.pop('Rvec', '') cam['R'] = RT[:3, :3] cam['T'] = RT[:3, 3:] return cameras, trans if __name__ == '__main__': import argparse parser = argparse.ArgumentParser() parser.add_argument('path', type=str) parser.add_argument('--subs', type=str, default=[], nargs='+') parser.add_argument('--pcd', type=str, default=[], nargs='+') parser.add_argument('--trans0', type=float, nargs=3, default=[], help='translation') parser.add_argument('--rot', type=str, nargs='+', default=[], help='control the rotation') parser.add_argument('--trans', type=float, nargs=3, default=[], help='translation') parser.add_argument('--debug', action='store_true') args = parser.parse_args() grids = [] cameras = read_cameras(args.path) cameras, trans = transform_cameras(cameras) print(repr(trans)) for pcd in args.pcd: if not os.path.exists(pcd): print(pcd, ' not exist') continue if pcd.endswith('.npy'): data = np.load(pcd) points = data[:, :3] colors = data[:, 3:] points = (trans[:3,:3] @ points.T + trans[:3,3:]).T p = create_pcd(points, colors=data[:, 3:]) grids.append(p) elif pcd.endswith('.ply'): p = o3d.io.read_point_cloud(pcd) grids.append(p) center = o3d.geometry.TriangleMesh.create_coordinate_frame( size=1, origin=[0, 0, 0]) grids.append(center) colorbar = generate_colorbar(len(cameras), rand=False) camera_locations = [] for ic, (cam, camera) in enumerate(cameras.items()): if len(args.subs) > 0 and cam not in args.subs:continue center = - camera['R'].T @ camera['T'] camera_locations.append(center) center = o3d.geometry.TriangleMesh.create_coordinate_frame( size=0.5, origin=[center[0, 0], center[1, 0], center[2, 0]]) center.rotate(camera['R'].T) grids.append(center) # TODO: add label camera_locations = np.stack(camera_locations).reshape(-1, 3) o3d.visualization.draw_geometries(grids)