def get_point_of_infinity(field, points, delta=5, ransac_number=30, num_rnd_lines=20): pts = bc.Points() points_of_cross = collections.deque(maxlen=ransac_number) dist = collections.deque(maxlen=ransac_number) for i in range(len(points)): pts.add_point( points[i][0], points[i][0] + np.array([field.x[i], field.y[i]], dtype=np.float32)) pts.make_lines() for cycle in range(ransac_number): subset = pts.get_subset_of_rnd_lines(num_rnd_lines) pt = pts.point_of_crossing(subset) if not np.isnan(pt[0]): points_of_cross.append(pt) dist.append(pts.get_sum_dist(pt, subset)) if pts.get_number_of_inliers() <= num_rnd_lines: break if len(dist) == 0: return [np.NaN, np.NaN], False id_temp_point = list(dist).index(min(dist)) temp_point = points_of_cross[id_temp_point] # Marking outliers pts.check_for_lines_in_interesting_area(temp_point, delta) inliers = pts.get_indexes_of_inliers() pt = pts.point_of_crossing(inliers) # if wasn't found point of infinity (some error in numpy.linalg.lstsq) if np.isnan(pt[0]): return pt, False return pt, True
print('ERROR') cv2.imwrite(pic_name, out) continue #2 mod_pts = mn.modification_points(interesting_points) pts1, pts2 = pai_module.find_opt_flow_lk_with_points(img1, img2, mod_pts) if len(pts1) == 0: print('ERROR') cv2.imwrite(pic_name, out) continue print() #5 CLEANING # pts1_clean = collections.deque() # actual points (1 point in optical flaw) # pts2_clean = collections.deque() # actual points (2 point in optical flaw) points_for_pai = bc_module.Points() r_summ = [] last_value = -1 t0 = time.time() blob = cv2.dnn.blobFromImage(img1, scale, (416,416), (0,0,0), True, crop=False) net.setInput(blob) outs = net.forward(get_output_layers(net)) class_ids = [] confidences = [] boxes = [] conf_threshold = 0.5 nms_threshold = 0.4 for out_data in outs: for detection in out_data: scores = detection[5:] class_id = np.argmax(scores)
cos_t = ((x - x_0) * v_x + (y - y_0) * v_y) / (np.linalg.norm([x - x_0, y - y_0]) * np.linalg.norm([v_x, v_y])) return np.arccos(cos_t) if __name__ == '__main__': video = 'input/calibration_video.mp4' #video = 'input/test1.mp4' cam = cv2.VideoCapture(video) img1 = cam.read()[1] img2 = cam.read()[1] max_corners = 5000 N_FRAMES = 700 times = collections.deque(maxlen=N_FRAMES) t_points = bc.Points() # for finding point of infinity y, x, z = img1.shape img_dim = (y, x, z) cap = bc.Capture(video) x1, y1, x2, y2 = x // 5, 0.45 * y // 1, 4 * x // 5, 0.85 * y // 1 p1_rec = np.array([x1, y1]) p2_rec = np.array([x2, y2]) mask = [p1_rec, p2_rec] rows = 10 cols = 20 max_cos = np.cos(20 * np.pi / 180) LINES = 20 grid = get_grid(p1_rec, p2_rec, rows, cols) deepness = 10 intensity_threshould = 20 arrow_size_factor = 10
def find_OF_crossing_pt(img1, img2, num_cycles = 30, num_rnd_lines = 20, delta = 15, method = 'lk', trace = False, path = 'output/'): """ Ransac algorithm gets 2 frames and returns a point of infinity. :param img1: source first image, array of coordinates of points. :param img2: source second image, array of coordinates of points. :param num_cycles: number of cycles in ransac. :param num_rnd_lines: size of subset in ransac. :param delta: max distance for inliers in ransac. :param method: optical flow finding method. :param trace: boolean value trace, saving points in file. :param path: folder for tracing points. :return: point of infinity coordinates and algorithm status: np.array([[], []]), True\False """ frame1 = img1.copy() frame2 = img2.copy() cycle = 0 # iterator min_points = 50 #max length of deque below ??????????????????????????????????????????????????? dist = deque() # vector characteristic for all points points_of_cross = deque() pts1, pts2 = np.array([]), np.array([]) # Temporary arrays of points, that containing optical flow qL = 0.15 N = 500 itt = 0 points = bc.Points() # Trying to generate points and build optical flow while len(pts1) < min_points: if itt > 5: # limit of iteration return [np.NaN, np.NaN], False pts1, pts2 = find_opt_flow(frame1, frame2, np.power(0.5, itt) * qL, N * np.power(2, itt), method=method) itt += 1 points.add_points(pts1, pts2) points.make_lines() # Generating img with all lines and current trace folder. if trace: out = img2.copy() inliers = points.get_indexes_of_inliers() for id in inliers: point = points.get_point_by_id(id) k, b = point.get_line() x1, y1 = point.get_pt1() x2, y2 = point.get_pt2() out = draw.draw_line(out, k, b, color=draw.red) out = draw.draw_point(out, np.array([x1, y1]), color=draw.green) out = draw.draw_point(out, np.array([x2, y2]), color=draw.dark_green) cv2.imwrite(path + " all_lines.jpg", out) # Cycles of ransac while cycle < num_cycles: # Generating subset of lines subset = points.get_subset_of_rnd_lines(num_rnd_lines) # Draw current subset, if need if trace: out = img2.copy() color = draw.blue for s in subset: k, b = points.get_point_by_id(s).get_line() out = draw.draw_line(out, k, b, color=color) # Trying to find current point of cross pt = points.point_of_crossing(subset) if not np.isnan(pt[0]): points_of_cross.append(pt) dist.append(points.get_sum_dist(pt, subset)) if trace: out = draw.draw_point(out, pt, color=draw.red, thickness=1, radius=10) cv2.imwrite(path + str(cycle) + ".jpg", out) # if there are not so much lines, is sufficient 1 subset(all lines) if points.get_number_of_inliers() <= num_rnd_lines: break cycle = cycle + 1 # if was a some error if len(dist) == 0: return [np.NaN, np.NaN], False # Main point of cross id_temp_point = list(dist).index(min(dist)) temp_point = points_of_cross[id_temp_point] # Marking outliers points.check_for_lines_in_interesting_area(temp_point, delta) inliers = points.get_indexes_of_inliers() pt = points.point_of_crossing(inliers) # if wasn't found point of infinity (some error in numpy.linalg.lstsq) if np.isnan(pt[0]): return pt, False # Drawing inliers and point of infinity if trace: out = img2.copy() for id in inliers: k, b = points.get_point_by_id(id).get_line() out = draw.draw_line(out, k, b) out = draw.draw_point(out, pt, radius=10, thickness=1) cv2.imwrite(path + "result.jpg", out) out = img2.copy() for i in points.get_indexes_of_inliers(): point = points.get_point_by_id(i) out = draw.draw_point(out, pt, color=draw.blue, thickness=1, radius=2) cv2.imwrite(path + 'unit_vector.jpg', out) return pt, True
sa.append(speed_g[frame_itt]) Bm7_v = get_median(B_v) B_median7.append(Bm7_v) make_result(out, frame_itt, time_start_itt, tmp_map, times_g[frame_itt + delta_fr], (lon_g[frame_itt + delta_fr], lat_g[frame_itt + delta_fr]), speed_g[frame_itt + delta_fr], None, A_x, A_y, A_z, B, ofSpeed=B_v, medianSpeed=Bm7_v, err='All optical flow is outlier', debug=debug, debug_graph=debug_graph) continue try: t0 = time.time() A_x, A_y, A_z, B, F_x, F_y, F_z, F_f, F_o = mm_of_movement.build_model(pts1, pts2, pai_in_frame, L) # preliminary model residuals = mm_of_movement.get_residuals(A_x, A_y, A_z, B, F_x, F_y, F_z, F_f, F_o, pts1, pts2) # generation of residual array tt_model2.append(time_tressing(t0, '\t\tmodel2')) except: log(frame_itt, time_start_itt, times_g[frame_itt + delta_fr], (lon_g[frame_itt + delta_fr], lat_g[frame_itt + delta_fr]), speed_g[frame_itt + delta_fr], A_x, A_y, A_z, B, -27 * B, B_median7[len(B_median7) - 1], err='Mistake in matrix #2') t0 = time.time() pai_points = bc_module.Points() pai_points.add_points(pts1, pts2) PAI, check = pai_points.find_PAI() tt_pai.append(time_tressing(t0, '\t\tpai finding')) if debug and check: t0 = time.time() out = draw.draw_arrow(out, pai_in_frame, PAI, color=draw.purple) out = draw.draw_point(out, PAI, color=draw.purple, thickness=4) tt_draw_another[len(tt_draw_another) - 1] += time_tressing(t0, '\t\tdrawing pai') # res_avr = mm_of_movement.sum_of_residuals(residuals) # n_pts = len(residuals) B_v = B_factor * B while len(Ba) + 1 != len(Ta): Ba.append(None) Ba.append(B_v) Bm7_v = get_median(B_v)