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 draw(self, color='b'): """ Draw the COM trajectory. Parameters ---------- color : char or triplet, optional Color letter or RGB values, default is 'b' for green. Returns ------- handle : openravepy.GraphHandle OpenRAVE graphical handle. Must be stored in some variable, otherwise the drawn object will vanish instantly. """ handles = draw_trajectory(self.P, color=color) handles.extend(draw_trajectory(self.Z, color='m')) com_last = self.p_last dcm_last = self.p_last + self.pd_last / self.omega cp_target = self.dcm_target + gravity / self.omega2 cp_last = dcm_last + gravity / self.omega2 for k in xrange(self.nb_steps): p, z = self.P[k], self.Z[k] handles.append(draw_line(z, p, color='c', linewidth=0.2)) handles.extend([ draw_point(self.dcm_target, color='b', pointsize=0.025), draw_point(cp_target, color='b', pointsize=0.025), draw_line(self.dcm_target, cp_target, color='b', linewidth=1), draw_point(com_last, color='g', pointsize=0.025), draw_point(cp_last, color='g', pointsize=0.025), draw_line(com_last, cp_last, color='g', linewidth=1), ]) return handles
def show_com(self): """Show a red ball at the location of the center of mass.""" self.__show_com = True self.__com_handle = [ draw_point(self.com, pointsize=0.0005 * self.mass, color='r'), draw_line( self.com, self.com + [0., 0., -1.], linewidth=2., color='y')]
from math import ceil from draw import initial_canvas, draw_canvas, draw_point # initialise canvas (32*2 col, 32 row) canvas = [[" " for x in range(64)] for y in range(32)] initial_canvas(canvas) a = 0.5 b = 10 for x in range(0, 30, int(ceil(1/a))): draw_point(x, a*x+b, canvas, mark='o') draw_canvas(canvas)
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 if np.linalg.norm( model_vector) > MIN_MODEL_VECTOR and np.linalg.norm( model_vector ) < COEFF_OPT_MORETHAN_MODEL * np.linalg.norm(opt_vector): pts1_clean.append(pt1) pts2_clean.append(pt2_opt) if debug: out = draw.draw_arrow(out, pt1, pt2_opt) # drawing if debug: out = draw.draw_point(out, pai_in_frame, thickness=3, color=draw.blue) cv2.imwrite('output/{}_model1.png'.format(frame_iterator), out) out = img2.copy() sum_current = [ ] # vector of sums of residuals per each points around point of start cef_current = [] # time_analisys(t, 'preanalisys of frame: ') # t_steps = time.time() for step in range(NUMBER_OF_STEPS): print(' START {} STEP'.format(step)) # t = time.time() M, sum_current_, residuals_clean, A_x, A_y, A_z, B = build_matrix( pai_in_frame, delta_vector, pts1_clean, pts2_clean, L)
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')
e2 = cale(a2, b2, dList[k:]) # print k, a2, e2 if e > e1 + e2: ea1 = a1 eb1 = b1 ea2 = a2 eb2 = b2 ex = xm ey = ym e = e1 + e2 # draw scatter canvas = [[" " for x in range(64)] for y in range(32)] initial_canvas(canvas) for (x, y) in dList: draw_point(x, y, canvas, mark='*') print "turn point: ({}, {})".format(ex, ey) # draw fit for x in range(0, ex, int(ceil(1 / ea1))): if ea1 * x + eb1 < 0: continue draw_point(x, ea1 * x + eb1, canvas, mark='o') for x in range(ex, 30, int(ceil(1 / ea2))): if ea2 * x + eb2 > 30: break draw_point(x, ea2 * x + eb2, canvas, mark='o') draw_canvas(canvas)
a = float((n * sum(x * y for (x, y) in dl) - sum(xl) * sum(yl))) / d b = float( sum(x**2 for x in xl) * sum(yl) - sum(x * y for (x, y) in dl) * sum(xl)) / d return a, b f = open("data1.txt", "r") dList = [] for line in f: tmp = eval(line) dList.append(tmp) # test # dList = map(lambda x: (x[0], min(30, x[1]+3)), dList) # draw scatter canvas = [[" " for x in range(64)] for y in range(32)] initial_canvas(canvas) for (x, y) in dList: draw_point(x, y, canvas, mark='*') #draw fit a, b = lsa(dList) print a, b for x in range(0, 30, int(ceil(1 / a))): print x, a * x + b draw_point(x, a * x + b, canvas, mark='o') draw_canvas(canvas)
filename = 'input/parsed-1750be37-5499-4c6e-9421-9bb15b277a94.txt' K = 270 times, lon, lat, dist, speeds = sf.get_data_from_parsed_file(filename) frame_delay = 1360 pt_min = np.array([min(lon), min(lat)]) pt_max = np.array([max(lon), max(lat)]) map_ = np.zeros((270, 270, 3)) + draw.white pts_map = [ np.array([(lon[i] - pt_min[0]) * K / (pt_max[0] - pt_min[0]), (-lat[i] + pt_max[1]) * K / (pt_max[1] - pt_min[1])]) for i in range(len(lon)) ] for pt in pts_map: map_ = draw.draw_point(map_, (int(pt[0]), int(pt[1])), 1, thickness=1, color=draw.blue) for i in range(len(times)): times[i][0] -= 13 times[i][1] -= 55 times[i][2] -= 28 if times[i][0] == 1: times[i][0] -= 1 times[i][1] += 60 m = times[i][1] times[i][1] -= m times[i][2] += m * 60 times = [t[2] for t in times] speed_g = [-1 for i in range(frame_delay)] lat_g = [-1 for i in range(1360)] lon_g = [-1 for i in range(1360)]
for i in range(N): if not good[i]: 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
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()
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 i += 1 is_captured, f2 = capture.read()
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
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 show_com(self): """Show a red ball at the COM location.""" self.__show_com = True self.__com_handle = draw_point( self.com, pointsize=0.0005 * self.mass, color='r')
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
#del_last_files('output/speed/') filename = 'input/parsed-1750be37-5499-4c6e-9421-9bb15b277a94.txt' cam = cv2.VideoCapture('input/1750be37-5499-4c6e-9421-9bb15b277a94.mp4') frame_rate = 25 times, lon, lat, dist, speeds = get_data_from_parsed_file(filename) frame_delay = 1360 # 685 + 27 * 25 K = 15_000 #MAP GENERATING: pt0 = -K * np.array([min(lon), min(lat)]) map_ = np.zeros((270, 240, 3)) + draw.cyan for k in range(len(lat)): dp = np.array([K * lon[k], K * lat[k]]) pt = pt0 + dp map_ = draw.draw_point(map_, (pt[1], pt[0]), 1, thickness=1, color=draw.blue) img = cam.read()[1] print(img.shape) img[50:320, 1630:1870] = map_ cv2.imwrite('map.png', img) #sys.exit('DONE') lat_g, lon_g, brng_g, speed_g, x = [], [], [], [], [] TIME = Time(13, 55, 3) add_time = TIME.get_sec() time_of_2_raw = -1 for i in range(frame_delay): lat_g.append(0) lon_g.append(0) brng_g.append(0)
weight = 0.1 for I in range(N_FRAMES): t0 = time.time() img1 = img2 img2 = cam.read()[1] m_points = bc.Points() # omega, omega_0, omega_1, omega_2 = get_corners(p1_rec, p2_rec, m_new) #points = gen_points(omega_0, omega_1, omega_2, omega, LINES) points, count = get_points(550, 30 * np.pi / 180, LINES, m_new) print('Number of points: {}'.format(count)) out = img1.copy() for pt_r in points: for pt in pt_r: out = draw.draw_point(out, np.array([pt[0], pt[1]]), radius=3, color=draw.gold) # for i in range(len(points)): # if i % 1000 == 0: # print(' {} / {}'.format(i, len(points))) # for j in range(len(points[i])): # out = draw.draw_point(out, points[i][j], 1) # out = draw.draw_point(out, m, color=draw.dark_green, thickness=3) # cv2.imwrite('out/{}_all.jpg'.format(I), draw.draw_grid(out, grid)) # out = img1.copy() #out = draw.draw_grid(out, grid) pts_filtered = intensity_threshould_filter_of_points( img1, points, intensity_threshould) print('filtered: {}'.format(len(pts_filtered)))
def find_OF_crossing_pt(self, num_cycles=30, num_rnd_lines=20, delta=15, trace=False, path='output/', img=None): """ 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 """ cycle = 0 # iterator #max length of deque below ??????????????????????????????????????????????????? dist = deque() # vector characteristic for all points points_of_cross = deque() self.make_lines() # Generating img with all lines and current trace folder. if trace: out = img.copy() inliers = self.get_indexes_of_inliers() for id in inliers: point = self.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 = self.get_subset_of_rnd_lines(num_rnd_lines) # Draw current subset, if need if trace: out = img.copy() color = draw.blue for s in subset: k, b = self.get_point_by_id(s).get_line() out = draw.draw_line(out, k, b, color=color) # Trying to find current point of cross pt = self.point_of_crossing(subset) if not np.isnan(pt[0]): points_of_cross.append(pt) dist.append(self.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 self.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 self.check_for_lines_in_interesting_area(temp_point, delta) inliers = self.get_indexes_of_inliers() pt = self.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 = img.copy() for id in inliers: k, b = self.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 = img.copy() for i in self.get_indexes_of_inliers(): point = self.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
lon_g.append(lon_) times_g.append(t_) times_GPS.append(t_gps) # Map Generating pt_min = np.array([min(lon), min(lat)]) pt_max = np.array([max(lon), max(lat)]) K = int(Height / 4) # window size for map map_ = np.zeros((K, K, 3)) + COLOR_OF_MAP_BACKGRAUND pts_map = [ np.array([(lon[i] - pt_min[0]) * K / (pt_max[0] - pt_min[0]), (-lat[i] + pt_max[1]) * K / (pt_max[1] - pt_min[1])]) for i in range(len(lon)) ] for pt in pts_map: map_ = draw.draw_point(map_, (int(pt[0]), int(pt[1])), 1, thickness=1, color=draw.blue) sa = [] Ta = [] Ba = [] B_median7 = [] max_y = [] out = -1 tmp_map = -1 PAI = -1 #time test tt_reading = [] tt_nn = [] tt_draw_rec = [] tt_bb_filtering = []
v_new = v_mtr - np.mean(deltas) plt.plot(x, v_mtr, x, v_gps, x, y, x, v_new) plt.show() plt.plot(x[:1400], v_mtr[:1400], x[:1400], v_gps[:1400], x[:1400], y[:1400], x[:1400], v_new[:1400]) plt.show() sys.exit() video = cv2.VideoCapture(data) times = collections.deque(maxlen=lengh) for i in range(1, lengh): t0 = time.time() print(' saving: {} / {}'.format(i, lengh)) out = frames[i].copy() out = draw.draw_point(out, m) out = draw.draw_mahalanobis_ellipse(out, r, m, K, color=draw.red) out = draw.draw_mahalanobis_ellipse(out, 3 * r, m, K, color=draw.blue) area1 = sf.get_box(frames[i - 1], mask) area2 = sf.get_box(frames[i], mask) pt, st = pai.find_OF_crossing_pt(area1, area2, method='lk') r_p = None if st: if not mask is None: pt += mask[0] if np.linalg.norm(pt) > np.linalg.norm( (img_dim[1], img_dim[0])): pt = (max(img_dim) / 2) * pt / np.linalg.norm(pt) r_point_sq = sf.get_mahalanobis_distance_sq(pt, m, K_inv) r_p = np.sqrt(r_point_sq) out = cv2.line(out, (int(m[0]), int(m[1])),
print('{} / {}'.format(I, N_FRAMES)) img1 = img2.copy() img2 = cam.read()[1] gray = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) middle_vectors = np.zeros((rows, cols, 3), np.float32) middle_vectors_D = np.zeros((rows, cols), np.float32) out = img1.copy() out_ = img1.copy() out__ = img1.copy() points = collections.deque( ) # points, that delta intensivity is bigger than for i in range(2, len(pts_start)): pt1 = (pts_start[i - 2][0], pts_start[i - 2][1]) pt2 = (pts_start[i][0], pts_start[i][1]) out__ = draw.draw_point(out__, pt1, 3, color=draw.black) F1 = int(gray[pt1[1]][pt1[0]]) F2 = int(gray[pt2[1]][pt2[0]]) d = np.abs(F1 - F2) if d > 25: points.append(pt1) # print(i) out = draw.draw_point(out, pt1, 3) out_ = draw.draw_point(out_, pt2, 3, color=draw.cyan) out = draw_grid(out, grid) out_ = draw_grid(out_, grid) cv2.imwrite('out/o.jpg', out) cv2.imwrite('out/o_.jpg', out_) cv2.imwrite('out/o__.jpg', out__)
for frame_itt in range(NUMBER_OF_FRAMES): print('FRAME: {}'.format(frame_itt)) file.write('frame: {}\n'.format(frame_itt)) file.flush() img1 = img2 img2 = cam.read()[1] if frame_itt < frame_delay: continue for lines_itt in range(len(LINES)): print(' Lines: {:02d}'.format(LINES[lines_itt]), end=' -> ') pic_name = 'output/{:02d}_lines={:03d}.png'.format(frame_itt, LINES[lines_itt]) out = img1.copy() out = draw.draw_point(out, pai[lines_itt], 5, draw.red, 5) out = cv2.ellipse(out, (pai_in_frame[0], pai_in_frame[1]), (lenght_of_axis, lenght_of_axis), 0, angle, 180 - angle, color=draw.cyan) _y_ = int(pai_in_frame[1]) + max_y_for_points _x1_ = int(x // 4) _x2_ = int(3 * x // 4) out = cv2.line(out, (_x1_, _y_), (_x2_, _y_), color=draw.cyan) #1.2 interesting_points = mn.intensity_threshould_filter_of_points(img1, all_points[lines_itt], intensity_threshould) # generation an interesting points for current frame if len(interesting_points) == 0: 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:
e2 = cale(a2, b2, dList[k:]) # print k, a2, e2 if e > e1 + e2: ea1 = a1 eb1 = b1 ea2 = a2 eb2 = b2 ex = xm ey = ym e = e1 + e2 # draw scatter canvas = [[" " for x in range(64)] for y in range(32)] initial_canvas(canvas) for (x,y) in dList: draw_point(x, y, canvas, mark='*') print "turn point: ({}, {})".format(ex, ey) # draw fit for x in range(0, ex, int(ceil(1/ea1))): if ea1 * x + eb1 < 0: continue draw_point(x, ea1*x+eb1, canvas, mark='o') for x in range(ex, 30, int(ceil(1/ea2))): if ea2 * x + eb2 > 30: break draw_point(x, ea2*x+eb2, canvas, mark='o') draw_canvas(canvas)
from draw import initial_canvas, draw_canvas, draw_point # initialise canvas (32*2 col, 32 row) canvas = [[" " for x in range(64)] for y in range(32)] initial_canvas(canvas) # read data f = open("data1.txt", "r") dList = [] for line in f: tmp = eval(line) dList.append(tmp) for (x, y) in dList: draw_point(x, y, canvas, mark='*') draw_canvas(canvas)
lon_ = lon[i] lat_ = lat[i] t_ = times[i] + frame_delay / 25 for k in range(dt * 25): speed_g.append(s) lat_g.append(lat_) lon_g.append(lon_) times_g.append(t_) # Map Generating pt_min = np.array([min(lon), min(lat)]) pt_max = np.array([max(lon), max(lat)]) K = int(Height / 4) # window size for map map_ = np.zeros((K, K, 3)) + COLOR_OF_MAP_BACKGRAUND pts_map = [np.array([(lon[i] - pt_min[0]) * K / (pt_max[0] - pt_min[0]), (-lat[i] + pt_max[1]) * K / (pt_max[1] - pt_min[1])]) for i in range(len(lon))] for pt in pts_map: map_ = draw.draw_point(map_, (int(pt[0]), int(pt[1])), 1, thickness=1, color=draw.blue) sa = [] Ta = [] Ba = [] B_median7 = [] out = -1 tmp_map = -1 PAI = -1 #time test tt_reading = [] tt_nn = [] tt_draw_rec = [] tt_bb_filtering = [] tt_draw_of = [] tt_log = []