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 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 example_calibrate_aruco_markers(filename_in, marker_length_mm = 3.75, marker_space_mm = 0.5, dct = aruco.DICT_6X6_1000):
image = cv2.imread(filename_in)
gray = tools_image.desaturate(image)
scale = (marker_length_mm / 2, marker_length_mm / 2, marker_length_mm / 2)
num_cols, num_rows = 4,5
board = aruco.GridBoard_create(num_cols, num_rows, marker_length_mm, marker_space_mm,aruco.getPredefinedDictionary(dct))
image_AR, image_cube = gray.copy(), gray.copy()
base_name, ext = filename_in.split('/')[-1].split('.')[0], filename_in.split('/')[-1].split('.')[1]
#board_width_px = int((num_cols * marker_length_mm + (num_cols - 1) * marker_space_mm))
#board_height_px= int((num_rows * marker_length_mm + (num_rows - 1) * marker_space_mm))
#image_board = aruco.drawPlanarBoard(board, (board_width_px, board_height_px))
camera_matrix = None#tools_pr_geom.compose_projection_mat_3x3(image.shape[1], image.shape[0])
corners, ids, _ = aruco.detectMarkers(cv2.cvtColor(image, cv2.COLOR_BGR2GRAY), aruco.getPredefinedDictionary(dct))
if len(corners)>0:
if len(corners)==1:corners,ids = numpy.array([corners]),numpy.array([ids])
else:corners, ids = numpy.array(corners), numpy.array(ids)
counters = numpy.array([len(ids)])
ret, camera_matrix, dist, rvecs, tvecs = aruco.calibrateCameraAruco(corners,ids,counters, board, gray.shape[:2],None, None)
image_markers = [tools_image.saturate(aruco.drawMarker(aruco.getPredefinedDictionary(dct), id, 100)) for id in ids]
#!!!
#camera_matrix = tools_pr_geom.compose_projection_mat_3x3(4200,4200)
for i,image_marker in enumerate(image_markers):
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners[i], marker_length_mm, camera_matrix, numpy.zeros(5))
image_AR = tools_render_CV.draw_image(image_AR,image_marker, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(), numpy.array(tvecs).flatten(),scale)
image_cube = tools_render_CV.draw_cube_numpy(image_cube, camera_matrix, numpy.zeros(5), numpy.array(rvecs).flatten(),numpy.array(tvecs).flatten(), scale)
cv2.imwrite(folder_out + base_name+'_AR.png', image_AR)
cv2.imwrite(folder_out + base_name+'_AR_cube.png', image_cube)
print(camera_matrix)
return camera_matrix
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