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]
