Commit f05c2ea7 authored by nviolante's avatar nviolante
Browse files

rm convert_to_eg3d

parent 76d41a96
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)
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
if __name__ == "__main__":
nums = [1046, 1052, 1063, 1071, 1075, 1132, 1133, 1134]
boxes = []
for num in nums:
record = np.load(
f"./camera_calib/results/img0000{num}_at8_new.npy", allow_pickle=True
).item()
cam2world = np.array(record['cam2world']).reshape(4,4)
eg3d_camera_sys = CoordSystem(
basis=np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
origin=np.array([[0, 0, 0]]),
)
box = create_box(
cam2world,
eg3d_camera_sys,
create_canonical_box(prediction=record["kpts_3d_pred"][0]),
faces,
f"car {num}",
)
boxes.append(box)
ps.init()
ps.set_ground_plane_mode("none")
ps.set_up_dir("neg_y_up")
add_coords("EG3D", eg3d_camera_sys)
add_coords(
"EG3D object system", transform_coord_system(eg3d_camera_sys, cam2world)
)
for box in boxes:
c = np.random.rand(3)
add_box(box, color=(c[0], c[1], c[2]))
ps.show()
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