Commit d2056dde authored by nviolante25's avatar nviolante25
Browse files

new

parent 323f3860
import cv2 as cv
import numpy as np
from scipy.spatial.transform import Rotation
from collections import namedtuple
import polyscope as ps
ObjectBox = namedtuple(
"ObjectBox", ["coord_system", "vertices", "faces", "name"]
)
CoordSystem = namedtuple("CoordSystem", ["basis", "origin"])
faces = np.array(
[
[3, 2, 0],
[3, 1, 0],
[3, 2, 6],
[3, 7, 6],
[1, 0, 4],
[1, 5, 4],
[7, 5, 4],
[7, 6, 4],
[7, 5, 1],
[7, 3, 1],
[6, 2, 0],
[6, 4, 0],
]
)
interp_dict = {
"bbox12": (
np.array(
[1, 3, 5, 7, 1, 2, 3, 4, 1, 2, 5, 6] # h direction # l direction
), # w direction
np.array([2, 4, 6, 8, 5, 6, 7, 8, 3, 4, 7, 8]),
),
"bbox12l": (
np.array(
[
1,
2,
3,
4,
]
), # w direction
np.array([5, 6, 7, 8]),
),
"bbox12h": (np.array([1, 3, 5, 7]), np.array([2, 4, 6, 8])), # w direction
"bbox12w": (np.array([1, 2, 5, 6]), np.array([3, 4, 7, 8])), # w direction
}
def get_box_height(prediction):
parents = prediction[interp_dict["bbox12"][0] - 1]
children = prediction[interp_dict["bbox12"][1] - 1]
lines = parents - children
lines = np.sqrt(np.sum(lines**2, axis=1))
h = np.sum(lines[:4]) / 4
return h
def get_transform_in_sys(object_sys, transformation):
to_world = np.eye(4)
to_world[:3, -1] = -object_sys.origin
to_sys_center = np.eye(4)
to_sys_center[:3, -1] = object_sys.origin
algin_axes_to_world = np.eye(4)
algin_axes_to_world[:3, :3] = object_sys.basis
return (
to_sys_center
@ np.linalg.inv(algin_axes_to_world)
@ transformation
@ algin_axes_to_world
@ to_world
)
def to_eg3d(extrinsics, height):
eg3d_camera_sys = CoordSystem(
basis=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
origin=np.array([[0, 0, 0]]),
)
translation = np.eye(4, dtype=np.float32)
translation[2, -1] = 2.7
object_origin = transform_points(
eg3d_camera_sys, eg3d_camera_sys.origin, translation @ extrinsics
)
object_basis = (
transform_points(
eg3d_camera_sys,
eg3d_camera_sys.basis + eg3d_camera_sys.origin,
translation @ extrinsics,
)
- object_origin
)
object_sys = CoordSystem(basis=object_basis, origin=object_origin)
# Object system to EG3D object system
rot_x = np.eye(4, dtype=np.float32)
rot_x[:3, :3] = Rotation.from_rotvec([90, 0, 0], degrees=True).as_matrix()
inv_z = np.eye(4, dtype=np.float32)
inv_z[2, 2] = -1
offset = np.eye(4, dtype=np.float32)
offset[1, -1] = -0.5 * height
M = get_transform_in_sys(object_sys, offset @ inv_z @ rot_x)
cam2world = M @ translation @ extrinsics
return cam2world
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]
children = prediction[interp_dict["bbox12"][1] - 1]
lines = parents - children
lines = np.sqrt(np.sum(lines**2, axis=1))
# averaged over the four parallel line segments
h, l, w = (
np.sum(lines[:4]) / 4,
np.sum(lines[4:8]) / 4,
np.sum(lines[8:]) / 4,
)
x_corners = [l, l, l, l, 0, 0, 0, 0]
y_corners = [0, h, 0, h, 0, h, 0, h]
z_corners = [w, w, 0, 0, w, w, 0, 0]
x_corners -= np.float32(l) / 2.0
y_corners -= np.float32(h) / 2.0
z_corners -= np.float32(w) / 2.0
corners_3d = np.array([x_corners, y_corners, z_corners])
return corners_3d.T
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)
def add_box(box: ObjectBox, color):
ps.register_surface_mesh(
box.name, box.vertices, box.faces, color=color, transparency=0.6
)
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)
roty = np.zeros((4, 4), dtype=np.float32)
rotx = np.zeros((4, 4), dtype=np.float32)
roty[:3, :3] = Rotation.from_rotvec([0, 90, 0], degrees=True).as_matrix()
roty[3, 3] = 1.0
rotx[:3, :3] = Rotation.from_rotvec([-90, 0, 0], degrees=True).as_matrix()
rotx[3, 3] = 1.0
egonet2eg3d = rotx @ roty
return egonet2eg3d
def transform_points(object_sys, points, transformation):
to_world = np.eye(4)
to_world[:3, -1] = -object_sys.origin
to_sys_center = np.eye(4)
to_sys_center[:3, -1] = object_sys.origin
algin_axes_to_world = np.eye(4)
algin_axes_to_world[:3, :3] = object_sys.basis
M = (
to_sys_center
@ np.linalg.inv(algin_axes_to_world)
@ transformation
@ algin_axes_to_world
@ to_world
)
new_points = (M @ np.block([points, np.ones((points.shape[0], 1))]).T).T
new_points = new_points[:, :3]
return new_points
def generate_cam2world(rotation):
rot_vec = cv.Rodrigues(rotation[:3, :3])[0]
rot_vec[1] += np.pi / 2.0 + 30 * np.pi / 180
rotation = np.eye(4)
rotation[:3, :3] = cv.Rodrigues(rot_vec)[0]
cam = transform_coord_system(eg3d_object_sys, rotation)
# convert eg3d object sys to egd camera sys
cam2object = np.eye(4)
eg3d_camera = transform_coord_system(eg3d_object_sys, cam2object)
#apply rotation in eg3d camera sys
t = np.eye(4)
t[2, 3] = -2.7
rotation_eg3d = get_transform_in_sys(eg3d_camera, rotation)
eg3d_camera = transform_coord_system(eg3d_camera, rotation_eg3d)
translation_eg3d = get_transform_in_sys(eg3d_camera, t)
eg3d_camera = transform_coord_system(eg3d_camera, translation_eg3d)
align_axes = np.eye(4)
align_axes[:3, :3] = cv.Rodrigues(np.array([0, 0, np.pi]))[0]
align_axes = get_transform_in_sys(eg3d_camera, align_axes)
cam2world = align_axes @ translation_eg3d @ rotation_eg3d
return cam2world
eg3d_object_sys = CoordSystem(
basis=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
origin=np.array([[0, 0, 0]]),
)
if __name__ == "__main__":
nums = [1046, 1052, 1063, 1071, 1075, 1132, 1133, 1134]
# nums = [1134]
boxes = []
cams = []
for num in nums:
record = np.load(
f"./camera_calib/results/img0000{num}_at8.npy", allow_pickle=True
).item()
cam2world = np.array(record['cam2world']).reshape(4,4)
# box = create_box(
# cam2world,
# eg3d_object_sys,
# create_canonical_box(prediction=record["kpts_3d_pred"][0]),
# faces,
# f"car {num}",
# )
# boxes.append(box)
box = ObjectBox(eg3d_object_sys, create_canonical_box(prediction=record['kpts_3d_pred'][0]), faces, 'test')
rotation = record['rotation']
cam2world = generate_cam2world(rotation)
cam = transform_coord_system(eg3d_object_sys, cam2world)
cams.append(cam)
box = ObjectBox(eg3d_object_sys, create_canonical_box(), faces, 'box')
cam2world = np.array([0.9931507213317242, 0.07556412707030787, -0.0891162578791227, 0.2406138962736313,
0.052034207843740826, -0.9689525219152446, -0.24170943607596646, 0.6526154774051095,
-0.10461398535736088, 0.23541680690679007, -0.9662478155698263, 2.6088691020385313,
0.0, 0.0, 0.0, 1.0]).reshape(4, 4)
# cam2world[:3, -1] = 0
cam = transform_coord_system(eg3d_object_sys, cam2world)
cams.append(cam)
cam2world = np.array([0.9931507213317242, -0.07556412707030787, 0.0891162578791227, -0.2406138962736313,
-0.052034207843740826, -0.9689525219152446, -0.24170943607596646, 0.6526154774051095,
0.10461398535736088, 0.23541680690679007, -0.9662478155698263, 2.6088691020385313, 0.0, 0.0, 0.0, 1.0]).reshape(4, 4)
# cam2world[:3, -1] = 0
cam = transform_coord_system(eg3d_object_sys, cam2world)
cams.append(cam)
ps.init()
ps.set_ground_plane_mode("none")
ps.set_up_dir("z_up")
add_coords("EG3D", eg3d_object_sys)
# add_coords(
# "EG3D object system", transform_coord_system(eg3d_camera_sys, cam2world)
# )
# add_coords("cam 1", cam1)
for i, cam in enumerate(cams):
add_coords(f"cam {i}", cam)
add_box(box, (0, 1, 0))
# for box in boxes:
# c = np.random.rand(3)
# add_box(box, color=(c[0], c[1], c[2]))
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