def get_pose_perspective(self,
image,
landmarks_2d,
landmarks_3d,
mat_trns=None,
idx_match=None):
if mat_trns is None: mat_trns = numpy.eye(4)
if idx_match is None:
idx_match = numpy.arange(0, 68, 1).tolist()
fx, fy = float(image.shape[1]), float(image.shape[0])
self.mat_camera = numpy.array([[fx, 0, fx / 2], [0, fy, fy / 2],
[0, 0, 1]])
dist_coefs = numpy.zeros((4, 1))
if landmarks_3d is None: L3D = self.model_68_points
else: L3D = landmarks_3d.copy()
L3D = numpy.array(
[pyrr.matrix44.apply_to_vector(mat_trns, v) for v in L3D])
#(_, rotation_vector, translation_vector) = cv2.solvePnP(L3D[idx_match], landmarks_2d[idx_match], self.mat_camera, dist_coefs)
#self.r_vec, self.t_vec = -rotation_vector, translation_vector
xxx = L3D[idx_match]
yyy = landmarks_2d[idx_match]
self.r_vec, self.t_vec, landmarks_2d_check = tools_pr_geom.fit_pnp(
L3D[idx_match], landmarks_2d[idx_match], self.mat_camera)
return self.r_vec.flatten(), self.t_vec.flatten()
def check_pnp(H, W, rvec, tvec):
image = numpy.full((H, W, 3), 64, dtype=numpy.uint8)
landmarks_3d = numpy.array(
[[-1, -1, -1], [-1, +1, -1], [+1, +1, -1], [+1, -1, -1], [-1, -1, +1],
[-1, +1, +1], [+1, +1, +1], [+1, -1, +1]],
dtype=numpy.float32)
aperture_x, aperture_y = 0.5, 0.5
mat_camera = tools_pr_geom.compose_projection_mat_3x3(
image.shape[1], image.shape[0], aperture_x, aperture_y)
landmarks_2d, jac = tools_pr_geom.project_points(landmarks_3d, rvec,
tvec, mat_camera,
numpy.zeros(5))
noise = 0 * numpy.random.rand(8, 1, 2)
colors = tools_IO.get_colors(8)
rvec2, tvec2, landmarks_2d_check = tools_pr_geom.fit_pnp(
landmarks_3d, landmarks_2d + noise, mat_camera)
for i, point in enumerate(landmarks_2d_check):
cv2.circle(image, (point[0], point[1]),
5,
colors[i].tolist(),
thickness=-1)
cv2.imwrite(folder_out + 'check_pnp.png', image)
return
def evaluate_K_bruteforce_F(filename_image, filename_points, f_min=1920, f_max=10000):
base_name = filename_image.split('/')[-1].split('.')[0]
image = cv2.imread(filename_image)
gray = tools_image.desaturate(image)
points_2d_all, points_gps_all,IDs = load_points(filename_points)
for f in numpy.arange(f_min,f_max,(f_max-f_min)/100):
points_xyz = shift_origin(points_gps_all, points_gps_all[0])
points_xyz, points_2d = points_xyz[1:], points_2d_all[1:]
labels = ['(%2.1f,%2.1f)' % (p[0], p[1]) for p in points_xyz]
image_AR = gray.copy()
camera_matrix = numpy.array([[f, 0., 1920 / 2], [0., f, 1080 / 2], [0., 0., 1.]])
rvecs, tvecs, points_2d_check = tools_pr_geom.fit_pnp(points_xyz, points_2d, camera_matrix, numpy.zeros(5))
rvecs, tvecs = numpy.array(rvecs).flatten(), numpy.array(tvecs).flatten()
image_AR = tools_draw_numpy.draw_points(image_AR, points_2d, color=(0, 0, 190), w=16,labels=labels)
image_AR = tools_draw_numpy.draw_points(image_AR, points_2d_check, color=(0, 128, 255), w=8)
for R in [10,100,1000]:image_AR = tools_render_CV.draw_compass(image_AR, camera_matrix, numpy.zeros(5), rvecs, tvecs, R)
cv2.imwrite(folder_out + base_name + '_%05d'%(f) + '.png', image_AR)
return
def check_homography(H, W, rvec, tvec):
image = numpy.full((H, W, 3), 64, dtype=numpy.uint8)
landmarks_3d = numpy.array(
[[-1, -1, -1], [-1, +1, -1], [+1, +1, -1], [+1, -1, -1], [-1, -1, 0],
[-1, +1, +0], [+1, +1, +0], [+1, -1, +0]],
dtype=numpy.float32)
aperture_x, aperture_y = 0.5, 0.5
mat_camera = tools_pr_geom.compose_projection_mat_3x3(
image.shape[1], image.shape[0], aperture_x, aperture_y)
landmarks_2d, jac = tools_pr_geom.project_points(
landmarks_3d[[4, 5, 6, 7]], rvec, tvec, mat_camera, numpy.zeros(5))
idx_selected = [4, 5, 6, 7]
colors = tools_IO.get_colors(8)
rvec2, tvec2, landmarks_2d_check = tools_pr_geom.fit_pnp(
landmarks_3d[idx_selected], landmarks_2d, mat_camera)
print('PNP R,T', rvec2, tvec2)
for i, point in enumerate(landmarks_2d_check):
cv2.circle(image, (point[0], point[1]),
5,
colors[idx_selected[i]].tolist(),
thickness=-1)
cv2.imwrite(folder_out + 'check_pnp.png', image)
landmarks_GT, lines_GT = soccer_data.get_GT()
landmarks_GT[:, 0] /= (2000 / 2)
landmarks_GT[:, 0] -= 1
landmarks_GT[:, 1] /= (1500 / 2)
landmarks_GT[:, 1] -= 1
lines_GT[:, [0, 2]] /= (2000 / 2)
lines_GT[:, [0, 2]] -= 1
lines_GT[:, [1, 3]] /= (1500 / 2)
lines_GT[:, [1, 3]] -= 1
image = numpy.full((H, W, 3), 32, dtype=numpy.uint8)
homography, result = tools_pr_geom.fit_homography(
landmarks_GT[[19, 18, 0, 1]], landmarks_2d)
for i, point in enumerate(result.astype(int)):
cv2.circle(image, (point[0], point[1]),
5,
colors[idx_selected[i]].tolist(),
thickness=-1)
cv2.imwrite(folder_out + 'check_homography.png', image)
playground = draw_playground_homography(image,
homography,
landmarks_GT,
lines_GT,
w=1,
R=4)
cv2.imwrite(folder_out + 'check_playground.png', playground)
Rs, Ts, Ns = homography_to_RT(homography, W, H)
camera_matrix_3x3 = tools_pr_geom.compose_projection_mat_3x3(W, H)
points_3d = numpy.array(landmarks_GT[[19, 18, 0, 1]])
points_3d = numpy.hstack((points_3d, numpy.zeros((4, 1))))
for (R, T, N) in zip(Rs, Ts, Ns):
print('Decomp R,T\n', R, T, N)
print()
rotation = R
translation = T
normal = N
points2d, jac = tools_pr_geom.project_points(points_3d, R, T,
camera_matrix_3x3,
numpy.zeros(5))
#draw_playground_RT()
return
def evaluate_fov(self,
filename_image,
filename_points,
a_min=0.4,
a_max=0.41,
do_shift=True,
zero_tvec=False,
list_of_R=[],
virt_obj=None,
do_debug=False):
base_name = filename_image.split('/')[-1].split('.')[0]
image = cv2.imread(filename_image)
W, H = image.shape[1], image.shape[0]
gray = tools_image.desaturate(image)
points_2d_all, points_xyz, IDs = self.load_points(filename_points)
if do_shift:
points_xyz = self.shift_scale(points_xyz, points_xyz[0])
points_xyz, points_2d = points_xyz[1:], points_2d_all[1:]
if len(points_xyz) <= 3:
return numpy.nan, numpy.full(3,
numpy.nan), numpy.full(3, numpy.nan)
err, rvecs, tvecs = [], [], []
a_fovs = numpy.arange(a_min, a_max, 0.005)
for a_fov in a_fovs:
camera_matrix = tools_pr_geom.compose_projection_mat_3x3(
W, H, a_fov, a_fov)
rvec, tvec, points_2d_check = tools_pr_geom.fit_pnp(
points_xyz, points_2d, camera_matrix, numpy.zeros(5))
if zero_tvec:
rvec, tvec, points_2d_check = tools_pr_geom.fit_R(
points_xyz, points_2d, camera_matrix, rvec, tvec)
err.append(
numpy.sqrt(
((points_2d_check - points_2d)**2).sum() / len(points_2d)))
rvecs.append(rvec.flatten())
tvecs.append(tvec.flatten())
idx_best = numpy.argmin(numpy.array(err))
if do_debug:
tools_IO.remove_files(self.folder_out, '*.png')
for i in range(len(a_fovs)):
color_markup = (159, 206, 255)
if i == idx_best: color_markup = (0, 128, 255)
camera_matrix = tools_pr_geom.compose_projection_mat_3x3(
W, H, a_fovs[i], a_fovs[i])
points_2d_check, jac = tools_pr_geom.project_points(
points_xyz, rvecs[i], tvecs[i], camera_matrix,
numpy.zeros(5))
image_AR = tools_draw_numpy.draw_ellipses(gray,
[((p[0], p[1]),
(25, 25), 0)
for p in points_2d],
color=(0, 0, 190),
w=4)
image_AR = tools_draw_numpy.draw_points(
image_AR,
points_2d_check.reshape((-1, 2)),
color=color_markup,
w=8)
for rad in list_of_R:
image_AR = tools_render_CV.draw_compass(image_AR,
camera_matrix,
numpy.zeros(5),
rvecs[i],
tvecs[i],
rad,
color=color_markup)
if virt_obj is not None:
for p in points_xyz:
image_AR = tools_draw_numpy.draw_cuboid(
image_AR,
tools_pr_geom.project_points(
p + self.construct_cuboid_v0(virt_obj),
rvecs[i], tvecs[i], camera_matrix,
numpy.zeros(5))[0])
cv2.imwrite(
self.folder_out + base_name + '_%05d' %
(a_fovs[i] * 1000) + '.png', image_AR)
return a_fovs[idx_best], numpy.array(rvecs)[idx_best], numpy.array(
tvecs)[idx_best]
def AR_points(self,
image,
filename_points,
camera_matrix_init,
dist,
do_shift_scale,
list_of_R=None,
virt_obj=None):
gray = tools_image.desaturate(image)
points_2d, points_gps, IDs = self.load_points(filename_points)
if do_shift_scale:
points_xyz = self.shift_scale(points_gps, points_gps[0])
else:
points_xyz = points_gps.copy()
IDs, points_xyz, points_2d = IDs[1:], points_xyz[1:], points_2d[1:]
image_AR = gray.copy()
camera_matrix = numpy.array(camera_matrix_init, dtype=numpy.float32)
rvecs, tvecs, points_2d_check = tools_pr_geom.fit_pnp(
points_xyz, points_2d, camera_matrix, dist)
rvecs, tvecs = numpy.array(rvecs).flatten(), numpy.array(
tvecs).flatten()
if list_of_R is not None:
RR = -numpy.sort(-numpy.array(list_of_R).flatten())
colors_ln = tools_draw_numpy.get_colors(
len(list_of_R), colormap=self.colormap_circles)[::-1]
for rad, color_ln in zip(RR, colors_ln):
image_AR = tools_render_CV.draw_compass(
image_AR,
camera_matrix,
numpy.zeros(5),
rvecs,
tvecs,
rad,
color=color_ln.tolist())
if virt_obj is not None:
for p in points_xyz:
cuboid_3d = p + self.construct_cuboid_v0(virt_obj)
M = tools_pr_geom.compose_RT_mat(rvecs,
tvecs,
do_rodriges=True,
do_flip=False,
GL_style=False)
P = tools_pr_geom.compose_projection_mat_4x4(
camera_matrix[0, 0], camera_matrix[1, 1],
camera_matrix[0, 2] / camera_matrix[0, 0],
camera_matrix[1, 2] / camera_matrix[1, 1])
image_AR = tools_draw_numpy.draw_cuboid(
image_AR,
tools_pr_geom.project_points_p3x4(cuboid_3d,
numpy.matmul(P, M)))
labels = [
'ID %02d: %2.1f,%2.1f' % (pid, p[0], p[1])
for pid, p in zip(IDs, points_xyz)
]
image_AR = tools_draw_numpy.draw_ellipses(image_AR,
[((p[0], p[1]), (25, 25), 0)
for p in points_2d],
color=(0, 0, 190),
w=4,
labels=labels)
image_AR = tools_draw_numpy.draw_points(image_AR,
points_2d_check,
color=(0, 128, 255),
w=8)
return image_AR