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 = []
