def example_01(filename_in): sobel = numpy.zeros((8, 17), dtype=numpy.float32) sobel[:, sobel.shape[1] // 2:] = +1 sobel[:, :sobel.shape[1] // 2] = -1 sobel = sobel / sobel.sum() image = cv2.imread(filename_in) gray = cv2.imread(filename_in, 0) result = cv2.filter2D(gray, 0, sobel) result2 = numpy.maximum(255 - result, result) result2 = exposure.adjust_gamma(result2, 6) image_bin = Ske.binarize(result2) image_ske = Ske.binarized_to_skeleton(image_bin) segemnts = Ske.skeleton_to_segments(image_ske) colors = tools_draw_numpy.get_colors(len(segemnts), shuffle=True) image_segm = tools_draw_numpy.draw_segments(tools_image.desaturate(image), segemnts, colors, w=1) cv2.imwrite(folder_out + 'filtered.png', result) cv2.imwrite(folder_out + 'filtered2.png', result2) cv2.imwrite(folder_out + 'ske.png', image_ske) cv2.imwrite(folder_out + 'segm.png', image_segm) return
def debug_projection_2d(X_target, X_result): colors_s = tools_draw_numpy.get_colors(len(X_target)) colors_s = numpy.array( [numpy.array((255, 255, 255.0)) * 0.25 + c * 0.75 for c in colors_s], dtype=numpy.uint8) padding = 10 xmin = int(min(X_target[:, 0].min(), X_result[:, 0].min())) - padding xmax = int(max(X_target[:, 0].max(), X_result[:, 0].max())) + padding ymin = int(min(X_target[:, 1].min(), X_result[:, 1].min())) - padding ymax = int(max(X_target[:, 1].max(), X_result[:, 1].max())) + padding image = numpy.full((ymax - ymin + 1, xmax - xmin + 1, 3), 32, dtype=numpy.uint8) for i, (x, y) in enumerate(X_result): cv2.circle( image, (int(x - xmin), int(y - ymin)), 12, (int(colors_s[i][0]), int(colors_s[i][1]), int(colors_s[i][2])), 2) for i, (x, y) in enumerate(X_target): cv2.circle(image, (int(x - xmin), int(y - ymin)), 4, colors_s[i].tolist(), -1) cv2.putText(image, '{0}'.format(i), (int(x - xmin), int(y - ymin)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, colors_s[i].tolist(), 1, cv2.LINE_AA) return image
def __init__(self, folder_in): self.scale = 1.0 self.draw_labels = False self.folder_in = folder_in self.textcolor = (0, 0, 0) self.memorized = [] self.display_primitives = 'L' self.w = 2 self.image_small = None self.temp_line = numpy.zeros(4, dtype=numpy.int) self.filenames = tools_IO.get_filenames(self.folder_in, '*.jpg,*.png') self.filenames = numpy.unique(self.filenames) self.dict_filenames = {x: i for i, x in enumerate(self.filenames)} self.current_frame_ID = 0 self.image_to_display = None self.current_markup_ID = 0 self.last_insert_size = 1 self.Lines = None self.Boxes = None if self.display_primitives == 'L': self.Lines = Lines() self.Lines.read_from_file(self.folder_in + 'lines.txt', self.dict_filenames) self.class_names_lines = self.init_class_names_lines() self.colors_lines = tools_draw_numpy.get_colors(len( self.class_names_lines), shuffle=True) if self.display_primitives == 'B': self.Boxes = Players(self.folder_in + 'boxes.txt', self.dict_filenames) self.class_names_boxes = self.init_class_names_boxes() self.colors_boxes = tools_draw_numpy.get_colors( len(self.class_names_boxes)) self.refresh_image() return
def plot_squarify(self,weights,labels,filename_out=None): colors2 = (tools_draw_numpy.get_colors(1 + len(labels), colormap='tab10', alpha_blend=0.25,shuffle=True) / 255)[1:] colors2 = numpy.hstack((colors2, numpy.full((len(labels), 1), 1))) fig = plt.figure(figsize=(12, 4)) fig = self.turn_light_mode(fig) squarify.plot(sizes=weights, label=labels, color=colors2,ax=plt.gca()) plt.axis('off') plt.tight_layout() if filename_out is not None: plt.savefig(self.folder_out + filename_out) return
def ex_bars(): W = 640 N = 32 image = numpy.full((N,W,3), 0, dtype=numpy.uint8) colors = tools_draw_numpy.get_colors(N, colormap='gray') for n in range(N - 1): image[-50:,int(n * W / (N - 1)):int((n + 1) * W / (N - 1))] = colors[n] tools_IO.remove_files(folder_out) image = tools_image.desaturate_2d(image) for cm_name in cmap_names: res = tools_image.hitmap2d_to_colormap(image, plt.get_cmap(cm_name)) cv2.imwrite(folder_out + '%s.png' % cm_name, res) return
def ex_pie_plot(df,idx_weights,idx_labels): col_label = df.columns.to_numpy()[idx_labels] col_weights = df.columns.to_numpy()[idx_weights] df_agg = df.groupby(col_label).sum() weights = df_agg.loc[:, col_weights].to_numpy() labels = df_agg.index.to_numpy() colors = (tools_draw_numpy.get_colors(1 + len(labels), colormap='tab20', alpha_blend=0.25, shuffle=True) / 255)[1:] colors = numpy.hstack((colors, numpy.full((len(labels), 1), 1))) idx = numpy.argsort(-weights) fig= plt.figure() ax = fig.gca() ax.pie(weights[idx], labels=labels[idx], autopct='%1.1f%%',shadow=False, startangle=90,counterclock=False,colors=colors, pctdistance=0.5) plt.savefig(folder_out + 'pie.png') return
def ex_circles(): W,H = 800,600 N =10 image = numpy.full((H, W, 3), 0, dtype=numpy.uint8) colors = tools_draw_numpy.get_colors(N,colormap='gray') for n in range(N): r = int(numpy.random.rand()*W) c = int(numpy.random.rand()*H) rad = int(H/10 + numpy.random.rand()*H/3) image = tools_draw_numpy.draw_circle(image, r, c, rad, colors[n]) for n in range(N-1): image[int(n*H/(N-1)):int((n+1)*H/(N-1)),-50:]=colors[n] tools_IO.remove_files(folder_out) image = tools_image.desaturate_2d(image) for cm_name in cmap_names: res = tools_image.hitmap2d_to_colormap(image,plt.get_cmap(cm_name)) cv2.imwrite(folder_out + 'res_%s.png'%cm_name,res) return
def keep_double_segments(self, segments, line_upper_bound, base_name=None, do_debug=False): tol_max = 12 tol_min = 2 lines = [] for i, segment in enumerate(segments): if line_upper_bound is None or tools_render_CV.is_point_above_line( segment[0], line_upper_bound): lines.append(self.interpolate_segment_by_line((segment))) else: lines.append((numpy.nan, numpy.nan, numpy.nan, numpy.nan)) lines = numpy.array(lines) has_pair = numpy.zeros(len(lines)) for l1 in range(len(lines) - 1): line1 = lines[l1] segment1 = segments[l1] if numpy.any(numpy.isnan(line1)): continue for l2 in range(l1 + 1, len(lines)): if l1 == 42 and l2 == 50: yy = 0 line2 = lines[l2] segment2 = segments[l2] if numpy.any(numpy.isnan(line2)): continue if self.are_same_segments(segment1, segment2): continue if not self.has_common_range(segment1, segment2, line1, line2): continue d1 = tools_render_CV.distance_point_to_line(line1, line2[:2]) d2 = tools_render_CV.distance_point_to_line(line1, line2[2:]) d3 = tools_render_CV.distance_point_to_line(line2, line1[:2]) d4 = tools_render_CV.distance_point_to_line(line2, line1[2:]) if numpy.any(numpy.isnan((d1, d2, d3, d4))): continue if (d1 > tol_max or d2 > tol_max): continue if (d3 > tol_max or d4 > tol_max): continue if d1 < tol_min and d2 < tol_min and d3 < tol_min and d4 < tol_min: continue len2 = numpy.linalg.norm(line2[:2] - line2[2:]) len1 = numpy.linalg.norm(line1[:2] - line1[2:]) d12 = max(numpy.linalg.norm(line1[:2] - line2[2:]), numpy.linalg.norm(line1[2:] - line2[2:])) d21 = max(numpy.linalg.norm(line1[:2] - line2[2:]), numpy.linalg.norm(line1[2:] - line2[2:])) if (d12 > len2 + 5) and (d21 > len1 + 5): continue p1, p2, d = tools_render_CV.distance_between_lines( line1, line2, clampAll=True) if d > tol_max: continue has_pair[l1] = 1 has_pair[l2] = 1 idx_good = (has_pair > 0) segments = numpy.array(segments, dtype=object) if do_debug: empty = numpy.full((self.H, self.W, 3), 32, dtype=numpy.uint8) colors_all = tools_draw_numpy.get_colors(len(segments), shuffle=True) image_segm = tools_draw_numpy.draw_segments(empty, segments, colors_all, w=1, put_text=True) cv2.imwrite(self.folder_out + base_name + '_1_segm.png', image_segm) #for i,segment in enumerate(segments): # image_segm = tools_draw_numpy.draw_segments(empty, [segment], colors_all[i].tolist(), w=1) # cv2.imwrite(self.folder_out + base_name + '_segm_%03d.png'%i, image_segm) image_segm = numpy.full((self.H, self.W, 3), 32, dtype=numpy.uint8) image_segm = tools_draw_numpy.draw_segments(image_segm, segments, (90, 90, 90), w=1) segments[~idx_good] = None image_segm = tools_draw_numpy.draw_segments(image_segm, segments, (0, 0, 255), w=1) #image_segm = utils_draw.draw_lines(image_segm, [line_upper_bound], (0, 0, 255), w=1) cv2.imwrite(self.folder_out + base_name + '_2_fltr.png', image_segm) return segments[idx_good] #, lines[idx_good]
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 export_boxes(folder_out, folder_images, folder_labels_GT, mat_proj, point_van_xy): ObjLoader = tools_wavefront.ObjLoader() tools_IO.remove_files(folder_out, create=True) local_filenames = get_filenames(folder_images, '*.png,*.jpg')[:10] for index, local_filename in enumerate(local_filenames): base_name = local_filename.split('/')[-1].split('.')[0] print(base_name) filename_image = folder_images + local_filename filename_label = folder_labels_GT + base_name + '.txt' filename_calib = folder_calib + base_name + '.txt' image = tools_image.desaturate(cv2.imread(filename_image)) H, W = image.shape[:2] target_BEV_W, target_BEV_H = 256, 256 h_ipersp = tools_render_CV.get_inverce_perspective_mat_v2( image, target_BEV_W, target_BEV_H, point_van_xy) image_BEV = cv2.warpPerspective(image, h_ipersp, (target_BEV_W, target_BEV_H), borderValue=(32, 32, 32)) image_BEV[:2, :2] = 255 cv2.imwrite(folder_out + base_name + '_BEV.png', image_BEV) ObjLoader.export_material(folder_out + base_name + '.mtl', (255, 255, 255), base_name + '_BEV.png') ObjLoader.export_mesh(folder_out + base_name + '_BEV.obj', numpy.array([[-1, -1, 0], [-1, +1, 0], [+1, +1, 0], [+1, -1, 0]]), idx_vertex=[[0, 1, 2], [2, 3, 0]], coord_texture=[[0, 0], [0, 1], [1, 1], [1, 0]], filename_material=base_name + '.mtl') records = tools_IO.load_mat(filename_label, delim=' ', dtype=numpy.str) records = filter_records(records) colors = tools_draw_numpy.get_colors(len(records), colormap='rainbow') for c in range(len(records)): color = colors[c] color_hex = '%02x%02x%02x' % (color[2], color[1], color[0]) record = records[c] record = numpy.insert(record, 1, '0') record = numpy.insert(record, 1, '0') #points_3d = get_cube_3D(record) #idx_vert = [[0,1,2],[0,2,7],[3,4,5],[3,5,6],[0,1,4],[0,4,5],[2,3,6],[2,6,7],[0,5,6],[0,6,7],[1,2,3],[1,3,4]] #ObjLoader.export_material(folder_out + color_hex + '.mtl',color[[2,1,0]]) #ObjLoader.export_mesh(folder_out + base_name + '_%03d.obj'%c, points_3d,idx_vertex=idx_vert,filename_material=color_hex + '.mtl') corners_3d = get_cube_3D(record) points_2d = project_2D(mat_proj, corners_3d) points_2d_BEV = project_2D_BEV(points_2d[[2, 3, 6, 7]], h_ipersp) #image_2d = tools_draw_numpy.draw_convex_hull(image_BEV, points_2d_BEV, color.tolist(), transperency=0.25) #cv2.imwrite(folder_out+ base_name + '_%03d.png'%c,image_2d) points_3d_BEV = numpy.zeros((8, 3), dtype=numpy.float32) points_3d_BEV[:4, :2] = points_2d_BEV / 128 - 1 points_3d_BEV[4:, :2] = points_2d_BEV / 128 - 1 points_3d_BEV[:4, 2] = -0.01 points_3d_BEV[4:, 2] = -0.05 idx_vert = [[0, 1, 2], [2, 3, 0], [4, 5, 6], [6, 7, 4], [0, 1, 5], [4, 5, 0], [2, 3, 6], [6, 7, 3], [0, 3, 4], [7, 4, 3], [1, 2, 5], [6, 2, 5]] ObjLoader.export_material(folder_out + color_hex + '.mtl', color[[2, 1, 0]]) ObjLoader.export_mesh(folder_out + base_name + '_%03d.obj' % c, points_3d_BEV, idx_vertex=idx_vert, filename_material=color_hex + '.mtl') obj_filenames = numpy.sort( get_filenames(folder_out, '%s_*.obj' % base_name)) with open(folder_out + base_name + '.obj', 'w') as g: for obj_filename in obj_filenames: with open(folder_out + obj_filename, 'r') as f: g.writelines('#-------------------------------------\n') for each in f.readlines(): g.writelines(each) os.remove(folder_out + obj_filename) return
def draw_boxes(folder_out, folder_images, folder_labels_GT, mat_proj, point_van_xy): draw_cuboids = True tools_IO.remove_files(folder_out, create=True) local_filenames = get_filenames(folder_images, '*.png,*.jpg')[:10] for index, local_filename in enumerate(local_filenames): base_name = local_filename.split('/')[-1].split('.')[0] print(base_name) filename_image = folder_images + local_filename filename_label = folder_labels_GT + base_name + '.txt' image = tools_image.desaturate(cv2.imread(filename_image)) H, W = image.shape[:2] target_BEV_W, target_BEV_H = int(H * 0.75), H if not os.path.exists(filename_label): continue records = tools_IO.load_mat(filename_label, delim=' ', dtype=numpy.str) records = filter_records(records) colors = tools_draw_numpy.get_colors(len(records), colormap='rainbow') image_2d = image.copy() h_ipersp = tools_render_CV.get_inverce_perspective_mat_v2( image, target_BEV_W, target_BEV_H, point_van_xy, 20, 2, 2) image_BEV = cv2.warpPerspective(image, h_ipersp, (target_BEV_W, target_BEV_H), borderValue=(32, 32, 32)) image_BEV = draw_grid(image_BEV, 20, 20, transp=0.9) for record, color in zip(records, colors): record = numpy.insert(record, 1, '0') record = numpy.insert(record, 1, '0') if draw_cuboids: points_2d = project_2D(mat_proj, get_cube_3D(record)) if (numpy.array(points_2d).min() < 0) or (numpy.array(points_2d).min() > W): continue lines_2d = numpy.array(points_cuboid_to_lines(points_2d)) points_2d_BEV = project_2D_BEV(points_2d[[2, 3, 6, 7]], h_ipersp) image_BEV = tools_draw_numpy.draw_contours( image_BEV, points_2d_BEV, color_fill=color.tolist(), color_outline=color.tolist(), transp_fill=0.25, transp_outline=0.75) else: points_2d = get_bbox(record) lines_2d = points_bbox_to_lines(points_2d) center_2d_BEV = default_2D_BEV(points_2d, h_ipersp)[0] image_BEV = tools_draw_numpy.draw_ellipse( image_BEV, (center_2d_BEV[0] - 10, center_2d_BEV[1] - 10, center_2d_BEV[0] + 10, center_2d_BEV[1] + 10), color.tolist(), transperency=0.75) #image_2d = tools_draw_numpy.draw_ellipse(image_2d,(center_2d_BEV[0]-10, center_2d_BEV[1]-10, center_2d_BEV[0]+10,center_2d_BEV[1]+10),color.tolist(),transperency=0.75) image_2d = tools_draw_numpy.draw_convex_hull(image_2d, points_2d, color.tolist(), transperency=0.75) image_2d = tools_draw_numpy.draw_lines(image_2d, lines_2d, color.tolist(), w=1) image_result = numpy.zeros((H, W + target_BEV_W, 3), dtype=numpy.uint8) image_result[:, :W] = image_2d image_result[:, W:] = image_BEV cv2.imwrite(folder_out + base_name + '.png', image_result) return
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