def draw_points_numpy_MVP(points_3d,
img,
mat_projection,
mat_view,
mat_model,
mat_trns,
color=(66, 0, 166)):
aperture = 0.5 * (1 - mat_projection[2][0])
camera_matrix_3x3 = tools_pr_geom.compose_projection_mat_3x3(
img.shape[1], img.shape[0], aperture, aperture)
M = pyrr.matrix44.multiply(mat_view.T,
pyrr.matrix44.multiply(mat_model.T, mat_trns.T))
X4D = numpy.full((points_3d.shape[0], 4), 1, dtype=numpy.float)
X4D[:, :3] = points_3d[:, :]
L3D = pyrr.matrix44.multiply(M, X4D.T).T[:, :3]
points_2d, jac = cv2.projectPoints(L3D, (0, 0, 0), (0, 0, 0),
camera_matrix_3x3,
numpy.zeros(4, dtype=float))
points_2d = points_2d.reshape((-1, 2))
for point in points_2d:
img = tools_draw_numpy.draw_circle(img, int(point[1]), int(point[0]),
4, color)
points_2d, jac = tools_pr_geom.project_points(L3D, (0, 0, 0), (0, 0, 0),
camera_matrix_3x3,
numpy.zeros(4))
points_2d = points_2d.reshape((-1, 2))
for point in points_2d:
img = tools_draw_numpy.draw_circle(img, int(point[1]), int(point[0]),
2, (255, 255, 255))
return img
def check_p3p(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))
landmarks_2d = landmarks_2d.reshape((-1, 2))
idx = [0, 1, 2]
poses = p3p(landmarks_2d[idx], landmarks_3d[idx], mat_camera)
(R, tvec2) = get_best_pose_p3p(poses, landmarks_2d, landmarks_3d,
mat_camera)
image = tools_render_CV.draw_cube_numpy(image, mat_camera, numpy.zeros(5),
R, tvec2)
cv2.imwrite(folder_out + 'check_p3p.png', image)
return
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 draw_cube(H, W, rvec, tvec):
image = numpy.full((H, W, 3), 64, dtype=numpy.uint8)
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)
image = tools_render_CV.draw_cube_numpy(image, mat_camera, numpy.zeros(5),
rvec, tvec)
cv2.imwrite(folder_out + 'cube.png', image)
return
def example_ray(filename_in, folder_out):
W, H = 800, 800
empty = numpy.full((H, W, 3), 32, dtype=numpy.uint8)
rvec, tvec = numpy.array((0, 0.1, 0)), numpy.array((0, 1, 5))
R = tools_GL3D.render_GL3D(filename_obj=filename_in,
W=W,
H=H,
is_visible=False,
do_normalize_model_file=True,
projection_type='P')
object = tools_wavefront.ObjLoader()
object.load_mesh(filename_in, do_autoscale=True)
points_3d = object.coord_vert
mat_camera = tools_pr_geom.compose_projection_mat_3x3(W, H)
cv2.imwrite(folder_out + 'cube_GL.png',
R.get_image_perspective(rvec, tvec))
cv2.imwrite(
folder_out + 'cube_CV.png',
tools_render_CV.draw_cube_numpy(empty, mat_camera, numpy.zeros(4),
rvec, tvec))
cv2.imwrite(
folder_out + 'cube_CV_MVP.png',
tools_render_CV.draw_cube_numpy_MVP(
numpy.full((H, W, 3), 76, dtype=numpy.uint8), R.mat_projection,
R.mat_view, R.mat_model, R.mat_trns))
cv2.imwrite(
folder_out + 'points_MVP.png',
tools_render_CV.draw_points_numpy_MVP(
points_3d, numpy.full((H, W, 3), 76, dtype=numpy.uint8),
R.mat_projection, R.mat_view, R.mat_model, R.mat_trns))
point_2d = (W - 500, 500)
ray_begin, ray_end = tools_render_CV.get_ray(point_2d, mat_camera,
R.mat_view, R.mat_model,
R.mat_trns)
ray_inter1 = tools_render_CV.line_plane_intersection(
(1, 0, 0), (-1, 0, 1), ray_begin - ray_end, ray_begin)
ray_inter2 = tools_render_CV.line_plane_intersection(
(1, 0, 0), (+1, 0, 1), ray_begin - ray_end, ray_begin)
points_3d_ray = numpy.array([ray_begin, ray_end, ray_inter1, ray_inter2])
result = tools_draw_numpy.draw_ellipse(
empty, ((W - point_2d[0] - 10, point_2d[1] - 10, W - point_2d[0] + 10,
point_2d[1] + 10)), (0, 0, 90))
result = tools_render_CV.draw_points_numpy_MVP(points_3d_ray,
result,
R.mat_projection,
R.mat_view,
R.mat_model,
R.mat_trns,
color=(255, 255, 255))
cv2.imwrite(folder_out + 'points_MVP.png', result)
return
def check_p3l(H, W, rvec, tvec):
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)
mat_camera = tools_pr_geom.compose_projection_mat_3x3(W, H, 0.5, 0.5)
line1 = numpy.hstack(
(landmarks_3d[3].flatten(), landmarks_3d[0].flatten()))
line2 = numpy.hstack(
(landmarks_3d[0].flatten(), landmarks_3d[1].flatten()))
line3 = numpy.hstack(
(landmarks_3d[1].flatten(), landmarks_3d[2].flatten()))
lines_3d = numpy.array([line1, line2, line3])
#landmarks_2d, jac = tools_pr_geom.project_points(landmarks_3d, rvec, tvec, mat_camera, numpy.zeros(5))
#landmarks_2d = landmarks_2d.reshape((-1,2))
#line1 = numpy.hstack((landmarks_2d[3].flatten(),landmarks_2d[0].flatten()))
#line2 = numpy.hstack((landmarks_2d[0].flatten(),landmarks_2d[1].flatten()))
#line3 = numpy.hstack((landmarks_2d[1].flatten(),landmarks_2d[2].flatten()))
#lines_2d = numpy.array([line1,line2,line3])
lines_2d = 0.75 * numpy.array(
[[772, 495, 1020, 520], [1020, 520, 525, 700], [525, 700, 240, 665]])
poses = tools_pr_geom.fit_p3l(lines_3d, lines_2d, mat_camera)
for option, (R, translation) in enumerate(poses):
image = numpy.full((H, W, 3), 64, dtype=numpy.uint8)
lines_start, jac = tools_pr_geom.project_points(
lines_3d[:, :3], R, translation, mat_camera, numpy.zeros(5))
lines_start = lines_start.reshape((-1, 2))
lines_end, jac = tools_pr_geom.project_points(lines_3d[:, 3:], R,
translation, mat_camera,
numpy.zeros(5))
lines_end = lines_end.reshape((-1, 2))
for line_start, line_end in zip(lines_start, lines_end):
cv2.line(image, (int(line_start[0]), int(line_start[1])),
(int(line_end[0]), int(line_end[1])), (0, 128, 255),
thickness=5)
for line in lines_2d:
cv2.line(image, (int(line[0]), int(line[1])),
(int(line[2]), int(line[3])), (0, 0, 255),
thickness=2)
image = tools_render_CV.draw_cube_numpy(image, mat_camera,
numpy.zeros(5), R, translation)
cv2.imwrite(folder_out + 'check_p3l_%02d.png' % option, image)
return
def stage_playground(folder_out,W,H,mat_projection,mat_view,mat_model,mat_trns):
M = pyrr.matrix44.multiply(mat_view.T, pyrr.matrix44.multiply(mat_model.T, mat_trns.T))
rvec, tvec = tools_pr_geom.decompose_to_rvec_tvec(M)
aperture = 0.5 * (1 - mat_projection[2][0])
camera_matrix = tools_pr_geom.compose_projection_mat_3x3(W, H, aperture, aperture)
H = tools_pr_geom.RT_to_H(rvec, tvec, camera_matrix)
empty = numpy.full((R.H, R.W, 3), 32, dtype=numpy.uint8)
result = tools_render_CV.draw_points_numpy_MVP(R.object.coord_vert, empty, R.mat_projection,R.mat_view, R.mat_model, R.mat_trns)
ids = [(f.split('.')[0]).split('_')[1] for f in tools_IO.get_filenames(folder_out, 'screenshot*.png')]
i = 0
if len(ids) > 0: i = 1 + numpy.array(ids, dtype=int).max()
cv2.imwrite(folder_out + 'screenshot_%03d.png' % i, result)
return
def example_ray_interception(filename_in):
W, H = 800, 800
rvec, tvec = (0, 0, 0), (0, 0, 5)
point_2d = (W - 266, 266)
R = tools_GL3D.render_GL3D(filename_obj=filename_in,
W=W,
H=H,
is_visible=False,
do_normalize_model_file=True,
projection_type='P')
cv2.imwrite('./images/output/cube_GL.png',
R.get_image_perspective(rvec, tvec))
mat_camera_3x3 = tools_pr_geom.compose_projection_mat_3x3(W, H)
ray_begin, ray_end = tools_render_CV.get_ray(point_2d, mat_camera_3x3,
R.mat_view, R.mat_model,
R.mat_trns)
triangle = numpy.array([[+1, -2, -2], [+1, -2, 2], [+1, +2, 2]])
collision = tools_render_CV.get_interception_ray_triangle(
ray_begin, ray_end - ray_begin, triangle)
print(collision)
return
def example_project_GL_vs_CV_acuro():
marker_length = 1
aperture_x, aperture_y = 0.5, 0.5
frame = cv2.imread('./images/ex_aruco/01.jpg')
R = tools_GL3D.render_GL3D(filename_obj='./images/ex_GL/box/box.obj',
W=frame.shape[1],
H=frame.shape[0],
is_visible=False,
projection_type='P')
mat_camera = tools_pr_geom.compose_projection_mat_3x3(
frame.shape[1], frame.shape[0], aperture_x, aperture_y)
axes_image, r_vec, t_vec = tools_aruco.detect_marker_and_draw_axes(
frame, marker_length, mat_camera, numpy.zeros(4))
cv2.imwrite(
'./images/output/cube_CV.png',
tools_render_CV.draw_cube_numpy(axes_image, mat_camera, numpy.zeros(4),
r_vec.flatten(), t_vec.flatten(),
(0.5, 0.5, 0.5)))
image_3d = R.get_image_perspective(r_vec.flatten(),
t_vec.flatten(),
aperture_x,
aperture_y,
scale=(0.5, 0.5, 0.5),
do_debug=True)
clr = (255 * numpy.array(R.bg_color)).astype(numpy.int)
cv2.imwrite('./images/output/cube_GL.png',
tools_image.blend_avg(frame, image_3d, clr, weight=0))
r_vec, t_vec = r_vec.flatten(), t_vec.flatten()
print('[ %1.2f, %1.2f, %1.2f], [%1.2f, %1.2f, %1.2f], %1.2f' %
(r_vec[0], r_vec[1], r_vec[2], t_vec[0], t_vec[1], t_vec[2],
aperture_x))
return
def example_project_GL_vs_CV(filename_in, folder_out):
tools_IO.remove_files(folder_out)
W, H = 800, 800
empty = numpy.full((H, W, 3), 32, dtype=numpy.uint8)
rvec, tvec, aperture = (0.0, 0, 0), [+5.0, 0, +15], 0.50
rvec = numpy.array(rvec)
tvec = numpy.array(tvec)
scale = 1.0
R = tools_GL3D.render_GL3D(filename_obj=filename_in,
W=W,
H=H,
is_visible=False,
projection_type='P')
RT_GL = tools_pr_geom.compose_RT_mat(rvec, tvec, do_flip=True)
camera_matrix_3x3 = tools_pr_geom.compose_projection_mat_3x3(
W, H, aperture, aperture)
cv2.imwrite(
folder_out + 'GL.png',
R.get_image_perspective(rvec,
tvec,
aperture,
aperture,
scale=scale * numpy.array((1, 1, 1)),
lookback=False,
do_debug=True))
cv2.imwrite(
folder_out + 'GL_RT.png',
R.get_image_perspective_M(RT_GL,
aperture,
aperture,
scale=scale * numpy.array((1, 1, 1)),
lookback=False,
do_debug=True))
object = tools_wavefront.ObjLoader()
object.load_mesh(filename_in, do_autoscale=True)
points_3d = numpy.array(object.coord_vert, dtype=numpy.float32)
mat_trans = scale * numpy.eye(4)
mat_trans[3, 3] = 1
mat_flip = numpy.eye(4)
mat_flip[0, 0] *= -1
RT_CV = tools_pr_geom.compose_RT_mat(rvec, tvec, do_flip=False)
cv2.imwrite(
folder_out + 'CV_MVP_points.png',
tools_render_CV.draw_points_numpy_MVP(points_3d, empty,
R.mat_projection, R.mat_view,
R.mat_model, R.mat_trns))
cv2.imwrite(
folder_out + 'CV_RT.png',
tools_render_CV.draw_points_numpy_RT(points_3d, empty, RT_CV,
camera_matrix_3x3))
cv2.imwrite(
folder_out + 'CV_numpy.png',
tools_render_CV.draw_cube_numpy(empty, camera_matrix_3x3,
numpy.zeros(4), rvec, tvec))
cv2.imwrite(
folder_out + 'CV_MVP_cube.png',
tools_render_CV.draw_cube_numpy_MVP(empty, R.mat_projection,
mat_flip.dot(R.mat_view),
R.mat_model, R.mat_trns))
cv2.imwrite(
folder_out + 'CV_cuboids_M.png',
tools_draw_numpy.draw_cuboid(empty,
tools_pr_geom.project_points_M(
points_3d, RT_CV, camera_matrix_3x3,
numpy.zeros(5)),
color=(0, 90, 255),
w=2))
cv2.imwrite(
folder_out + 'CV_cuboids.png',
tools_draw_numpy.draw_cuboid(empty,
tools_pr_geom.project_points(
points_3d, rvec, tvec,
camera_matrix_3x3, numpy.zeros(5))[0],
color=(0, 190, 255),
w=2))
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 homography_to_RT(homography, W, H):
fx, fy = W, H
K = tools_pr_geom.compose_projection_mat_3x3(fx, fy)
num, Rs, Ts, Ns = cv2.decomposeHomographyMat(homography, K)
return Rs, Ts, Ns
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]