def render_car(self, car_pose, image, intrinsic, fill=False):
pose = np.array(car_pose['pose'])
car = self.get_car(car_pose['car_id'])
# Intrinsic dependent on image size
h, w = image.shape[:2]
intrinsic = uts.intrinsic_vec_to_mat(intrinsic, (h, w))
# project 3D points to 2d image plane
rmat = uts.euler_angles_to_rotation_matrix(pose[:3])
rvect, _ = cv2.Rodrigues(rmat)
imgpts, jac = cv2.projectPoints(np.float32(car['vertices']),
rvect,
pose[3:],
intrinsic,
distCoeffs=None)
mask = np.zeros((h, w), dtype='uint8')
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))
if fill:
cv2.fillPoly(mask, [pts], 255)
else:
cv2.polylines(mask, [pts], True, 255, thickness=2)
return mask
def vis_car(car, pose):
# project 3D points to 2d image plane
rmat = uts.euler_angles_to_rotation_matrix(pose[:3])
rvect, _ = cv2.Rodrigues(rmat)
imgpts, jac = cv2.projectPoints(np.float32(car['vertices']),
rvect,
pose[3:],
intrinsic,
distCoeffs=None)
def _to_proj_mat(self, rot, trans):
"""convert 6 dof represenatation to projection matrix
"""
ext = np.zeros((4, 4), dtype=np.float32)
ext[:3, :3] = uts.euler_angles_to_rotation_matrix(rot)
ext[:3, 3] = trans
ext[3, 3] = 1.0
ext = np.linalg.inv(ext)
ext = np.transpose(ext)
ext = np.float32(ext.flatten())
return ext
def render_car_cv2(self, pose, car_name, image):
"""Render a car instance given pose and car_name
"""
car = self.car_models[
car_name] #composed by many vertices and face meshes
pose = np.array(pose) #rotation and tranlation vector with shape (6)
# project 3D points to 2d image plane
# euler angles: successively rotate on Z-Y-X
rmat = uts.euler_angles_to_rotation_matrix(pose[:3])
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] * mask.shape[1],
imgpts[idx, 0, 1] * mask.shape[0]
] 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 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])
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))
return mask
def findTrans(self, image_name):
"""Show the annotation of a pose file in an image
Input:
image_name: the name of image
Output:
depth: a rendered depth map of each car
masks: an instance mask of the label
image_vis: an image show the overlap of car model and image
"""
car_pose_file = '%s/%s.json' % (self._data_config['pose_dir'],
image_name)
with open(car_pose_file) as f:
car_poses = json.load(f)
image_file = '%s/%s.jpg' % (self._data_config['image_dir'], image_name)
image = cv2.imread(image_file, cv2.IMREAD_UNCHANGED)[:, :, ::-1]
#intrinsic = self.dataset.get_intrinsic(image_name)
### we use only camera5 intrinsics
intrinsic = self.dataset.get_intrinsic("Camera_5")
self.intrinsic = uts.intrinsic_vec_to_mat(intrinsic)
merged_image = image.copy()
dis_trans_all = []
for car_pose in car_poses:
car_name = car_models.car_id2name[car_pose['car_id']].name
car = self.car_models[car_name]
pose = np.array(car_pose['pose'])
# project 3D points to 2d image plane
rmat = uts.euler_angles_to_rotation_matrix(pose[:3])
x_y_z_R = np.matmul(rmat,
np.transpose(np.float32(car['vertices'])))
x_y_z = x_y_z_R + pose[3:][:, None]
x2 = x_y_z[0] / x_y_z[2]
y2 = x_y_z[1] / x_y_z[2]
u = intrinsic[0] * x2 + intrinsic[2]
v = intrinsic[1] * y2 + intrinsic[3]
###
fx = intrinsic[0]
fy = intrinsic[1]
cx = intrinsic[2]
cy = intrinsic[3]
xc = ((u.max() + u.min()) / 2 - cx) / fx
yc = ((v.max() + v.min()) / 2 - cy) / fy
ymin = (v.min() - cy) / fy
ymax = (v.max() - cy) / fy
Rymin = x_y_z_R[1, :].min()
Rymax = x_y_z_R[1, :].max()
Rxc = x_y_z_R[0, :].mean()
Ryc = x_y_z_R[1, :].mean()
Rzc = x_y_z_R[2, :].mean()
# Rxc = 0
# Ryc = 0
# Rzc = 0
# Rxc = (x_y_z_R[0, :].max() + x_y_z_R[0, :].min())/2
# Ryc = (x_y_z_R[1, :].max() + x_y_z_R[1, :].min())/2
# Rzc = (x_y_z_R[2, :].max() + x_y_z_R[2, :].min())/2
# Because the car highest point happened in the center!
#zc = (Ryc - Rymin) / (yc - ymin)
zc = (Ryc - Rymax) / (yc - ymax)
xt = zc * xc - Rxc
yt = zc * yc - Ryc
zt = zc - Rzc
pred_pose = np.array([xt, yt, zt])
dis_trans = np.linalg.norm(pred_pose - pose[3:])
# pose_pred_all = np.concatenate([car_pose['pose'][:3], pred_pose])
# mask = self.render_car_cv2(pose_pred_all, car_name, image)
# cv2.addWeighted(image.astype(np.uint8), 1.0, mask.astype(np.uint8), 0.5, 0, merged_image)
# plt.imshow(merged_image)
print(dis_trans)
dis_trans_all.append(dis_trans)
return dis_trans_all
if False:
xmin = (u.min() - cx) / fx
xmax = (u.max() - cx) / fx
ymin = (v.min() - cy) / fy
ymax = (v.max() - cy) / fy
Rxmin = x_y_z_R[0, :].min()
Rxmax = x_y_z_R[0, :].max()
Rymin = x_y_z_R[1, :].min()
Rymax = x_y_z_R[1, :].max()
Rzmin = x_y_z_R[2, :].min()
Rzmax = x_y_z_R[2, :].max()
# z1 = (Rxmax - Rxmin) / (xmax - xmin)
# z2 = (Rymax - Rymin) / (ymax - ymin)
#xt = (xmax*xmin) /(ymax*xmin-ymin*xmax) * (ymin*Rxmin/xmin - ymax*Rxmax/ymin - Rymin)
xt = (Rxmax * xmin - Rxmin * xmax) / (xmax - xmin)
yt = (Rymax * ymin - Rymin * ymax) / (ymax - ymin)
ztxmin = (xt + Rxmin) / xmin - Rzmin
ztxmax = (xt + Rxmax) / xmax - Rzmin
ztymin = (yt + Rymin) / ymin - Rzmin
ztymax = (yt + Rymax) / ymax - Rzmin
pred_pose = np.array([xt, yt, ztymin])
dis_trans = np.linalg.norm(pred_pose - pose[3:])
pred_pose = np.array([xt, yt, ztxmin])
dis_trans = np.linalg.norm(pred_pose - pose[3:])