def construct_cuboid(self, dim, rvec, tvec):
d0, d1, d2 = dim[0], dim[1], dim[2]
#x_corners = [+d0/2, +d0/2, -d0/2, -d0/2, +d0/2, +d0/2, -d0/2, -d0/2]
#y_corners = [+d1/2, +d1/2, +d1/2, +d1/2, -d1/2, -d1/2, -d1/2, -d1/2]
#z_corners = [+d2/2, -d2/2, -d2/2, +d2/2, +d2/2, -d2/2, -d2/2, +d2/2]
x_corners = [
-d0 / 2, -d0 / 2, +d0 / 2, +d0 / 2, +d0 / 2, +d0 / 2, -d0 / 2,
-d0 / 2
]
y_corners = [
-d1 / 2, +d1 / 2, -d1 / 2, +d1 / 2, -d1 / 2, +d1 / 2, -d1 / 2,
+d1 / 2
]
z_corners = [
-d2 / 2, -d2 / 2, -d2 / 2, -d2 / 2, +d2 / 2, +d2 / 2, +d2 / 2,
+d2 / 2
]
X = numpy.array([x_corners, y_corners, z_corners],
dtype=numpy.float32).T
RT = tools_pr_geom.compose_RT_mat(rvec,
tvec,
do_rodriges=True,
do_flip=False,
GL_style=False)
Xt = tools_pr_geom.apply_matrix_GL(RT, X)[:, :3]
uu = 0
return Xt
def init_mat_model(self, rvec, tvec, do_rodriges=False, do_flip=False):
self.mat_model = tools_pr_geom.compose_RT_mat(rvec,
tvec,
do_rodriges=do_rodriges,
do_flip=do_flip)
glUniformMatrix4fv(glGetUniformLocation(self.shader, "model"), 1,
GL_FALSE, self.mat_model)
return
def __init_mat_model(self,rvec, tvec,do_rodriges=False):
self.mat_model = tools_pr_geom.compose_RT_mat(rvec, tvec, do_flip=False, do_rodriges=do_rodriges)
glUniformMatrix4fv(glGetUniformLocation(self.shader, "model"), 1, GL_FALSE, self.mat_model)
# check
#rvec_model, tvec_model = tools_pr_geom.decompose_to_rvec_tvec(self.mat_model)
#print(rvec,tvec)
#print(rvec_model,tvec_model)
#print()
return
def __init_mat_view_RT(self, rvec,tvec,do_flip=True,do_rodriges=False):
self.mat_view = tools_pr_geom.compose_RT_mat(rvec,tvec,do_flip,do_rodriges)
glUniformMatrix4fv(glGetUniformLocation(self.shader, "view"), 1, GL_FALSE, self.mat_view)
#check
#rvec_view, tvec_view = tools_pr_geom.decompose_to_rvec_tvec(self.mat_view,do_flip=do_flip)
#print(rvec,tvec)
#print(rvec_view,tvec_view)
#print()
return
def get_pretty_model_rotation(self, mat_model):
if mat_model is None: return numpy.nan, numpy.nan, numpy.nan
rvec, tvec = tools_pr_geom.decompose_to_rvec_tvec(mat_model)
a_pitch_deg, a_yaw_deg, a_roll_deg = rvec[[0, 1, 2]] * 180 / numpy.pi
#check
rvec_check = numpy.array(
(a_pitch_deg, a_yaw_deg, a_roll_deg)) * numpy.pi / 180
mat_model_check3 = tools_pr_geom.compose_RT_mat(rvec_check, (0, 0, 0),
do_rodriges=False,
do_flip=False)
return a_pitch_deg, a_yaw_deg, a_roll_deg
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 centralize_points(self,
filename_image,
filename_points_in,
filename_points_out,
mat_camera,
rvec,
tvec,
do_debug=False):
image = cv2.imread(filename_image)
H, W = image.shape[:2]
points_2d_all, points_gps_all, IDs = self.load_points(
filename_points_in)
points_xyz = self.shift_scale(points_gps_all, points_gps_all[0])
M = tools_pr_geom.compose_RT_mat(rvec,
tvec,
do_rodriges=True,
do_flip=False,
GL_style=False)
M_GL = tools_pr_geom.compose_RT_mat(rvec,
tvec,
do_rodriges=True,
do_flip=True,
GL_style=True)
P = tools_pr_geom.compose_projection_mat_4x4(
mat_camera[0, 0], mat_camera[1, 1],
mat_camera[0, 2] / mat_camera[0, 0],
mat_camera[1, 2] / mat_camera[1, 1])
loss, a_yaws = [], numpy.arange(0, 360, 1)
cuboid_3d_centred = self.construct_cuboid_v0((-1, +30, 0, +1, +31, 1))
for a_yaw in a_yaws:
R_CV = tools_pr_geom.compose_RT_mat((0, 0, a_yaw * numpy.pi / 180),
(0, 0, 0),
do_rodriges=True,
do_flip=False,
GL_style=False)
cuboid_r_CV = tools_pr_geom.apply_matrix_GL(
R_CV.T, cuboid_3d_centred)
points_2d = tools_pr_geom.project_points_p3x4(cuboid_r_CV,
numpy.matmul(P, M),
check_reverce=True)
if not numpy.isnan(points_2d[:, 0].mean()):
loss.append(numpy.abs(points_2d[:, 0].mean() - W / 2))
else:
loss.append(numpy.inf)
#if do_debug:cv2.imwrite(self.folder_out + 'AR2_GL_%03d.png' % (a_yaw),tools_draw_numpy.draw_cuboid(gray,points_2d,color=(0, 0, 255)))
a_yaw = a_yaws[numpy.argmin(loss)]
R_CV = tools_pr_geom.compose_RT_mat((0, 0, -a_yaw * numpy.pi / 180),
(0, 0, 0),
do_rodriges=True,
do_flip=True,
GL_style=False)
R_GL = tools_pr_geom.compose_RT_mat((0, 0, -a_yaw * numpy.pi / 180),
(0, 0, 0),
do_rodriges=True,
do_flip=True,
GL_style=True)
iR_CV = numpy.linalg.inv(R_CV)
iR_GL = numpy.linalg.inv(R_GL)
points_xyz_t_CV = tools_pr_geom.apply_matrix_GL(R_CV.T, points_xyz)
points_xyz_t_GL = tools_pr_geom.apply_matrix_GL(R_GL, points_xyz)
points_xyz_t_CV[:, 1] *= -1
points_xyz_t_GL[:, 1] *= -1
flip = numpy.identity(4)
flip[1][1] = -1
M_new_CV = numpy.matmul(M, iR_CV)
M_new_CV = numpy.matmul(M_new_CV, flip)
M_new_GL = numpy.matmul(iR_GL, M_GL)
M_new_GL = numpy.matmul(flip, M_new_GL)
#check
gray = tools_image.desaturate(image)
fov = mat_camera[0, 2] / mat_camera[0, 0]
mat_projection = tools_pr_geom.compose_projection_mat_4x4_GL(
W, H, fov, fov)
# cv2.imwrite(self.folder_out + 'GL_0.png',tools_render_CV.draw_points_numpy_MVP_GL(points_xyz, gray, mat_projection, M_GL, numpy.eye(4),numpy.eye(4),w=8))
# cv2.imwrite(self.folder_out + 'GL_1.png',tools_render_CV.draw_points_numpy_MVP_GL(points_xyz_t_GL, gray, mat_projection, M_new_GL, numpy.eye(4),numpy.eye(4),w=8))
# cv2.imwrite(self.folder_out + 'CV_0.png',tools_draw_numpy.draw_points(gray,tools_pr_geom.project_points_p3x4(points_xyz, numpy.matmul(P, M)),color=(0, 0, 255),w=10))
# cv2.imwrite(self.folder_out + 'CV_1.png',tools_draw_numpy.draw_points(gray,tools_pr_geom.project_points_p3x4(points_xyz_t_CV, numpy.matmul(P, M_new_CV)),color=(0, 0, 255), w=10))
self.save_points(filename_points_out, IDs[1:], points_2d_all[1:],
points_xyz_t_CV[1:], W, H)
mat_camera_new = numpy.matmul(P, M_new_CV)
return mat_camera_new
def evaluate_matrices_GL(self,
image,
rvec,
tvec,
a_fov,
points_3d=None,
virt_obj=None,
do_debug=False):
if numpy.any(numpy.isnan(rvec)) or numpy.any(numpy.isnan(tvec)):
return numpy.full((4, 4), numpy.nan), numpy.full(
(4, 4), numpy.nan), numpy.full((4, 4), numpy.nan)
H, W = image.shape[:2]
mR = tools_pr_geom.compose_RT_mat(rvec, (0, 0, 0),
do_rodriges=True,
do_flip=True,
GL_style=True)
mRT = tools_pr_geom.compose_RT_mat(rvec,
tvec,
do_rodriges=True,
do_flip=True,
GL_style=True)
imR = numpy.linalg.inv(mR)
mat_projection = tools_pr_geom.compose_projection_mat_4x4_GL(
W, H, a_fov, a_fov)
T = numpy.matmul(mRT, imR)
if do_debug:
import tools_GL3D
cuboid_3d = self.construct_cuboid_v0(virt_obj)
gray = tools_image.desaturate(cv2.resize(image, (W, H)))
filename_obj = self.folder_out + 'temp.obj'
self.save_obj_file(filename_obj, virt_obj)
R = tools_GL3D.render_GL3D(filename_obj=filename_obj,
W=W,
H=H,
do_normalize_model_file=False,
is_visible=False,
projection_type='P',
textured=False)
tools_IO.remove_file(filename_obj)
cv2.imwrite(
self.folder_out + 'AR0_GL_m1.png',
R.get_image_perspective(rvec,
tvec,
a_fov,
a_fov,
mat_view_to_1=False,
do_debug=True))
cv2.imwrite(
self.folder_out + 'AR0_GL_v1.png',
R.get_image_perspective(rvec,
tvec,
a_fov,
a_fov,
mat_view_to_1=True,
do_debug=True))
cv2.imwrite(
self.folder_out + 'AR0_CV_cube.png',
tools_render_CV.draw_cube_numpy_MVP_GL(gray,
mat_projection,
numpy.eye(4),
mRT,
numpy.eye(4),
points_3d=cuboid_3d))
cv2.imwrite(
self.folder_out + 'AR0_CV_pnts.png',
tools_render_CV.draw_points_numpy_MVP_GL(points_3d,
gray,
mat_projection,
numpy.eye(4),
mRT,
numpy.eye(4),
w=8))
#cv2.imwrite(self.folder_out + 'AR1_CV_pnts.png',tools_render_CV.draw_points_numpy_MVP_GL(points_3dt, gray, mat_projection, numpy.eye(4), mR,numpy.eye(4), w=8))
cv2.imwrite(
self.folder_out + 'AR1_CV.png',
tools_render_CV.draw_cube_numpy_MVP_GL(gray,
mat_projection,
mR,
numpy.eye(4),
T,
points_3d=cuboid_3d))
cv2.imwrite(
self.folder_out + 'AR1_GL.png',
R.get_image(mat_view=numpy.eye(4),
mat_model=mR,
mat_trans=T,
do_debug=True))
return numpy.eye(4), mR, T
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