def screen_record(): x, y, w, h = 0, 50, 1024, 768 meta = MetaData.from_screen(w - x, h - y) arrow = Arrow(meta.width) last_time = 0 with mss() as sct: # Part of the screen to capture monitor = {"top": y, "left": x, "width": w, "height": h} while True: frame = numpy.array(sct.grab(monitor)) try: processed_img = process_img(frame, meta) lines = find_lines(processed_img) p = separate_lines(lines) cv2.circle(frame, (int(p.x), int(p.y)), 2, [0, 255, 255], 10) arrow.add_point(int(p.x)) except: pass draw_arrow(frame, arrow, meta.width) cv2.imshow("OpenCV/Numpy normal", frame) fps = "fps: {}".format(1 / (time.time() - last_time)) last_time = time.time() print(fps) # Press "q" to quit if cv2.waitKey(50) & 0xFF == ord("q"): cv2.destroyAllWindows() break
def video_record(video): meta = MetaData.from_video(video) cap = cv2.VideoCapture(video) arrow = Arrow(meta.width) cpt_frame = 0 while cap.isOpened(): ret, frame = cap.read() cpt_frame += 1 if ret: try: processed_img = process_img(frame, meta) lines = find_lines(processed_img) l1, l2 = separate_lines(lines) p = intersect_droit(l1, l2) arrow.add_point(int(p.x)) processed_img = cv2.cvtColor(processed_img, cv2.COLOR_GRAY2BGR) cv2.imshow('window', processed_img) draw_infos(frame, p, l1, l2) draw_arrow(frame, arrow, meta.width) cv2.imshow('window', frame) except: pass # draw_arrow(frame, arrow, meta.width) # cv2.imshow('window', frame) if cv2.waitKey(25) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()
def make_result(img, nfr, time_start_itt, Map, tGPS, loc=None, gpsSpeed=None, residuals=None, A_x=None, A_y=None, A_z=None, B=None, ofSpeed=None, medianSpeed=None, pai=None, err=None, time_per_frame=[], debug=True, debug_graph=True): log(nfr, time_start_itt, tGPS, loc, gpsSpeed, A_x, A_y, A_z, B, ofSpeed, medianSpeed, pai, err) if debug_graph: t0 = time.time() while len(time_per_frame) + 1 != len(Ta): time_per_frame.append(None) time_per_frame.append(time.time() - time_start_itt) f, a = plt.subplots() a.set_title('Time per frame') a.plot(Ta, time_per_frame, 'ro', color='red', markersize=3, label='time') f.legend() f.savefig('output/Time_per_frame.png') tt_draw_graph.append(time_tressing(t0, '\t\tdrawing graph')) # f.clf() if debug: t0 = time.time() lt = 6 out = img.copy() Width = img.shape[1]# Height = img.shape[0]# fs = Height / Width pic_name = 'output/{:05d}.png'.format(nfr) if residuals is not None: for r in residuals: pt1 = r[1] pt2 = r[2] pt3 = pt1 + r[3] out = draw.draw_arrow(out, pt1, pt2) out = draw.draw_arrow(out, pt1, pt3, color=draw.cyan) tt_draw_of.append(time_tressing(t0, '\t\tof drawing')) t0 = time.time() a = Map.shape[1] dw = int(0.05 * Width) dh = int(0.05 * Height) out = cv2.circle(out, (int(Width - dw - a / 2 - 3), int(dh + a / 2)) , 3 + int(np.sqrt(2) * a / 2), COLOR_OF_MAP_BACKGRAUND, -1) out[dh:dh + a, Width - dw - a:Width - dw] = Map line1, line2, line3 = '???', '???', '???' if ofSpeed is not None: line1 = '{:+02.03f}'.format(ofSpeed) if medianSpeed is not None: line2 = '{:+02.03f}'.format(medianSpeed) if gpsSpeed is not None: line3 = '{:+02.03f}'.format(gpsSpeed) out = draw.draw_text(out, (2 * Width // 3, Height - fs * 200), 'Speed by ALG = ' + line1 + ' km/h', font_scale = fs, color = draw.white, line_type=lt) out = draw.draw_text(out, (2 * Width // 3, Height - fs * 200), 'Speed by ALG = ' + line1 + ' km/h', font_scale = fs, color = draw.blue) out = draw.draw_text(out, (2 * Width // 3, Height - fs * 150), 'Speed by AM7 = ' + line2 + ' km/h', font_scale = fs, color = draw.white, line_type=lt) out = draw.draw_text(out, (2 * Width // 3, Height - fs * 150), 'Speed by AM7 = ' + line2 + ' km/h', font_scale = fs, color = draw.dark_red) out = draw.draw_text(out, (2 * Width // 3, Height - fs * 100), 'Speed by GPS = ' + line3 + ' km/h', font_scale = fs, color = draw.white, line_type=lt) out = draw.draw_text(out, (2 * Width // 3, Height - fs * 100), 'Speed by GPS = ' + line3 + ' km/h', font_scale = fs, color = draw.dark_green) cv2.imwrite(pic_name, out) tt_draw_interface.append(time_tressing(t0, '\t\tanother drawing'))
def draw(self, frame, mask, points, output, scale=10, pref=''): out = draw.draw_rectangle(frame, mask, draw.cyan) avr_eps = self.get_avr_eps() for i in range(N): t0 = time.time() color = draw.green if self.eps[i] < avr_eps: color = draw.red if self.eps[i] == -1: color = draw.cyan pt1 = points[i][0] pt2 = pt1 + scale * np.array([self.x[i], self.y[i]]) out = draw.draw_point(out, pt1, 1, draw.blue) out = draw.draw_point(out, pt2, 1, draw.blue) out = draw.draw_arrow(out, pt1, pt2, color) tk = time.time() times.append(tk - t0) T = (sum(times) / len(times)) * (N - i) t_min = np.int(T // 60) t_sec = T % 60 print(pref + 'drawing: {} / {}\t {} min {:.2f} sec'.format( i, N, t_min, t_sec)) cv2.imwrite(output, out)
def do(): y_ = 800 x_ = 800 video_adr = 'input/calibration_video.mp4' cam = cv2.VideoCapture(video_adr) _, img = cam.read() img2 = img y, x, z = img.shape m = np.array([x // 2 - 50, y * 0.4], np.float32) L = 10 LINES = 250 intensity_threshould = 25 arrow_size_factor = 1000 N = 5 for itt in range(4): img1 = img2 _, img2 = cam.read() points, count = mn.get_points(550, 30 * np.pi / 180, LINES, m) pts_filtered = mn.intensity_threshould_filter_of_points( img1, points, intensity_threshould) mod_pts = mn.modification_points(pts_filtered) # pts1, pts2 = pai.find_opt_flow_lk_with_points(img1, img2, mod_pts) coords = [] data = [] with open('out/rotations/frame{}.txt'.format(itt)) as file: for line in file: dx, dy, summ, count = line.split('|') coords.append([int(dx), int(dy)]) data.append(np.float32(summ) / np.float32(count)) # sys.exit('') min_dx = min(coords)[0] min_dy = min(coords)[1] max_dx = max(coords)[0] max_dy = max(coords)[1] min_val_coord = coords[data.index(min(data))] cols = max_dx - min_dx + 1 rows = max_dy - min_dy + 1 frame = draw_spectrum(x_, y_, rows, cols, data, coords) m_new = m + np.array(min_val_coord, np.float32) dm = (m_new - m) / N cv2.imwrite('out/rotations/{}.png'.format(itt), frame) out = draw.draw_point(img2, m, color=draw.dark_green, thickness=5, radius=3) points, count = mn.get_points(550, 30 * np.pi / 180, LINES, m) pts_filtered = mn.intensity_threshould_filter_of_points( img1, points, intensity_threshould) mod_pts = mn.modification_points(pts_filtered) pts1, pts2 = pai.find_opt_flow_lk_with_points(img1, img2, mod_pts) for i in range(N): print(' step: ', i, N) m_ = m + i * dm F_x = collections.deque() F_y = collections.deque() F_z = collections.deque() F_f = collections.deque() F_o = collections.deque() for k in range(len(pts1)): if k % (len(pts1) // 10) == 0: print(' arrows: ', k, len(pts1)) pt1 = pts1[k] pt2 = pts2[k] u = pt1[0] - m_[0] v = pt1[1] - m_[1] du_x, dv_x = vector_flow_rotation_x(u, v, L) du_y, dv_y = vector_flow_rotation_y(u, v, L) du_z, dv_z = vector_flow_rotation_z(u, v) du_f, dv_f = vector_flow_forward(u, v, L) F_x.append([du_x, dv_x]) F_y.append([du_y, dv_y]) F_z.append([du_z, dv_z]) F_f.append([du_f, dv_f]) F_o.append(pt2 - pt1) out = draw.draw_arrow(out, pt1, pt2) M = np.zeros((4, 4), np.float32) V = np.zeros(4, np.float32) for k in range(len(F_f)): M[0][0] += F_x[k][0]**2 + F_x[k][1]**2 M[0][1] += F_x[k][0] * F_y[k][0] + F_x[k][1] * F_y[k][1] M[0][2] += F_x[k][0] * F_z[k][0] + F_x[k][1] * F_z[k][1] M[0][3] += F_x[k][0] * F_f[k][0] + F_x[k][1] * F_f[k][1] M[1][1] += F_y[k][0]**2 + F_y[k][1]**2 M[1][2] += F_y[k][0] * F_z[k][0] + F_y[k][1] * F_z[k][1] M[1][3] += F_y[k][0] * F_f[k][0] + F_y[k][1] * F_f[k][1] M[2][2] += F_z[k][0]**2 + F_z[k][1]**2 M[2][3] += F_z[k][0] * F_f[k][0] + F_z[k][1] * F_f[k][1] M[3][3] += F_f[k][0]**2 + F_f[k][1]**2 V[0] += F_o[k][0] * F_x[k][0] + F_o[k][1] * F_x[k][1] V[1] += F_o[k][0] * F_y[k][0] + F_o[k][1] * F_y[k][1] V[2] += F_o[k][0] * F_z[k][0] + F_o[k][1] * F_z[k][1] V[3] += F_o[k][0] * F_f[k][0] + F_o[k][1] * F_f[k][1] for k in range(4): for l in range(k): M[k][l] = M[l][k] A_x, A_y, A_z, B = np.linalg.solve(M, V) print(A_x, A_y, A_z, B) res = draw.draw_point(img2, m_, color=draw.dark_green, radius=5, thickness=3) for k in range(len(pts1)): if k % (len(pts1) // 10) == 0: print(' arrows: ', k, len(pts1)) pt1 = pts1[k] vvx = A_x * np.array(F_x[k], np.float32) vvy = A_y * np.array(F_y[k], np.float32) vvz = A_z * np.array(F_z[k], np.float32) vvf = B * np.array(F_f[k], np.float32) dpt2 = vvx + vvy + vvz + vvf out = draw.draw_arrow(out, pt1, pt1 + dpt2, color=draw.cyan) res = draw.draw_arrow(res, pt1, pt1 + (pts2[k] - pts1[k]) - dpt2, color=draw.red) cv2.imwrite('out/rotations/{}_{}.png'.format(itt, i), out) cv2.imwrite('out/rotations/{}_{}_r.png'.format(itt, i), res) m = m_new print(itt, 30) print('DONE')
out = draw.draw_mahalanobis_ellipse(out, r, m, K, color=draw.blue) out = draw.draw_mahalanobis_ellipse(out, 3 * r, m, K, color=draw.red) out = draw.draw_mahalanobis_ellipse( out, np.sqrt(sf.get_mahalanobis_distance_sq(pt, m, K_inv)), m, K, color=draw.gold) if status[i] == 1: out = draw.draw_arrow(out, m, m - np.array([0, 100]), color=draw.red, thickness=3) else: out = draw.draw_arrow(out, m, m + np.array([100, 0]), color=draw.red, thickness=3) out = draw.draw_arrow(out, m, m - np.array([100, 0]), color=draw.red, thickness=3) text = dict(text='r(alpha) = ' + str(3 * r),
continue t0 = time.time() th = 2 if eps[i] > avr_eps: color = draw.red elif eps[i] == -1: color = draw.purple else: color = draw.green pt1 = points1[i][0] + mask[0] dpt = np.array([norm_x[i], norm_y[i]]) pt2 = pt1 + dpt frame1 = draw.draw_point(frame1, pt1, radius=1, color=draw.blue) frame1 = draw.draw_point(frame1, pt2, radius=1, color=draw.blue) frame1 = draw.draw_arrow(frame1, pt2, pt1, color, thickness=th) #frame2 = draw.draw_text(frame2, pt1, text='({}|{})'.format(i // k, i % k), font_scale=0.25, line_type=1) tk = time.time() times.append(tk - t0) if i % (N // 10) == 0: T = (sum(times) / len(times)) * (N - i) t_min = np.int(T // 60) t_sec = T % 60 print(' drawing: {} | {} min {} sec'.format(i, t_min, t_sec)) cv2.imwrite('norm.jpg', frame1) #for i in range(N): #dmf.make(video, m,k, 700, 5, out_data=source_data, out_pic=out_img) ex, ey = 0, 0 count = 0 for i in range(N):
t0 = time.time() residuals.sort(key=lambda r: np.linalg.norm(r[2] - r[1] - r[3]) / np.linalg.norm(r[3])) ind_max = len(residuals) for i in range(len(residuals)): r = residuals[i] if np.linalg.norm(r[2] - r[1] - r[3]) / np.linalg.norm(r[3]) > OUTLIER_THRESHOLD: ind_max = i break if debug: rr = residuals[ind_max:] # out = img2.copy() for r in rr: pt1 = r[1] pt2 = r[2] pt3 = pt1 + r[3] out = draw.draw_arrow(out, pt1, pt2, color=draw.red) out = draw.draw_arrow(out, pt1, pt3, color=draw.gold) # cv2.imwrite('output/red{}.png'.format(frame_itt), out) residuals = residuals[:ind_max] pts1 = [r[1] for r in residuals] pts2 = [r[2] for r in residuals] tt_of_filtering.append(time_tressing(t0, '\t\tof filtering')) if len(pts1) == 0: # print('All lines is outliers!!!') B_v = B_factor * B T_v = frame_itt / 25 # - 120 Ba.append(B_v) Ta.append(T_v) sa.append(speed_g[frame_itt]) Bm7_v = get_median(B_v) B_median7.append(Bm7_v)
#4 residuals = 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 B_values = [B] out = img1.copy() droped_out = [] points = bc_module.Points() for r in residuals: pt1 = r[1] pt2 = r[2] point = bc_module.Point(pt1, pt2) # pt1 + r[3] point.make_line() k, b = point.get_line() pt3 = pt1 + r[3] out = draw.draw_line(out, k, b, color = draw.gold) out = draw.draw_arrow(out, pt1, pt2) out = draw.draw_arrow(out, pt1, pt3, color=draw.cyan) points.add_point(pt1, pt2) out = draw.draw_text(out, (2 * Width//3, 30), 'A_x = {:02.05f}'.format(A_x), font_scale = fs, color = draw.blue) out = draw.draw_text(out, (2 * Width//3, 60), 'A_y = {:02.05f}'.format(A_y), font_scale = fs, color = draw.blue) out = draw.draw_text(out, (2 * Width//3, 90), 'A_z = {:02.05f}'.format(A_z), font_scale = fs, color = draw.blue) out = draw.draw_text(out, (2 * Width//3, 120), ' B = {:02.05f}'.format(B), font_scale = fs, color = draw.blue) PAI, _ = points.find_PAI() if _: out = draw.draw_arrow(out, pai_in_frame, PAI, color=draw.purple) out = draw.draw_point(out, PAI, color=draw.purple, thickness=3) cv2.imwrite('output/{}_A.png'.format(frame_itt), out) out = img1.copy()
def action(method): video = 'test1.mp4' cam = cv2.VideoCapture(video) lengh = np.int(cam.get(cv2.CAP_PROP_FRAME_COUNT)) print(lengh) _, frame1 = cam.read() y, x, z = frame1.shape x1, y1, x2, y2 = x // 3, 3*y//5, 2*x//3, 0.94 * y // 1 p1 = np.array([x1, y1]) p2 = np.array([x2, y2]) mask = [p1, p2] times = deque(maxlen=lengh) #times = deque(maxlen=lengh) for itt in range(lengh): t0 = time.time() _, frame2 = cam.read() if not _: break frame2 = draw.draw_rectangle(frame2, mask, color=draw.cyan) area1 = sf.get_box(frame1, mask) area2 = sf.get_box(frame2, mask) y_, x_, z_ = area1.shape print(y_, x_) points1 = np.zeros((100, 2), dtype = np.float) yy0 = 0.02 * y_ // 1 dy = (y_ - 2 * yy0) // 10 xx0 = 0.02 * x_ // 1 dx = (x_ - 2*xx0) // 10 for i in range(10): for j in range(10): points1[i][0] = xx0 + i * dx points1[i][1] = yy0 + j * dy points1, points2 = pai.find_opt_flow(area1, area2, method=method) N = len(points1) N_ = len(points2) print(' N is {} and {}'.format(N, N_)) out = frame2.copy() norms = deque(maxlen=N) for i in range(N): norms.append(np.linalg.norm(points1[i] - points2[i])) mid_norm = sum(norms) / N for i in range(N): p1 = points1[i] + mask[0] p2 = points2[i] + mask[0] if np.linalg.norm(p1 - p2) < mid_norm: out = draw.draw_point(out, p1, radius=3) out = draw.draw_point(out, p2, radius=3) out = draw.draw_arrow(out, p1, p2) out = draw.draw_text(img=out, pt=(3*x//4, 80), text='points: {}'.format(N),color=draw.blue, font_scale=1, line_type=2) out = draw.draw_text(img=out, pt=(0, 80), text='{}'.format(itt),color=draw.blue, font_scale=1, line_type=2) # out = draw.draw_text(out, (3*x//4, 80), text_properties) # small = cv2.resize(out, (0,0), fx=0.7, fy=0.7) # cv2.imshow('frame', small) cv2.imwrite('out/{}.jpg'.format(itt), out) # #cv2.imshow('area', area) # # k = cv2.waitKey(20) frame1 = frame2 tk = time.time() times.append(tk - t0) T = sum(times) / len(times) T = T * (lengh - itt) t_min = int(T // 60) t_sec = T % 60 print('{} min {:.02f} sec'.format(t_min, t_sec)) cv2.destroyAllWindows()
pts1, pts2 = pai.find_opt_flow_lk_with_points(img1, img2, mod_pts) A_x, A_y, A_z, B, F_x, F_y, F_z, F_f, F_o = build_model( pts1, pts2, pai_in_frame, L) # preliminary model residuals = 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 # drawing if debug: for r_i in range(min(len_max, len(residuals))): r = residuals[r_i] pt1_f = r[1] pt2_f = r[2] pt1_m = pt1_f pt2_m = r[3] + pt1_f out = draw.draw_arrow(out, pt1_f, pt2_f, color=draw.red) out = draw.draw_arrow(out, pt1_m, pt2_m, color=draw.cyan) pts1_clean = collections.deque( ) # actual points (1 point in optical flaw) pts2_clean = collections.deque( ) # actual points (2 point in optical flaw) for r_i in range(len(residuals)): r = residuals[r_i] pt1 = r[1] # coordinate of first point of optical flaw pt2_opt = r[2] # coordinate of second point of optical flaw opt_vector = r[2] - r[1] # coordinate of vector of optical flaw model_vector = r[3] # coordinate of model vector rsdl_vector = r[4] # coordinate of residual vector # criterion of interesting points
def get_field(cam, points, n_frames, min_num_points=5, algorithm=Algorithm.Modificated, trace=True, pref=''): N = len(points) sumX = np.zeros(N, dtype=np.float32) sumY = np.zeros(N, dtype=np.float32) sumX2 = np.zeros(N, dtype=np.float32) sumY2 = np.zeros(N, dtype=np.float32) Num = np.zeros(N, dtype=np.int) avr_x = np.zeros(N, dtype=np.float32) avr_y = np.zeros(N, dtype=np.float32) std_x2 = np.zeros(N, dtype=np.float32) std_y2 = np.zeros(N, dtype=np.float32) eps = np.zeros(N, dtype=np.float32) _, frame1 = cam.read() _, frame2 = cam.read() # frame stack frames = collections.deque(maxlen=n_frames) frames.append(frame1) frames.append(frame2) # times = collections.deque() for itt in range(n_frames): st, points_new = -1, -1 t0 = time.time() if algorithm == Algorithm.Simple: print('Simple') points_new, st = get_optical_flow_field_lk(frame1, frame2, points) elif algorithm == Algorithm.Modificated: print('Modificated') points_new, st = get_mod_optical_flow_field_lk( frame1, frame2, points) tk = time.time() times.append(tk - t0) for i in range(N): if st[i] == 1: addX = points_new[i][0][0] - points[i][0][0] addY = points_new[i][0][1] - points[i][0][1] Num[i] += 1 sumX[i] += addX sumY[i] += addY sumX2[i] += addX**2 sumY2[i] += addY**2 frame1 = frame2 _, frame2 = cam.read() # frames.append(frame2) # if not _: sys.exit('Video is ended!') if trace: T = (sum(times) / len(times)) * (n_frames - itt) t_sec = T % 60 t_hou = np.int(T // 60 // 60) t_min = np.int(T % 3600 // 60) print(pref + 'optical flow: {} / {}\t times: {} h {} min {} sec'.format( itt, n_frames, t_hou, t_min, t_sec)) for itt in range(N): if Num[itt] < min_num_points: eps[itt] = -1 else: avr_x[itt] = sumX[itt] / Num[itt] avr_y[itt] = sumY[itt] / Num[itt] std_x2[itt] = sumX2[itt] / Num[itt] - avr_x[itt]**2 std_y2[itt] = sumY2[itt] / Num[itt] - avr_y[itt]**2 eps[itt] = np.sqrt(std_x2[itt] + std_y2[itt]) if trace: print(pref + 'calculating (stage I): {} / {}'.format(itt, N)) avr_eps = sum(eps) / len(eps) # count = 0 ind = collections.deque(maxlen=N) m = np.int(np.sqrt(len(points))) for i in range(len(eps)): r, c = id_to_ij(i, m) if eps[i] > avr_eps and r != 0 and c != 0 and r != m and c != m and i < len( points) - m: # count += 1 ind.append(i) print(ind) pt1 = np.zeros((len(ind), 2), np.float32) pt2 = np.zeros((len(ind), 2), np.float32) vector_x = np.zeros(len(ind), np.float32) vector_y = np.zeros(len(ind), np.float32) vector_c = np.zeros(len(ind), np.float32) for i in range(len(ind)): pt1[i][0] = points[ind[i] - m - 1][0][0] pt1[i][1] = points[ind[i] - m - 1][0][1] pt2[i][0] = points[ind[i] + m + 1][0][0] pt2[i][1] = points[ind[i] + m + 1][0][1] print('stage II') for itt in range(1, n_frames): print('frame: {} / {}'.format(itt, n_frames)) for i in range(len(ind)): print(' area: {} / {}'.format(i, len(ind))) area1 = frames[itt - 1][np.int(pt1[i][1]):np.int(pt2[i][1]), np.int(pt1[i][0]):np.int(pt2[i][0])] area2 = frames[itt][np.int(pt1[i][1]):np.int(pt2[i][1]), np.int(pt1[i][0]):np.int(pt2[i][0])] pts1, pts2 = pai.find_opt_flow(area1, area2, method='lk', prop=0.3) print(type(pts1)) if type(pts1) is type(None): pts1 = np.array([-1, -1], dtype=np.float32) pts2 = np.array([-1, -1], dtype=np.float32) vectors = pts2 - pts1 out = area1.copy() for j in range(len(pts1)): out = draw.draw_arrow(out, pts1[j], pts2[j]) cv2.imwrite('out/{}_{}.jpg'.format(itt, i), out) for _i in range(len(vectors)): vector_x[i] += vectors[_i][0] vector_y[i] += vectors[_i][1] vector_c[i] += 1 vector_x[i] /= vector_c[i] vector_y[i] /= vector_c[i] for i in range(len(ind)): avr_x[ind[i]] = vector_x[i] avr_y[ind[i]] = vector_y[i] # print(ind) # print(len(ind)) # print(avr_eps) # print(n_frames) # print(len(frames)) return Field(avr_x, avr_y, eps)
def draw(self, screen): draw_line(screen, self.p1, self.p2, self.color) draw_arrow(screen, (self.p1 + self.p2) * 0.5, (self.p1 + self.p2) * 0.5 + self.normal() * 10)
# print(angle) if 180 - angle < 20: if pts2[i][1] < m[1]: continue v = pts1[i] - pts2[i] i_, j_ = get_ij_in_grid_by_pt(pts1[i], grid) if i_ == -1 or j_ == -1: continue count += 1 good_points[i] = True t_points.add_point(pts1[i], pts2[i]) print(' {} arrows / {} points'.format(count, len(points))) for i in range(len(pts1)): if good_points[i]: out = draw.draw_arrow(out, (pts1[i][0], pts1[i][1]), (pts2[i][0], pts2[i][1]), thickness=1) # out = draw.draw_point(out, pts1[i], radius=2, color=draw.black) # out = draw.draw_point(out, pts2[i], radius=2, color=draw.black) for l in range(rows): for k in range(cols): if is_in_side_of_rectangle(pts1[i], grid[l][k][0], grid[l][k][1]): middle_vectors[l][k][0] += pts2[i][0] - pts1[i][0] middle_vectors[l][k][1] += pts2[i][1] - pts1[i][1] middle_vectors[l][k][2] += 1 else: out = draw.draw_arrow(out, (pts1[i][0], pts1[i][1]), (pts2[i][0], pts2[i][1]), thickness=1, color=draw.red)
if len(angles_objects) != 0: al = min([ np.pi / 4 if a > np.pi / 2 or a < 0 else a for a in angles_objects ]) bt = max([3 * np.pi / 4 if a > np.pi else a for a in angles_objects]) if al < 0 or al > np.pi / 4: al = np.pi / 4 if bt < 3 * np.pi / 4: bt = 3 * np.pi / 4 print('alpha =', al * 180 / np.pi, 'betta =', bt * 180 / np.pi) if debug: out = draw.draw_arrow(out, pai_in_frame, pai_in_frame + (int(3 * lenght_of_axis * np.cos(al)), int(3 * lenght_of_axis * np.sin(al))), color=draw.red, thickness=2) out = draw.draw_arrow(out, pai_in_frame, pai_in_frame + (int(3 * lenght_of_axis * np.cos(bt)), int(3 * lenght_of_axis * np.sin(bt))), color=draw.blue, thickness=2) for an in angles_objects: out = draw.draw_arrow(out, pai_in_frame, pai_in_frame + (int(3 * lenght_of_axis * np.cos(an)),
def get_functia_by_field(field, points, frame): pt0, st = get_point_of_infinity(field, points, delta=5, ransac_number=30, num_rnd_lines=5) if not st: sys.exit('ERROR, i do not see a point of infinity.') f = draw.draw_point(frame, pt0, color=draw.cyan, thickness=3) alpha = np.zeros((2, 3), dtype=np.float32) betta = np.zeros((2, 3), dtype=np.float32) N = field.get_num_of_grid_points() X = collections.deque(maxlen=N) Y = collections.deque(maxlen=N) X_ = collections.deque(maxlen=N) Y_ = collections.deque(maxlen=N) R = collections.deque(maxlen=N) theta = collections.deque(maxlen=N) V = collections.deque(maxlen=N) ind = collections.deque(maxlen=N) for i in range(N): e1 = (pt0 - points[i][0]) / np.linalg.norm(pt0 - points[i][0]) e2 = np.array([field.x[i], field.y[i]]) / np.linalg.norm( np.array([field.x[i], field.y[i]])) a1 = np.arctan2(e1[0], e1[1]) * 180 / np.pi a2 = np.arctan2(e2[0], e2[1]) * 180 / np.pi da = np.abs(a1 - a2) # print(da) if field.eps[i] == -1 or da > 10: continue ind.append(i) X.append(np.linalg.norm(points[i][0] - pt0)) Y.append(np.sqrt(field.x[i]**2 + field.y[i]**2)) f = draw.draw_point(f, points[i][0], color=draw.gold, radius=3, thickness=3) r, t = euclidean_to_polar(points[i][0][0], points[i][0][1], pt0[0], pt0[1]) R.append(r) theta.append(t) V.append(np.sqrt(field.x[i]**2 + field.y[i]**2)) for i in range(1, 2 + 1): for j in range(0, 2 + 1): top_a = 0 top_b = 0 bot_a = 0 bot_b = 0 for k in range(len(ind)): top_a += V[k] * (R[k]**i) * np.cos(j * theta[k]) bot_a += (R[k]**(2 * i)) * (np.cos(j * theta[k])**2) top_b += V[k] * (R[k]**i) * np.sin(j * theta[k]) bot_b += (R[k]**(2 * i)) * (np.sin(j * theta[k])**2) alpha[i - 1][j] = top_a / bot_a betta[i - 1][j] = top_b / bot_b # new_points = np.zeros((N, 2), dtype=np.float32) par = {'alpha': alpha, 'betta': betta} # for i in range(len(ind)): # data_x = points[ind[i]][0][0] # data_y = points[ind[i]][0][1] # data_r, data_t = euclidean_to_polar(data_x, data_y, pt0[0], pt0[1]) # X_.append(np.linalg.norm(np.array([data_x, data_y]) - pt0)) # data = our_fun(par, data_r, data_t) # Y_.append(data) # e1 = (pt0 - points[ind[i]][0]) / np.linalg.norm(pt0 - points[ind[i]][0]) # f = draw.draw_arrow(f, points[ind[i]][0], points[ind[i]][0] + 10 * data * e1, color=draw.cyan) for i in range(N): data_x = points[i][0][0] data_y = points[i][0][1] data_r, data_t = euclidean_to_polar(data_x, data_y, pt0[0], pt0[1]) X_.append(np.linalg.norm(np.array([data_x, data_y]) - pt0)) data = our_fun(par, data_r, data_t) Y_.append(data) e1 = (pt0 - points[i][0]) / np.linalg.norm(pt0 - points[i][0]) f = draw.draw_arrow(f, points[i][0], points[i][0] + 10 * data * e1, color=draw.cyan) print(par) # f = draw.draw_arrow(f, points[i][0], 50 * e1 + points[i][0], thickness=1) # f = draw.draw_point(f, points[i][0], radius=4, color=draw.gold, thickness=4) # p = scipy.polyfit(X, Y, 2) # # x = np.zeros(N, dtype=np.float32) # y = np.zeros(N, dtype=np.float32) # for i in range(N): # pass ## y[i] = p2[0] * X[i] ** 2 + p2[1] * X[i] + p2[2] # plt.plot(X, Y, 'o') plt.plot(X, Y, 'o', X_, Y_, 'o') cv2.imwrite('out/task3/out.jpg', f) return par
cam = cv2.VideoCapture(0) is_read, img1 = cam.read() is_read, img2 = cam.read() img1 = cv2.resize(img1, (X, Y)) img2 = cv2.resize(img2, (X, Y)) while is_read: out = img2 pts1, pts2 = pai.find_opt_flow_lk_with_points(img1, img2, grid_m, 15, 15) norms = [np.linalg.norm(pts1[i] - pts2[i]) for i in range(len(pts1))] n_max = max(norms) n_min = min(norms) # gray = np.zeros((Y, X, 3), int) for i in range(N): out = draw.draw_point(out, grid[i], radius=2) for i in range(len(pts1)): out = draw.draw_arrow(out, pts1[i], pts2[i]) gray[pts1[i][1]][pts1[i][2]] != (norms[i] - n_min, norms[i] - n_min, norms[i] - n_min) cv2.imshow('cam', out) # cv2.imshow('gray', gray) if cv2.waitKey(1) & 0xFF == ord('q'): break img1 = img2 is_read, img2 = cam.read() img2 = cv2.resize(img2, (X, Y)) # When everything is done, release the capture cam.release() cv2.destroyAllWindows()
def make_txt(cam, dyn_points10, dyn_points25, dyn_points50, dyn_points75, dyn_points100): img = cam.read()[1] pt10_e = dyn_points10[0][3] pt25_e = dyn_points25[0][3] pt50_e = dyn_points50[0][3] pt75_e = dyn_points75[0][3] pt100_e = dyn_points100[0][3] for i in range(len(dyn_points10)): pt10_s = dyn_points10[i][3] pt25_s = dyn_points25[i][3] pt50_s = dyn_points50[i][3] pt75_s = dyn_points75[i][3] pt100_s = dyn_points100[i][3] img = draw.draw_arrow(img, pt10_e, pt10_s, color=draw.green, thickness=3) img = draw.draw_arrow(img, pt25_e, pt25_s, color=draw.blue, thickness=2) img = draw.draw_arrow(img, pt50_e, pt50_s, color=draw.red, thickness=1) img = draw.draw_arrow(img, pt75_e, pt75_s, color=draw.purple, thickness=1) img = draw.draw_arrow(img, pt100_e, pt100_s, color=draw.gold, thickness=1) pt10_e = pt10_s pt25_e = pt25_s pt50_e = pt50_s pt75_e = pt75_s pt100_e = pt100_s cv2.imwrite('output/_.png', img) with open('output/out.txt', 'w') as file: for i in range(len(dyn_points10)): line = 'frame {}:\n'.format(i) file.write(line) line = ' 10: [{:e}, {:e}, {:e}, {:e}], {:e}, {}, {}, {}\n'.format( dyn_points10[i][0][0], dyn_points10[i][0][1], dyn_points10[i][0][2], dyn_points10[i][0][3], dyn_points10[i][1], dyn_points10[i][2], dyn_points10[i][3][0], dyn_points10[i][3][1]) file.write(line) line = ' 25: [{:e}, {:e}, {:e}, {:e}], {:e}, {}, {}, {}\n'.format( dyn_points25[i][0][0], dyn_points25[i][0][1], dyn_points25[i][0][2], dyn_points25[i][0][3], dyn_points25[i][1], dyn_points25[i][2], dyn_points25[i][3][0], dyn_points25[i][3][1]) file.write(line) line = ' 50: [{:e}, {:e}, {:e}, {:e}], {:e}, {}, {}, {}\n'.format( dyn_points50[i][0][0], dyn_points50[i][0][1], dyn_points50[i][0][2], dyn_points50[i][0][3], dyn_points50[i][1], dyn_points50[i][2], dyn_points50[i][3][0], dyn_points50[i][3][1]) file.write(line) line = ' 75: [{:e}, {:e}, {:e}, {:e}], {:e}, {}, {}, {}\n'.format( dyn_points75[i][0][0], dyn_points75[i][0][1], dyn_points75[i][0][2], dyn_points75[i][0][3], dyn_points75[i][1], dyn_points75[i][2], dyn_points75[i][3][0], dyn_points75[i][3][1]) file.write(line) line = '100: [{:e}, {:e}, {:e}, {:e}], {:e}, {}, {}, {}\n'.format( dyn_points100[i][0][0], dyn_points100[i][0][1], dyn_points100[i][0][2], dyn_points100[i][0][3], dyn_points100[i][1], dyn_points100[i][2], dyn_points100[i][3][0], dyn_points100[i][3][1]) file.write(line) T10, T25, T50, T75, T100 = 0, 0, 0, 0, 0 for i in range(len(dyn_points10)): T10 += dyn_points10[i][2] T25 += dyn_points25[i][2] T50 += dyn_points50[i][2] T75 += dyn_points75[i][2] T100 += dyn_points100[i][2] with open('output/out.txt', 'a') as file: line = 'TOTAL:\n' file.write(line) line = ' 10: {:02.05f} sec\n'.format(T10) file.write(line) line = ' 25: {:02.05f} sec\n'.format(T25) file.write(line) line = ' 50: {:02.05f} sec\n'.format(T50) file.write(line) line = ' 75: {:02.05f} sec\n'.format(T75) file.write(line) line = '100: {:02.05f} sec\n'.format(T100) file.write(line)
r_i_sq = sf.get_mahalanobis_distance_sq(pt, m, K_inv) r_i = np.sqrt(r_i_sq) # drawing point, arrow and text text = dict(text='r = ' + str(r_i), font_scale=1, line_type=2, color=draw.blue) if r_i_sq > r_sq: text['color'] = draw.red out = draw.draw_text(out, (0, 50), **text) text.pop('text') text.pop('color') out = draw.draw_text(out, (0, 80), text='x = ' + str(pt[0]), color=draw.black, **text) out = draw.draw_text(out, (0, 110), text='y = ' + str(pt[1]), color=draw.black, **text) out = draw.draw_rectangle(out, mask, color=draw.cyan) out = draw.draw_arrow(out, m, pt) out = draw.draw_point(out, pt, radius=3, thickness=6, color=draw.red) # mahalanobis ellipses out = draw.draw_mahalanobis_ellipse(out, r, m, K, color=draw.blue, draw_center_pt = True, draw_axes = True, draw_extremum_pts = True) out = draw.draw_mahalanobis_ellipse(out, 3*r, m, K, color=draw.red, draw_center_pt = False, draw_axes = False, draw_extremum_pts = False) p = path + '{:05d}.jpg'.format(i) cv2.imwrite(p, out) f1 = f2
out = draw.draw_point(out, np.array([pt2[0], pt2[1]]), radius=1, color=draw.red) cv2.imwrite('output/pic/points_all.png', out) sys.exit() # out = draw.draw_point(out, m, color=draw.dark_green, thickness=3) good = np.zeros(len(pts1), bool) for i in range(len(pts1)): if is_directions_to(pts1[i], pts2[i], m, 30 * np.pi / 180): good[i] = True else: out = draw.draw_arrow(out, pts1[i], arrow_size_factor * (pts2[i] - pts1[i]) + pts1[i], color=draw.red) for i in range(len(pts1)): if good[i]: m_points.add_point(pts1[i], pts2[i]) out = draw.draw_arrow(out, pts1[i], arrow_size_factor * (pts2[i] - pts1[i]) + pts1[i], color=draw.green) m_points.mark_inlier_all() m_points.make_lines() pt_m, st = m_points.get_OF_by_max_dens() # for i in range(m_points.get_number_of_points()):
def make(video='C://Users//moshe.f//Desktop//TEST//calibration//23.mp4', m=100, k=100, nf=50, min_number_of_points=5, out_data=None, out_pic=None): cam = cv2.VideoCapture(video) lengh = np.int(cam.get(cv2.CAP_PROP_FRAME_COUNT)) print(lengh) lk_params = dict(winSize=(15, 15), maxLevel=2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.3)) _, frame1 = cam.read() y, x, z = frame1.shape # рабочая область x1, y1, x2, y2 = x // 5, y // 5, 4 * x // 5, 0.92 * y // 1 p1 = np.array([x1, y1]) p2 = np.array([x2, y2]) mask = [p1, p2] # ___________________ times = deque(maxlen=lengh) area1 = sf.get_box(frame1, mask) y_, x_, z_ = area1.shape # greed points points1 = np.zeros((m * k, 1, 2), dtype = np.float32) yy0 = 0.02 * y_ // 1 dy = (y_ - 2 * yy0) // m xx0 = 0.02 * x_ // 1 dx = (x_ - 2 * xx0) // k for i in range(m*k): points1[i][0][0] = xx0 + dx * (i % k) points1[i][0][1] = yy0 + dy * (i // k) #______________________ sumX = np.zeros(m*k, dtype=np.float32) sumY = np.zeros(m*k, dtype=np.float32) sumX2 = np.zeros(m*k, dtype=np.float32) sumY2 = np.zeros(m*k, dtype=np.float32) Num = np.zeros(m*k, dtype=np.int) avr_x = np.zeros(m*k, dtype=np.float32) avr_y = np.zeros(m*k, dtype=np.float32) std_x2 = np.zeros(m*k, dtype=np.float32) std_y2 = np.zeros(m*k, dtype=np.float32) eps = np.zeros(m*k, dtype=np.float32) avr_eps = 0 counter = 0 # data collection for itt in range(nf): t0 = time.time() _, frame2 = cam.read() area1 = sf.get_box(frame1, mask) area2 = sf.get_box(frame2, mask) points2, st, err = cv2.calcOpticalFlowPyrLK(cv2.cvtColor(area1, cv2.COLOR_BGR2GRAY), cv2.cvtColor(area2, cv2.COLOR_BGR2GRAY), points1, None, **lk_params) for i in range(m*k): if st[i] == 1: addX = points2[i][0][0] - points1[i][0][0] addY = points2[i][0][1] - points1[i][0][1] Num[i] += 1 sumX[i] += addX sumY[i] += addY sumX2[i] += addX ** 2 sumY2[i] += addY ** 2 frame1 = frame2 tk = time.time() times.append(tk - t0) if itt % (nf // 10) == 0: T = (sum(times) / len(times)) * (nf - itt) t_min = int(T // 60) t_sec = T % 60 print('{} | {} min {:.02f} sec'.format(itt, t_min, t_sec)) times.clear() # data analysise for i in range(m*k): t0 = time.time() if Num[i] < min_number_of_points: eps[i] = -1 else: avr_x[i] = sumX[i] / Num[i] avr_y[i] = sumY[i] / Num[i] std_x2[i] = sumX2[i] / Num[i] - avr_x[i] ** 2 std_y2[i] = sumY2[i] / Num[i] - avr_y[i] ** 2 eps[i] = np.sqrt(std_x2[i] + std_y2[i]) if np.isnan(eps[i]): sys.exit('Arg sqrt in eps is bad in step {}!!! arg = {}'.format(i, std_x2[i] + std_y2[i])) tk = time.time() times.append(tk - t0) if i % 10 == 0: T = (sum(times) / len(times)) * (m*k - i) t_min = np.int(T // 60) t_sec = T % 60 print('calculate {} | {} min {} sec'.format(i, t_min, t_sec)) times.clear() with open('trace/eps.txt', 'w') as f: for i in range(m*k): f.write('{}\n'.format(eps[i])) # average eps avr_eps = avr(eps, -1) # print(' >>> {} <<< '.format(i)) print('avr_eps = {}'.format(avr_eps)) color = draw.black frame2 = draw.draw_rectangle(frame2, mask, color=draw.cyan) #frame2 = sf.get_box(frame2, mask) if out_pic is not None: for i in range(m*k): #print(i, m*k) t0 = time.time() th = 2 if eps[i] > avr_eps: color = draw.red elif eps[i] == -1: color = draw.purple else: color = draw.green if np.isnan(eps[i]): th = 3 color = draw.black pt1 = points1[i][0] + mask[0] #dpt = np.array([0, 0]) #if not np.isnan(avr_x[i]) and not np.isnan(avr_y[i]): dpt = np.array([avr_x[i], avr_y[i]]) pt2 = pt1 + dpt frame2 = draw.draw_point(frame2, pt1, radius=1, color=draw.blue) frame2 = draw.draw_point(frame2, pt2, radius=1, color=draw.blue) frame2 = draw.draw_arrow(frame2, pt2, pt1, color, thickness=th) #frame2 = draw.draw_text(frame2, pt1, text='({}|{})'.format(i // k, i % k), font_scale=0.25, line_type=1) tk = time.time() times.append(tk - t0) if i % (m*k // 10) == 0: T = (sum(times) / len(times)) * (m*k - i) t_min = np.int(T // 60) t_sec = T % 60 print(' drawing: {} | {} min {} sec'.format(i, t_min, t_sec)) cv2.imwrite(out_pic, frame2) if out_data is not None: with open(out_data, 'w') as f: for i in range(m*k): line = '{}|{}|{}|{}|{}\n'.format(i // k, i % k, avr_x[i], avr_y[i], eps[i]) f.write(line) # out = frame2.copy() # for i in range(N): # pt1 = pts1[i] + mask[0] # pt2 = pts2[i] + mask[0] # out = draw.draw_point(out, pt1, radius=3) # out = draw.draw_point(out, pt2, radius=3) # out = draw.draw_arrow(out, pt1, pt2) # cv2.imwrite('out/{}.jpg'.format(itt), out) print('done') return avr_x, avr_y, eps
residuals.sort(key=lambda r: np.linalg.norm(r[2] - r[1] - r[3]) / np.linalg .norm(r[3])) ind_max = len(residuals) for i in range(len(residuals)): r = residuals[i] if np.linalg.norm(r[2] - r[1] - r[3]) / np.linalg.norm( r[3]) > OUTLIER_THRESHOLD: ind_max = i break if debug: rr = residuals[ind_max:] for r in rr: pt1 = r[1] pt2 = r[2] pt3 = pt1 + r[3] out = draw.draw_arrow(out, pt1, pt2, color=draw.red) out = draw.draw_arrow(out, pt1, pt3, color=draw.gold) residuals = residuals[:ind_max] pts1 = [r[1] for r in residuals] pts2 = [r[2] for r in residuals] tt_of_filtering.append(time_tressing(t0, '\t\tof filtering')) if len(pts1) == 0: B_v = B_factor * B T_v = frame_itt / 25 Ba.append(B_v) Ta.append(T_v) sa.append(speed_g[frame_itt]) Bm7_v = get_median(B_v) B_median7.append(Bm7_v) make_result(out, frame_itt,
def do(): video_adr = 'input/calibration_video.mp4' cam = cv2.VideoCapture(video_adr) _, img = cam.read() y, x, z = img.shape 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]) m_start = np.array([x // 2 - 50, y * 0.4], np.float32) #mask = [p1_rec, p2_rec] L = 10_000 LINES = 250 intensity_threshould = 25 rows = 10 cols = 20 weight = 0.1 N = rows * cols arrow_size_factor = 10 max_cos = np.cos(20 * np.pi / 180) LINES = 20 img2 = img grid = cf.get_grid(p1_rec, p2_rec, rows, cols) for itt_frame in range(4): img1 = img2 img2 = cam.read()[1] out = draw.draw_rectangle(img2, [ m_start + np.array([-5, -5], np.float32), m_start + np.array([5, 5], np.float32) ], color=draw.cyan) results = [] time_per_frame = 0 for du_m in range(-5, 5 + 1): for dv_m in range(-5, 5 + 1): T = 0 m_temp = m_start + np.array([du_m, dv_m], np.float32) points, count = mn.get_points(550, 30 * np.pi / 180, LINES, m_temp) F_x = collections.deque() F_y = collections.deque() F_z = collections.deque() F_f = collections.deque() F_o = collections.deque() vector_result = [] # residuals = collections.deque() # POINTS = collections.deque() print('START {} ITERATION'.format(itt_frame)) if not _: break print(' START points generation:', end=' ') t0 = time.time() pts_filtered = mn.intensity_threshould_filter_of_points( img1, points, intensity_threshould) tk = time.time() print(tk - t0) T += tk - t0 print(' START optical flow:', end=' ') t0 = time.time() mod_pts = mn.modification_points(pts_filtered) pts1, pts2 = pai.find_opt_flow_lk_with_points( img1, img2, mod_pts) tk = time.time() print(tk - t0) T += tk - t0 print(' START buildinig of vectors:', end=' ') t0 = time.time() for i in range(len(pts1)): pt1 = pts1[i] pt2_o = pts2[i] # POINTS.append(pts1[i]) u = pts1[i][0] - m_temp[0] v = pts1[i][1] - m_temp[1] du_x, dv_x = vector_flow_rotation_x(u, v, L) du_y, dv_y = vector_flow_rotation_y(u, v, L) du_z, dv_z = vector_flow_rotation_z(u, v) du_f, dv_f = vector_flow_forward(u, v, L) F_x.append([du_x, dv_x]) F_y.append([du_y, dv_y]) F_z.append([du_z, dv_z]) F_f.append([du_f, dv_f]) F_o.append(pts2[i] - pts1[i]) tk = time.time() print(tk - t0) T += tk - t0 print(' START calculations:', end=' ') t0 = time.time() M = np.zeros((4, 4), np.float32) V = np.zeros(4, np.float32) for i in range(len(F_f)): M[0][0] += F_x[i][0]**2 + F_x[i][1]**2 M[0][1] += F_x[i][0] * F_y[i][0] + F_x[i][1] * F_y[i][1] M[0][2] += F_x[i][0] * F_z[i][0] + F_x[i][1] * F_z[i][1] M[0][3] += F_x[i][0] * F_f[i][0] + F_x[i][1] * F_f[i][1] M[1][1] += F_y[i][0]**2 + F_y[i][1]**2 M[1][2] += F_y[i][0] * F_z[i][0] + F_y[i][1] * F_z[i][1] M[1][3] += F_y[i][0] * F_f[i][0] + F_y[i][1] * F_f[i][1] M[2][2] += F_z[i][0]**2 + F_z[i][1]**2 M[2][3] += F_z[i][0] * F_f[i][0] + F_z[i][1] * F_f[i][1] M[3][3] += F_f[i][0]**2 + F_f[i][1]**2 V[0] += F_o[i][0] * F_x[i][0] + F_o[i][1] * F_x[i][1] V[1] += F_o[i][0] * F_y[i][0] + F_o[i][1] * F_y[i][1] V[2] += F_o[i][0] * F_z[i][0] + F_o[i][1] * F_z[i][1] V[3] += F_o[i][0] * F_f[i][0] + F_o[i][1] * F_f[i][1] for i in range(4): for j in range(i): M[i][j] = M[j][i] A_x, A_y, A_z, B = np.linalg.solve(M, V) tk = time.time() print(tk - t0) T += tk - t0 print(' START drawing:', end=' ') t0 = time.time() summ = 0 counter = 0 for i in range(len(pts1)): pt = pts1[i] vvx = A_x * np.array(F_x[i], np.float32) vvy = A_y * np.array(F_y[i], np.float32) vvz = A_z * np.array(F_z[i], np.float32) vvf = B * np.array(F_f[i], np.float32) pt2_v = vvx + vvy + vvz + vvf pt2_o = F_o[i] r = pt2_o - pt2_v norm_r = np.linalg.norm(r) vector_result.append(norm_r) summ += norm_r counter += 1 # with open('out/rotations/{}_{}/data.txt'.format(du_m, dv_m), 'a') as f: # f.write('{} / {} = {} | summ = {}\n'.format(summ, counter, summ / counter, summ)) tk = time.time() print(tk - t0) T += tk - t0 t0 = time.time() print(' Modification of points of infinity:', end=' ') tk = time.time() print(tk - t0) T += tk - t0 print('TIME: {} min {:02.02f} sec'.format( int((T) // 60), (T) % 60)) results.append([summ, m_temp]) # with open('out/rotations/frame{}.txt'.format(itt_frame), 'a') as file: # file.write('{}|{}|{}|{}\n'.format(du_m, dv_m, summ, counter)) time_per_frame += T vector_result.sort() LEN = int(0.9 * len(vector_result)) counter = 0 summ = 0 for i in range(LEN): counter += 1 summ += vector_result[i] with open('out/rotations/frame{}.txt'.format(itt_frame), 'a') as file: file.write('{}|{}|{}|{}\n'.format(du_m, dv_m, summ, counter)) r = min(results) print('END FRAME ANALISYS: {} min {:02.04f} sec'.format( int(time_per_frame // 60), time_per_frame % 60)) out = draw.draw_arrow(out, m_start, r[1]) cv2.imwrite('out/rotations/{}.jpg'.format(itt_frame), out) m_start = (1 - weight) * m_start + weight * r[1]