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 save_debug(self):
image_temp = cv2.resize(
self.image_mask,
(2 * self.image_mask.shape[1], 2 * self.image_mask.shape[0]),
interpolation=cv2.INTER_NEAREST)
image_temp = numpy.pad(image_temp, (1, 1),
'constant',
constant_values=(0, 0))
image_temp = tools_image.desaturate(image_temp)
image_temp = tools_draw_numpy.draw_points(
image_temp,
1 + 2 * self.corners_xy[self.signs > 0],
color=(0, 32, 255),
w=0)
image_temp = tools_draw_numpy.draw_points(
image_temp,
1 + 2 * self.corners_xy[self.signs <= 0],
color=(255, 32, 0),
w=0)
cv2.imwrite(self.folder_out + 'debug.png', image_temp)
return
def sraighten_segment(self, segment, min_len=10, do_debug=False):
if len(segment) == 0 or self.get_length_segment(segment) < min_len:
return []
idx_DL = numpy.lexsort(
(-segment[:, 0], segment[:, 1])) # traverce from top
idx_DR = numpy.lexsort(
(segment[:, 0], segment[:, 1])) # traverce from top
idx_RD = numpy.lexsort(
(segment[:, 1], segment[:, 0])) # traverce from left
idx_RU = numpy.lexsort(
(-segment[:, 1], segment[:, 0])) # traverce from left
segment_DL = segment[idx_DL]
segment_DR = segment[idx_DR]
segment_RD = segment[idx_RD]
segment_RU = segment[idx_RU]
sorted_x_DL = segment[idx_DL, 0] # sould decrease
sorted_x_DR = segment[idx_DR, 0] # sould increase
sorted_y_RD = segment[idx_RD, 1] # sould increase
sorted_y_RU = segment[idx_RU, 1] # sould decrease
dx_DL = (-sorted_x_DL +
numpy.roll(sorted_x_DL, -1))[:-1] # should be [-1..0]
dx_DR = (-sorted_x_DR +
numpy.roll(sorted_x_DR, -1))[:-1] # should be [0..+1]
dy_RD = (-sorted_y_RD +
numpy.roll(sorted_y_RD, -1))[:-1] # should be [0..+1]
dy_RU = (-sorted_y_RU +
numpy.roll(sorted_y_RU, -1))[:-1] # should be [-1..0]
th = 1
dx_DL_good = numpy.array(-th <= dx_DL) & numpy.array(dx_DL <= 0)
dx_DR_good = numpy.array(0 <= dx_DR) & numpy.array(dx_DR <= th)
dy_RD_good = numpy.array(0 <= dy_RD) & numpy.array(dy_RD <= th)
dy_RU_good = numpy.array(-th <= dy_RU) & numpy.array(dy_RU <= 0)
if numpy.all(dx_DL_good) or numpy.all(dx_DR_good) or numpy.all(
dy_RD_good) or numpy.all(dy_RU_good):
return [segment]
#search best cut
pos_DL, L_DL = tools_IO.get_longest_run_position_len(dx_DL_good)
pos_DR, L_DR = tools_IO.get_longest_run_position_len(dx_DR_good)
pos_RD, L_RD = tools_IO.get_longest_run_position_len(dy_RD_good)
pos_RU, L_RU = tools_IO.get_longest_run_position_len(dy_RU_good)
best_s = int(numpy.argmax([L_DL, L_DR, L_RD, L_RU]))
pos_best = [pos_DL, pos_DR, pos_RD, pos_RU][best_s]
len_best = [L_DL, L_DR, L_RD, L_RU][best_s]
segment_best = [segment_DL, segment_DR, segment_RD, segment_RU][best_s]
if do_debug:
cv2.imwrite(self.folder_out + 'DL_bin.png',
255 * dx_DL_good.reshape((1, -1)))
cv2.imwrite(self.folder_out + 'DR_bin.png',
255 * dx_DR_good.reshape((1, -1)))
cv2.imwrite(self.folder_out + 'RD_bin.png',
255 * dy_RD_good.reshape((1, -1)))
cv2.imwrite(self.folder_out + 'RU_bin.png',
255 * dy_RU_good.reshape((1, -1)))
image = numpy.full((720, 1280, 3), 64, dtype=numpy.uint8)
cv2.imwrite(
self.folder_out + 'DL.png',
tools_draw_numpy.draw_points(image,
segment_DL,
tools_draw_numpy.get_colors(
len(segment_DL)),
w=1))
cv2.imwrite(
self.folder_out + 'DR.png',
tools_draw_numpy.draw_points(image,
segment_DR,
tools_draw_numpy.get_colors(
len(segment_DR)),
w=1))
cv2.imwrite(
self.folder_out + 'RD.png',
tools_draw_numpy.draw_points(image,
segment_RD,
tools_draw_numpy.get_colors(
len(segment_RD)),
w=1))
cv2.imwrite(
self.folder_out + 'RU.png',
tools_draw_numpy.draw_points(image,
segment_RU,
tools_draw_numpy.get_colors(
len(segment_RU)),
w=1))
#print('len_in=%d pos_best=%d, len_best=%d'%(len(segment), pos_best,len_best))
s1, s2, s3 = [], [], []
if len_best > min_len:
s1 = [segment_best[pos_best:pos_best + len_best]]
if pos_best > 0:
s2 = self.sraighten_segment(segment_best[:pos_best], min_len)
if (pos_best >= 0) and (pos_best + len_best < len(segment_best)):
s3 = self.sraighten_segment(segment_best[pos_best + len_best:],
min_len)
return s1 + s2 + s3
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 BEV_points(self,
image,
filename_points,
do_shift_scale,
target_W,
target_H,
dots_pr_meter=None,
draw_points=True,
major_ticks=None,
minor_ticks=None,
cuboids_3d=None,
points_2d=None,
draw_hits=False,
col_hit=(128, 255, 0),
col_miss=(0, 64, 255),
iou=0.3):
factor = 1.2
gray = tools_image.desaturate(image, level=0.5)
marker_2d, marker_xy, IDs = self.load_points(filename_points)
marker_xy, marker_xy_ext, cuboids_xy_ext, points_2d_ext, image_R, H = self.prepare_assets(
marker_2d, marker_xy, do_shift_scale, target_W, target_H,
dots_pr_meter, factor, major_ticks, minor_ticks, cuboids_3d,
points_2d)
numpy.set_printoptions(precision=4)
print(H)
image_warped = cv2.warpPerspective(gray,
H, (target_W, target_H),
borderValue=(255, 255, 255))
image_warped = tools_draw_numpy.extend_view_from_image(image_warped,
factor=factor,
color_bg=(255,
255,
255))
image_BEV = tools_image.put_layer_on_image(image_R, image_warped,
(255, 255, 255))
if draw_points:
colors_ln = tools_draw_numpy.get_colors(
1, colormap=self.colormap_circles)[::-1]
image_BEV = tools_draw_numpy.draw_points(
image_BEV,
marker_xy_ext[:1],
color=colors_ln[-1].tolist(),
w=8)
if len(cuboids_xy_ext) > 0:
col_obj = tools_draw_numpy.get_colors(
len(cuboids_xy_ext), colormap=self.colormap_objects)
for p, clr, metainfo in zip(cuboids_xy_ext, col_obj,
cuboids_3d.metainfo):
if draw_hits and metainfo is not None:
clr = numpy.array(col_hit) if float(
metainfo) >= iou else numpy.array(col_miss)
image_BEV = tools_draw_numpy.draw_contours(
image_BEV,
p,
color_fill=clr.tolist(),
color_outline=clr.tolist(),
transp_fill=0.3,
transp_outline=1.0)
image_BEV = tools_draw_numpy.draw_lines(
image_BEV,
numpy.array([[p[0, 0], p[0, 1], p[1, 0], p[1, 1]]]),
color=clr.tolist(),
w=5)
if len(points_2d_ext) > 0:
col_obj = tools_draw_numpy.get_colors(
len(points_2d_ext), colormap=self.colormap_objects)
for p, clr in zip(points_2d_ext, col_obj):
if len(p) == 1:
image_BEV = tools_draw_numpy.draw_points(image_BEV,
p,
clr.tolist(),
w=12)
else:
image_BEV = tools_draw_numpy.draw_contours(
image_BEV,
p,
color_fill=clr.tolist(),
color_outline=clr.tolist(),
transp_fill=0.3,
transp_outline=1.0)
if draw_points:
labels = [
'ID %02d: %2.1f,%2.1f' % (pid, p[0], p[1])
for pid, p in zip(IDs, marker_xy)
]
image_BEV = tools_draw_numpy.draw_ellipses(
image_BEV,
[((p[0], p[1]), (25, 25), 0) for p in marker_xy_ext[1:]],
color=(0, 0, 190),
w=4,
labels=labels[1:])
return image_BEV
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