101 lines
3.5 KiB
Python
101 lines
3.5 KiB
Python
|
'''
|
||
|
|
@ 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)
|