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