def __init__(self, offset, geometry):
self.offset = offset
# self.position = (0, 0, 0)
self.original_geometry = geometry
self.geometry = TriangleMesh()
self.geometry += self.original_geometry
# Correctly apply the offset of the geometry, only needs to be done once
self.set_transform()
class GeometryWrapper:
def __init__(self, offset, geometry):
self.offset = offset
# self.position = (0, 0, 0)
self.original_geometry = geometry
self.geometry = TriangleMesh()
self.geometry += self.original_geometry
# Correctly apply the offset of the geometry, only needs to be done once
self.set_transform()
def add_to(self, scene):
scene.add_geometry(self.geometry)
def set_transform(self, position=(0, 0, 0), rotation=identity):
self.geometry.clear()
self.geometry += self.original_geometry
matrix = np.dot(make_matrix(position, rotation),
make_matrix(self.offset, identity))
self.geometry.transform(matrix)
# position_diff = np.ma.subtract(position, self.position)
# rotation_diff =
# self.transform_geometry(position_diff)
# self.position = position
def transform_geometry(self, position):
self.geometry.transform(make_matrix(position))
def compute_vertex_normals(self):
self.original_geometry.compute_vertex_normals()
self.geometry.compute_vertex_normals()
def paint_uniform_color(self, color):
self.original_geometry.paint_uniform_color(color)
self.geometry.paint_uniform_color(color)
def render_car_cv2(self, pose, car_name, image):
"""Render a car instance given pose and car_name
"""
car = self.car_models[car_name]
pose = np.array(pose)
# project 3D points to 2d image plane
rmat = uts.euler_angles_to_rotation_matrix(pose[:3]) # roll, pitch, yaw
rvect, _ = cv2.Rodrigues(rmat)
imgpts, jac = cv2.projectPoints(np.float32(car['vertices']), rvect, pose[3:], self.intrinsic, distCoeffs=None)
mask = np.zeros(image.shape)
for face in car['faces'] - 1:
pts = np.array([[imgpts[idx, 0, 0], imgpts[idx, 0, 1]] for idx in face], np.int32)
pts = pts.reshape((-1, 1, 2))
cv2.polylines(mask, [pts], True, (0, 255, 0))
### Open3d
if False:
from open3d import TriangleMesh, Vector3dVector, Vector3iVector, draw_geometries
mesh = TriangleMesh()
mesh.vertices = Vector3dVector(car['vertices'])
mesh.triangles = Vector3iVector(car['faces'])
mesh.paint_uniform_color([1, 0.706, 0])
car_v2 = np.copy(car['vertices'])
car_v2[:, 2] += car_v2[:, 2].max()*3
mesh2 = TriangleMesh()
mesh2.vertices = Vector3dVector(car_v2)
mesh2.triangles = Vector3iVector(car['faces'])
mesh2.paint_uniform_color([0, 0.706, 0])
draw_geometries([mesh, mesh2])
print("A mesh with no normals and no colors does not seem good.")
print("Painting the mesh")
mesh.paint_uniform_color([1, 0.706, 0])
draw_geometries([mesh])
print("Computing normal and rendering it.")
mesh.compute_vertex_normals()
print(np.asarray(mesh.triangle_normals))
draw_geometries([mesh])
return mask
def open_3d_vis(args, output_dir):
"""
http://www.open3d.org/docs/tutorial/Basic/visualization.html
-- Mouse view control --
Left button + drag : Rotate.
Ctrl + left button + drag : Translate.
Wheel : Zoom in/out.
-- Keyboard view control --
[/] : Increase/decrease field of view.
R : Reset view point.
Ctrl/Cmd + C : Copy current view status into the clipboard. (A nice view has been saved as utilites/view.json
Ctrl/Cmd + V : Paste view status from clipboard.
-- General control --
Q, Esc : Exit window.
H : Print help message.
P, PrtScn : Take a screen capture.
D : Take a depth capture.
O : Take a capture of current rendering settings.
"""
json_dir = os.path.join(
output_dir, 'json_' + args.list_flag + '_trans') + '_iou' + str(1.0)
args.test_dir = json_dir
args.gt_dir = args.dataset_dir + 'car_poses'
args.res_file = os.path.join(output_dir,
'json_' + args.list_flag + '_res.txt')
args.simType = None
car_models = load_car_models(os.path.join(args.dataset_dir, 'car_models'))
det_3d_metric = Detect3DEval(args)
evalImgs = det_3d_metric.evaluate()
image_id = evalImgs['image_id']
print(image_id)
# We only draw the most loose constraint here
gtMatches = evalImgs['gtMatches'][0]
dtScores = evalImgs['dtScores']
mesh_car_all = []
# We also save road surface
xmin, xmax, ymin, ymax, zmin, zmax = np.inf, -np.inf, np.inf, -np.inf, np.inf, -np.inf
for car in det_3d_metric._gts[image_id]:
car_model = car_models[car['car_id']]
R = euler_angles_to_rotation_matrix(car['pose'][:3])
vertices = np.matmul(R, car_model['vertices'].T) + np.asarray(
car['pose'][3:])[:, None]
xmin, xmax, ymin, ymax, zmin, zmax = update_road_surface(
xmin, xmax, ymin, ymax, zmin, zmax, vertices)
mesh_car = TriangleMesh()
mesh_car.vertices = Vector3dVector(vertices.T)
mesh_car.triangles = Vector3iVector(car_model['faces'] - 1)
# Computing normal
mesh_car.compute_vertex_normals()
# we render the GT car in BLUE
mesh_car.paint_uniform_color([0, 0, 1])
mesh_car_all.append(mesh_car)
for i, car in enumerate(det_3d_metric._dts[image_id]):
if dtScores[i] > args.dtScores:
car_model = car_models[car['car_id']]
R = euler_angles_to_rotation_matrix(car['pose'][:3])
vertices = np.matmul(R, car_model['vertices'].T) + np.asarray(
car['pose'][3:])[:, None]
mesh_car = TriangleMesh()
mesh_car.vertices = Vector3dVector(vertices.T)
mesh_car.triangles = Vector3iVector(car_model['faces'] - 1)
# Computing normal
mesh_car.compute_vertex_normals()
if car['id'] in gtMatches:
# if it is a true positive, we render it as green
mesh_car.paint_uniform_color([0, 1, 0])
else:
# else we render it as red
mesh_car.paint_uniform_color([1, 0, 0])
mesh_car_all.append(mesh_car)
# Draw the road surface here
# x = np.linspace(xmin, xmax, 100)
# every 0.1 meter
xyz = get_road_surface_xyz(xmin, xmax, ymin, ymax, zmin, zmax)
# Pass xyz to Open3D.PointCloud and visualize
pcd_road = PointCloud()
pcd_road.points = Vector3dVector(xyz)
pcd_road.paint_uniform_color([0, 0, 1])
mesh_car_all.append(pcd_road)
# draw mesh frame
mesh_frame = create_mesh_coordinate_frame(size=3, origin=[0, 0, 0])
mesh_car_all.append(mesh_frame)
draw_geometries(mesh_car_all)
det_3d_metric.accumulate()
det_3d_metric.summarize()