Commit 41e9a689 authored by nviolante's avatar nviolante
Browse files

wirking object system

parent 2ee31da7
......@@ -23,7 +23,8 @@ interp_dict = {
),
}
ObjectBox = namedtuple('ObjectBox', ['origin', 'basis', 'vertices', 'faces', 'name'])
ObjectBox = namedtuple('ObjectBox', ['coord_system', 'vertices', 'faces', 'name'])
CoordSystem = namedtuple('CoordSystem', ['basis', 'origin'])
faces = np.array([[3, 2, 0],
[3, 1, 0],
......@@ -40,6 +41,15 @@ faces = np.array([[3, 2, 0],
])
def transform_coord_system(coord_system: CoordSystem, transformation):
new_origin = (transformation @ np.block([coord_system.origin, 1.0]).T)
new_origin /= new_origin[-1]
new_origin = new_origin[:3].T
new_basis = (transformation @ np.block([coord_system.basis, np.ones((coord_system.basis.shape[0], 1))]).T).T
new_basis = new_basis[:, :3] - new_origin
return CoordSystem(new_basis, new_origin)
def create_canonical_box(l=2, h=1, w=1, prediction=None):
if prediction is not None:
parents = prediction[interp_dict['bbox12'][0] - 1]
......@@ -58,32 +68,21 @@ def create_canonical_box(l=2, h=1, w=1, prediction=None):
corners_3d = np.array([x_corners, y_corners, z_corners])
return corners_3d.T
def transform_box(transformation, basis, origin, vertices, faces, name):
def create_box(transformation, coord_system, vertices, faces, name):
new_vertices = (transformation @ np.block([vertices, np.ones((vertices.shape[0], 1))]).T).T
new_vertices = new_vertices[:, :3]
new_coord_system = transform_coord_system(coord_system, transformation)
return ObjectBox(new_coord_system, new_vertices, faces, name)
new_origin = (transformation @ np.block([origin, 1.0]).T)
new_origin /= new_origin[-1]
new_origin = new_origin[:3].T
new_basis = (transformation @ np.block([basis, np.ones((basis.shape[0], 1))]).T).T
new_basis = new_basis[:, :3]
return ObjectBox(new_origin, new_basis, new_vertices, faces, name)
def add_box(box: ObjectBox, color, axis=True):
def add_box(box: ObjectBox, color):
ps.register_surface_mesh(box.name, box.vertices, box.faces, color=color, transparency=0.6)
if axis:
object_sys = ps.register_point_cloud(f"{box.name} sys", box.origin, radius=0.02, color=(1,1,1))
object_sys.add_vector_quantity("X", box.basis[0][None, :] - box.origin, length=0.1, enabled=True, radius=0.01, color=(1, 0, 0))
object_sys.add_vector_quantity("Y", box.basis[1][None, :] - box.origin, length=0.1, enabled=True, radius=0.01, color=(0, 1, 0))
object_sys.add_vector_quantity("Z", box.basis[2][None, :] - box.origin, length=0.1, enabled=True, radius=0.01, color=(0, 0, 1))
def add_coords(name, basis, origin):
coord_sys = ps.register_point_cloud(f"{name} sys", origin, radius=0.02, color=(1, 1, 1))
coord_sys.add_vector_quantity("X", basis[0][None, :] - origin, length=0.1, enabled=True, radius=0.01, color=(1, 0, 0))
coord_sys.add_vector_quantity("Y", basis[1][None, :] - origin, length=0.1, enabled=True, radius=0.01, color=(0, 1, 0))
coord_sys.add_vector_quantity("Z", basis[2][None, :] - origin, length=0.1, enabled=True, radius=0.01, color=(0, 0, 1))
add_coords(box.name, box.coord_system)
def add_coords(name: str, coord_system: CoordSystem):
coord_sys = ps.register_point_cloud(f"{name} sys", coord_system.origin, radius=0.02, color=(1, 1, 1))
coord_sys.add_vector_quantity("X", coord_system.basis[0][None, :], length=0.1, enabled=True, radius=0.01, color=(1, 0, 0))
coord_sys.add_vector_quantity("Y", coord_system.basis[1][None, :], length=0.1, enabled=True, radius=0.01, color=(0, 1, 0))
coord_sys.add_vector_quantity("Z", coord_system.basis[2][None, :], length=0.1, enabled=True, radius=0.01, color=(0, 0, 1))
def create_egonet2eg3d():
egonet2eg3d = np.zeros((4, 4), dtype=np.float32)
......@@ -96,48 +95,54 @@ def create_egonet2eg3d():
egonet2eg3d = rotx @ roty
return egonet2eg3d
def create_egonet2eg3d_obj():
base = create_egonet2eg3d()
if __name__ == "__main__":
#------------------------------------------------------------------------------------------------
# EgoNet and EG3D coordinate system
#------------------------------------------------------------------------------------------------
origin = np.array([[0, 0, 0]])
egonet_basis = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]
])
egonet_sys = CoordSystem(basis=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
origin=np.array([[0, 0, 0]])
)
egonet2eg3d = create_egonet2eg3d()
eg3d_basis = (egonet2eg3d @ np.block([egonet_basis, np.ones((3,1))]).T).T
eg3d_basis = eg3d_basis[:, :3]
eg3d_sys = transform_coord_system(egonet_sys, egonet2eg3d)
#------------------------------------------------------------------------------------------------
# Cars
#------------------------------------------------------------------------------------------------
# #------------------------------------------------------------------------------------------------
# # Cars
# #------------------------------------------------------------------------------------------------
record1075 = np.load("./camera_calib/results/img00001075.npy", allow_pickle=True).item()
record1134= np.load("./camera_calib/results/img00001134.npy", allow_pickle=True).item()
E1075 = record1075["extrinsics"].reshape(4, 4)
E1134 = record1134["extrinsics"].reshape(4, 4)
# E1075[:3, :3] = E1075[:3, :3].T
# E1134[:3, :3] = E1134[:3, :3].T
# E1075[:3, :3] = Rotation.from_euler('xyz', Rotation.from_matrix(E1075[:3, :3]).as_euler('yxz')).as_matrix()
# E1134[:3, :3] = Rotation.from_euler('xyz', Rotation.from_matrix(E1134[:3, :3]).as_euler('yxz')).as_matrix()
translation = np.eye(4, dtype=np.float32)
translation[0, -1] = 2.7
box1075 = transform_box(translation @ egonet2eg3d @ E1075,
egonet_basis,
origin,
create_canonical_box(prediction=record1075['kpts_3d_pred'][0]),
faces,
"car 1075")
box1134 = transform_box(translation @ egonet2eg3d @ E1134,
egonet_basis,
origin,
create_canonical_box(prediction=record1134['kpts_3d_pred'][0]),
faces,
"car 1134")
eg3d_object_sys = transform_coord_system(egonet_sys,
egonet2eg3d @ E1075
)
box1075 = create_box(translation @ egonet2eg3d @ E1075,
egonet_sys,
create_canonical_box(prediction=record1075['kpts_3d_pred'][0]),
faces,
"car 1075")
rotx = np.eye(4, dtype=np.float32)
rotx[:3, :3] = Rotation.from_rotvec([90, 0, 0], degrees=True).as_matrix()
inv_y = np.eye(4, dtype=np.float32)
inv_y[2,2] = -1
eg3d_object_sys = transform_coord_system(egonet_sys,
translation @ egonet2eg3d @ E1075 @ inv_y @ rotx
)
#------------------------------------------------------------------------------------------------
......@@ -146,23 +151,13 @@ if __name__ == "__main__":
ps.init()
ps.set_up_dir('z_up')
# ps.register_surface_mesh("canonical", create_canonical_box(prediction=record1134['kpts_3d_pred'][0]), faces, color=(1,0,0))
add_coords("EgoNet sys", egonet_basis, origin)
add_coords('EG3D sys', eg3d_basis, origin)
add_coords("EgoNet", egonet_sys)
add_coords('EG3D', eg3d_sys)
add_coords("test", eg3d_object_sys)
# Cars ------------------
# box = transform_box(np.eye(4),
# egonet_basis,
# origin,
# create_canonical_box(prediction=record1075['kpts_3d_pred'][0]),
# # create_canonical_box(1, 2, 1),
# faces,
# "canonical")
# add_box(box, color=(1, 1, 1), axis=False)
add_box(box1075, color=(0, 1, 0), axis=True)
add_box(box1134, color=(0, 0, 1), axis=False)
# ps.register_point_cloud("1075", record1075['kpts_3d_pred'][0])
# ps.register_point_cloud("1134", record1134['kpts_3d_pred'][0])
add_box(box1075, color=(0, 1, 0))
ps.show()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment