def get_point_transform():
hom = Homography(np.array(pts_src_), np.array(pts_real_))
# dist = hom.get_point_transform([1136, 184], [942, 363])
A = [1701, 746]
B = [1851, 43]
dist = hom.get_point_transform(A, B)
print()
def test_equal(self):
identity = Homography.identity()
eps = 1e-7
shift_by_eps = Homography.translation(eps, eps)
self.assertTrue(shift_by_eps.equal(identity, 2 * eps))
self.assertFalse(shift_by_eps.equal(identity, eps / 2.))
self.assertAlmostEqual(shift_by_eps, identity)
def test_affine(self):
affine = Affine.translation(42, 142)
actual = Homography.from_affine(affine)
expected = Homography.translation(42, 142)
self.assertAlmostEqual(actual, expected)
as_affine = actual.to_affine()
self.assertAlmostEqual(affine, as_affine)
def test_constructor(self):
""" verify all construction methods are identical. """
h_from_ndarray = Homography([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
h_from_affine = Homography(Affine.identity())
h_from_homography = Homography(h_from_ndarray)
self.assertEqual(h_from_ndarray, h_from_affine)
self.assertEqual(h_from_ndarray, h_from_homography)
self.assertEqual(h_from_ndarray, Homography.identity())
def test_arithmetics(self):
h1 = Homography.rotation(90)
h2 = Homography.translation(3, 4)
h3 = Homography.scale(5, 6)
h = h3 * h2 * h1
actual = h([7, 8])
self.assertTrue(np.allclose(actual, [5 * (8 + 3), 6 * (-7 + 4)]))
via_3d = h.apply(7, 8) / h.apply(7, 8)[2]
self.assertTrue(np.allclose(actual, via_3d[:2]))
def test_distance2(self):
h1 = Homography.identity()
h2 = Homography.scale(0.5, 0.25)
img_size = [100, 100]
dist = h1.dist(h2, *img_size)
expected_dist = np.sqrt(50**2 + 75**2)
self.assertAlmostEqual(expected_dist, dist)
dist = h1.dist_sourcespace(h2, *img_size)
expected_dist_source = np.sqrt(100**2 + 300**2)
self.assertAlmostEqual(expected_dist_source, dist)
dist = h1.dist_bidirectional(h2, *img_size)
expected_dist_bidi = max(expected_dist_source, expected_dist)
self.assertAlmostEqual(expected_dist_bidi, dist)
def test_homography_decomposition(self):
"""
Test homography decomposition by following steps:
1. prepare affine with random rotation R and translation t
2. make homography of affine from step 1, with random projection P
**NOTE: t first, then R !! ***
3. decompose homography using P to get R' and t'
4. compare (R, t) and (R', t')
"""
# step 1
R = ortho_group.rvs(dim=3)
R /= np.linalg.det(R)
t = np.random.random((3, )) * 2 - 1
P = np.random.random((3, 3))
P[0, 0] = P[1, 1] = P[2, 2] = 1
P[0, 1:] = 0
P[1, 2] = 0
# step 2
A = np.array([R[:, 0], R[:, 1], R @ t]).T
H = P @ A
# step 3
hom = Homography(H, P)
R_, t_ = hom.R, hom.t
# step 4
np.testing.assert_almost_equal(R, R_)
np.testing.assert_almost_equal(t, t_)
def __init__(self, homography_file, filter_file, tune_param):
self.bridge = CvBridge()
# load homography matrix
self.homography_file = str(homography_file)
self.homography = Homography(self.homography_file)
self.homography_matrix = self.homography.get_homography_matrix()
if self.homography_matrix is None:
print("File doesnt have a homography matrix")
print("Run find_homography script")
# load parameters used in the filter process
self.filter_file = filter_file
self.filter_param = Parameters(filter_file)
self.tune_param = tune_param
# configure parameters
if tune_param:
self.filter_param.create_trackbar()
if os.path.exists(self.filter_file):
self.filter_param.load()
self.filter_param.set_trackbar_values()
# autonomous mode
else:
if os.path.exists(self.filter_file):
self.filter_param.load()
else:
print("Filter file doesnt exist")
exit(1)
self.init = True
#
self.filter_res = 0
self.image = None
self.warp_res = None
self.color_res = None
self.filter_res = None
self.lanes_list = None
def test_distance(self):
h1 = Homography.identity()
h2 = Homography.translation(3, 4)
img_size = [42, 123456]
dist = h1.dist_sourcespace(h2, *img_size)
expected_dist = np.sqrt(3**2 + 4**2)
self.assertAlmostEqual(expected_dist, dist)
dist = h1.dist(h2, *img_size)
self.assertAlmostEqual(expected_dist, dist)
expected_rel_dist = expected_dist / np.linalg.norm(img_size)
rel_dist = h1.rel_dist(h2, *img_size)
self.assertAlmostEqual(expected_rel_dist, rel_dist)
self.assertAlmostEqual(h1.norm(42, 123456), 0)
self.assertAlmostEqual(h2.norm(42, 123456), expected_dist)
def __init__(self):
# Pub/Sub
self.sub_camera = rospy.Subscriber('/zed/rgb/image_rect_color', Image,
self.sub_callback_image)
self.pub_bounding_box = rospy.Publisher('/bounding_box',
bounding_box,
queue_size=10)
self.pub_state = rospy.Publisher('/state',
Float32MultiArray,
queue_size=10)
self.pub_image = rospy.Publisher('/image_bounded',
Image,
queue_size=10)
# Image data
self.bridge = CvBridge()
self.H = Homography()
def test_ransac(self):
"""
Test ransac by following steps:
1. make random homography
2. make inliers with noise
3. make outliers
4. RUN
"""
N_TOTAL = 1000
N_INLIER = 500
N_ITER = trial_count(N_INLIER / N_TOTAL, 4)
N_OUTLIER = N_TOTAL - N_INLIER
NOISE_SIZE = 0
# step 1
gt = np.random.random((3, 3))
gt[2, :] *= 100
gt[2, 2] = 1
# step 2
hom = Homography(gt)
x = np.random.random((N_INLIER, 2))
y = hom.transform(x)
x += np.random.random((N_INLIER, 2)) * NOISE_SIZE
y += np.random.random((N_INLIER, 2)) * NOISE_SIZE
inliers = np.concatenate((x, y), axis=1)
# step 3
outliers = np.random.random((N_OUTLIER, 4))
data = np.concatenate((inliers, outliers), axis=0)
# step 4
from ransac import RANSAC
ransac_inst = RANSAC(
n_sample=4,
n_iter=N_ITER,
err_thres=0.1,
)
model = ProjectionModel()
best_func, best_inliers = ransac_inst(data, model)
np.testing.assert_almost_equal(best_func.M, gt)
def __init__(self, camera, pnts_pattern):
self.rep = Representation()
self.h = Homography(pnts_pattern)
self.tcp = TCP()
self.a = Angle()
self.camera = camera
self.hom = np.zeros((3, 3))
self.inv_hom = np.zeros((3, 3))
self.Frame = np.zeros((3, 3))
self.inv_Frame = np.zeros((3, 3))
self.hom_final_camera = np.zeros((3, 3))
def test_from_points(self):
src = np.array(
[[0.0, 0.0], [200.0, 0.0], [200.0, 100.0], [0.0, 100.0]])
diff = np.array([10, 20])
dst = src - diff
h = homography.from_points(src, dst)
expected = Homography.translation(-diff[0], -diff[1])
self.assertEqual(h, expected)
src = [Point(x, y) for x, y in src]
dst = [Point(x, y) for x, y in dst]
h = homography.from_points(src, dst)
self.assertEqual(h, expected)
class ObjectFinder:
def __init__(self):
# Pub/Sub
self.sub_camera = rospy.Subscriber('/zed/rgb/image_rect_color', Image,
self.sub_callback_image)
self.pub_bounding_box = rospy.Publisher('/bounding_box',
bounding_box,
queue_size=10)
self.pub_state = rospy.Publisher('/state',
Float32MultiArray,
queue_size=10)
self.pub_image = rospy.Publisher('/image_bounded',
Image,
queue_size=10)
# Image data
self.bridge = CvBridge()
self.H = Homography()
def sub_callback_image(self, data):
img = self.bridge.imgmsg_to_cv2(data, 'bgr8')
box = color_seg(img)
msg = bounding_box()
msg.x_min = box[0][0]
msg.y_min = box[0][1]
msg.x_max = box[1][0]
msg.y_max = box[1][1]
dx, dy = self.H.find_box_bottom_center(box)
print('BOX LOCAL: ', dx, dy)
state_msg = Float32MultiArray()
state_msg.data = [dx, dy]
self.pub_state.publish(state_msg)
img_new = img
cv2.rectangle(img_new, (box[0][0], box[0][1]), (box[1][0], box[1][1]),
(0, 255, 0), 2)
msg_img = self.bridge.cv2_to_imgmsg(img_new, 'bgr8')
self.pub_image.publish(msg_img)
def test_todict(self):
h1 = Homography.translation(2, 3)*Homography.scale(2, 1)
h2 = Homography.from_dict(h1.to_dict())
self.assertEqual(h1, h2)
def draw_axis_camera(self, image, pnts):
cv2.line(image, (pnts[0][0], pnts[0][1]), (pnts[1][0], pnts[1][1]), RED, 2)
cv2.line(image, (pnts[0][0], pnts[0][1]), (pnts[2][0], pnts[2][1]), GREEN, 2)
return image
def draw_points(self, image, pnts):
for pnt in pnts:
cv2.circle(image, (int(pnt[0]), int(pnt[1])), 10, BLUE, -1)
return image
if __name__ == '__main__':
pnts_pattern = np.float32([[0, 0], [1.1, 0], [0, 1.1], [1.1, 1.1]])
homography = Homography(pnts_pattern)
pnts = np.float32([[0, 0],
[1, 0],
[0, 1]])
corners = np.float32([[-2.5, -2.5],
[2.5, -2.5],
[-2.5, 2.5],
[2.5, 2.5]])
pnts_final = np.float32([[0, 0],
[500, 0],
[0, 500],
[500, 500]])
class Cal_camera():
def __init__(self, camera, pnts_pattern):
self.rep = Representation()
self.h = Homography(pnts_pattern)
self.tcp = TCP()
self.a = Angle()
self.camera = camera
self.hom = np.zeros((3, 3))
self.inv_hom = np.zeros((3, 3))
self.Frame = np.zeros((3, 3))
self.inv_Frame = np.zeros((3, 3))
self.hom_final_camera = np.zeros((3, 3))
def get_pxls_homography(self, image, scale=1):
pts_image = self.h.read_image(image, scale)
return pts_image
def calculate_homography(self, pxls_pattern):
self.hom = self.h.calculate(pxls_pattern)
self.inv_hom = linalg.inv(self.hom)
def get_pxl_origin(self, folder, scale=1):
pxl_pnts = []
for f in sorted(folder):
im_uEye = cv2.imread(f)
pxl_pnts.append(self.tcp.read_image(im_uEye, scale))
return pxl_pnts
def get_pxl_orientation(self, folder, scale=1):
for f in sorted(folder):
name = basename(f)
if name == "move_x.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_x = self.tcp.read_image(im_uEye, scale)
elif name == "move_y.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_y = self.tcp.read_image(im_uEye, scale)
elif name == "move_o.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_origin = self.tcp.read_image(im_uEye, scale)
return pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y
def calculate_TCP_orientarion(self, pxl_pnts, pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y, dx, dy):
pxl_TCP = self.tcp.calculate_origin(pxl_pnts)
print "TCP :", pxl_TCP
factor, angle_y, angle_x = self.tcp.calculate_orientation(pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y, dx, dy)
return pxl_TCP, factor, angle_y, angle_x
def calculate_angle_TCP(self, origin, axis_x, pattern):
pnt_origin = self.rep.transform(self.hom, origin)
pnt_x = self.rep.transform(self.hom, axis_x)
pnt_pattern = self.rep.transform(self.hom, pattern)
l_1 = np.float32([pnt_origin[0], pnt_x[0]])
l_2 = pnt_pattern[0:2]
vd1 = self.a.director_vector(l_1[0], l_1[1])
vd2 = self.a.director_vector(l_2[0], l_2[1])
angle = self.a.calculate_angle(vd1, vd2)
return angle
def calculate_frame(self, pxl_TCP, angle):
pnts_TCP = self.rep.transform(self.hom, pxl_TCP)[0]
a = np.deg2rad(angle)
self.Frame = np.float32([[np.cos(a), -np.sin(a), pnts_TCP[0]],
[np.sin(a), np.cos(a), pnts_TCP[1]],
[0, 0, 1]])
self.Orientation = np.float32([[np.cos(a), -np.sin(a), 0],
[np.sin(a), np.cos(a), 0],
[0, 0, 1]])
self.inv_Orientation = linalg.inv(self.Orientation)
self.inv_Frame = linalg.inv(self.Frame)
def calculate_hom_final(self, img, pnts, corners, pnts_final):
# pxls_camera = self.rep.transform(self.inv_hom, pnts)
im_measures = self.rep.define_camera(img.copy(), self.hom)
#------------------ Data in (c)mm
image_axis = self.rep.draw_axis_camera(im_measures, pnts)
#------------------
pnts_Frame = pnts
pnts_camera = self.rep.transform(self.Frame, pnts_Frame)
image_axis_tcp = self.rep.draw_axis_camera(image_axis, pnts_camera)
#------------------
corners_Frame = corners
corners_camera = self.rep.transform(self.Frame, corners_Frame)
img = self.rep.draw_points(image_axis_tcp, corners_camera)
#------------------
pxls_corner = self.rep.transform(self.inv_hom, corners_camera)
self.hom_final_camera, status = cv2.findHomography(pxls_corner.copy(), pnts_final)
self.write_config_file()
return self.hom_final_camera
def write_config_file(self):
hom_vis = self.hom_final_camera
hom_TCP = np.dot(self.inv_hom, self.Frame)
inv_hom_TCP = np.dot(self.inv_Frame, self.hom)
data = dict(
hom_vis=hom_vis.tolist(),
hom=hom_TCP.tolist(),
inv_hom=inv_hom_TCP.tolist(),
)
filename = '../../config/' + self.camera + '_config.yaml'
with open(filename, 'w') as outfile:
yaml.dump(data, outfile)
print data
def visualize_data(self):
print "Homography: "
print self.hom
print "Frame: "
print self.Frame
print "Final homography: "
print self.hom_final_camera
def draw_pattern_axis(self, pnts, img):
pnts_axis_pattern = pnts
pxls_axis_pattern = self.rep.transform(self.inv_hom, pnts_axis_pattern)
pnt_axis_pattern_final = self.rep.transform(self.hom_final_camera, pxls_axis_pattern)
img_pattern_NIT = self.rep.draw_axis_camera(img, pnt_axis_pattern_final)
return img_pattern_NIT
def draw_TCP_axis(self, pnts, img):
pnts_axis = pnts
pnts_axis_TCP = self.rep.transform(self.Frame, pnts_axis)
pxls_axis_TCP = self.rep.transform(self.inv_hom, pnts_axis_TCP)
pnt_axis_TCP_final = self.rep.transform(self.hom_final_camera, pxls_axis_TCP)
img_final_axis = self.rep.draw_axis_camera(img, pnt_axis_TCP_final)
return img_final_axis
def draw_axis(self, pnts, img):
img_TCP = self.draw_pattern_axis(pnts, img)
img_final = self.draw_TCP_axis(pnts, img_TCP)
return img_final
def detect_motion(self, puntosHomography):
capture = openCv.VideoCapture(self.video)
openCv.namedWindow('frame', openCv.WINDOW_NORMAL) #nuevo ajuste imagen
capture.set(openCv.CAP_PROP_POS_FRAMES, self.start_frame)
coordinates_data = self.coordinates_data
DetectorHelper.calculateMask(coordinates_data, self.contours,
self.bounds, self.masks)
statuses = [True] * len(coordinates_data)
times = [None] * len(coordinates_data)
firstFrame = None
#nuevo
comienzo = time.time()
count_AUX = 1
# Agregar deteccion de bicicletas
#bicycleClassif = openCv.CascadeClassifier('body.xml')
while capture.isOpened():
result, frame = capture.read()
if frame is None:
break
if not result:
raise CaptureReadError(
"Error reading video capture on frame %s" % str(frame))
blurred = openCv.GaussianBlur(frame.copy(), (5, 5), 3)
grayed = openCv.cvtColor(blurred, openCv.COLOR_BGR2GRAY)
new_frame = frame.copy()
if firstFrame is None:
firstFrame = grayed
continue
self.detectMoves(new_frame, firstFrame, grayed)
self.calculateStatusByTime(capture, grayed, times, statuses)
self.drawingUtils.drawContoursInFrame(new_frame, statuses,
self.coordinates_data)
# Agregar deteccion de bicicletas
#bicycle = bicycleClassif.detectMultiScale(grayed,
#scaleFactor = 1.1,#1.1
#minNeighbors = 91,
#minSize=(50,90)
#)
#for (x,y,w,h) in bicycle:
# openCv.rectangle(new_frame, (x,y),(x+w,y+h),(0,128,255),2)
# openCv.putText(new_frame,'Ciclista',(x,y-10),2,0.7,(0,128,255),2,openCv.LINE_AA)
Homography.getVideoHomography(new_frame, puntosHomography)
openCv.imshow(str(self.video), new_frame)
#FOTO DEL ESTADO DEL BICICLETERO
momento = time.time()
if ((int(momento) -
int(comienzo)) == (count_AUX *
MotionDetector.FOTO_ESTADO_BICICLETERO)):
self.capturatorMobile.takePhotoStateBicycle(capture)
count_AUX = count_AUX + 1
k = openCv.waitKey(MotionDetector.SPEED)
if k == KEY_QUIT:
break
capture.release()
openCv.destroyAllWindows()
def test_projectivity(self):
h = Homography.translation(3, 4)
self.assertAlmostEqual(h.projectivity(), 0)
class CrossDetector():
def __init__(self, homography_file, filter_file, tune_param):
self.bridge = CvBridge()
# load homography matrix
self.homography_file = str(homography_file)
self.homography = Homography(self.homography_file)
self.homography_matrix = self.homography.get_homography_matrix()
if self.homography_matrix is None:
print("File doesnt have a homography matrix")
print("Run find_homography script")
# load parameters used in the filter process
self.filter_file = filter_file
self.filter_param = Parameters(filter_file)
self.tune_param = tune_param
# configure parameters
if tune_param:
self.filter_param.create_trackbar()
if os.path.exists(self.filter_file):
self.filter_param.load()
self.filter_param.set_trackbar_values()
# autonomous mode
else:
if os.path.exists(self.filter_file):
self.filter_param.load()
else:
print("Filter file doesnt exist")
exit(1)
self.init = True
#
self.filter_res = 0
def process(self, compressed_image):
#self.filter_param.create_trackbar()
#self.filter_param.update_trackbar_values()
start = time.time()
#image = self.bridge.compressed_imgmsg_to_cv2(compressed_image, "bgr8")
image = compressed_image
#if self.init == True:
# cv2.imwrite("/home/nesvera/image_test.jpg", image)
warp_res = cv2.warpPerspective(image.copy(), self.homography_matrix,
(400, 400))
#color_res = warp_res
color_res = cv2.cvtColor(warp_res, cv2.COLOR_BGR2GRAY)
filter_res = self.filter(color_res)
self.find_lanes(filter_res.copy())
periodo = time.time() - start
fps = 1 / periodo
print(fps)
# configure parameters mode
if self.tune_param == 1:
cv2.imshow("image", image)
cv2.imshow("warp_res", warp_res)
cv2.imshow("color_res", color_res)
cv2.imshow("filter_res", filter_res)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cv2.destroyAllWindows()
exit(1)
elif key == ord('l'):
self.filter_param.load()
self.filter_param.set_trackbar_values()
elif key == ord('s'):
self.filter_param.save(self.filter_param)
self.filter_param.update_trackbar_values()
def filter(self, image):
ret, thr = cv2.threshold(image, self.filter_param.gray_lower_bound,
255, cv2.THRESH_BINARY)
#ret, thr = cv2.threshold(image, self.filter_param.gray_lower_bound, self.filter_param.gray_upper_bound, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
# talvez aplicar sobel(gradient) + threshould
filtered = thr
return filtered
def find_lanes(self, image):
'''
top-left = roi_0_x, roi_0_y
bot-right = roi_1_x, roi_1_y
'''
# image size
img_h, img_w = image.shape
# initial search position y
#y_pos_init = self.filter_param.roi_0_y
# random initial position of y
#y_pos_init = random.randint(self.filter_param.roi_0_y,
# self.filter_param.roi_1_y + 1)
y_pos_init = int(
(self.filter_param.roi_0_y + self.filter_param.roi_1_y) / 2)
# get entire horizontal line for the first scan of lanes
hor_line = image[y_pos_init, :]
# list of lanes... lane is a class with points, width ...
lanes_list = []
# store temporary points for a interesting area of the horizontal search
points_found = []
for index, pixel in enumerate(hor_line):
if pixel == 255:
points_found.append(index)
elif pixel == 0 and len(points_found) > 0:
# after to collect all sequence of bright pixels, get the median
new_lane_center_point = np.array(
(int(np.median(points_found)), y_pos_init))
points_found = []
# create a Lane obj and add to the list of lanes
lanes_list.append(
Lane(new_lane_center_point, len(points_found)))
# from the points found in the first search, find all points connected to this lane
# using sliding window with a box of fixed size
for lane in lanes_list:
# get last point found of a lane
last_point = lane.get_last_point()
x_lane_start = last_point[0]
y_lane_start = last_point[1]
# search up
x_pos_cur = x_lane_start
y_pos_cur = y_lane_start + 1
points_found = []
while y_pos_cur >= self.filter_param.roi_0_y:
x_box_start = x_pos_cur - int(
self.filter_param.search_box_w / 2)
x_box_end = x_pos_cur + int(self.filter_param.search_box_w / 2)
# get pixels from a search box
hor_line = image[y_pos_cur, x_box_start:x_box_end]
# real index of a hor_line pixel in the image
hor_line_image_index = [
i for i in range(x_box_start, x_box_end)
]
for index, pixel in enumerate(hor_line):
# store bright pixels
if pixel == 255:
points_found.append(index)
if len(points_found) > 0:
lane_center = hor_line_image_index[int(
np.median(points_found))]
new_lane_point = np.array((lane_center, y_pos_cur))
points_found = []
# append a new point to the list
lane.add_point(new_lane_point, len(points_found))
# x start point for the next search
x_pos_cur = lane_center
# "feature" existente aqui: como nao tem uma condicao de parada
# quando a linha acabar a baixa continuara procurando seguindo verticalmente
# no ultimo x. Talvez seja bom para a pista com faixa pontilhada, mas talvez
# de problema....
# solucao: caso nao ter nenhum ponto brilhante, subir n posicoes, se nao houver
# um ponto brilhante, acaba
# move window to the next line
y_pos_cur = y_pos_cur - 1
# search down
x_pos_cur = x_lane_start
y_pos_cur = y_lane_start + 1
points_found = []
while y_pos_cur <= self.filter_param.roi_1_y:
x_box_start = x_pos_cur - int(
self.filter_param.search_box_w / 2)
x_box_end = x_pos_cur + int(self.filter_param.search_box_w / 2)
# get pixels from a search box
hor_line = image[y_pos_cur, x_box_start:x_box_end]
# real index of a hor_line pixel in the image
hor_line_image_index = [
i for i in range(x_box_start, x_box_end)
]
for index, pixel in enumerate(hor_line):
# store bright pixels
if pixel == 255:
points_found.append(index)
if len(points_found) > 0:
lane_center = hor_line_image_index[int(
np.median(points_found))]
new_lane_point = np.array((lane_center, y_pos_cur))
points_found = []
# append a new point to the list
lane.add_point(new_lane_point, len(points_found))
# x start point for the next search
x_pos_cur = lane_center
# "feature" existente aqui: como nao tem uma condicao de parada
# quando a linha acabar a baixa continuara procurando seguindo verticalmente
# no ultimo x. Talvez seja bom para a pista com faixa pontilhada, mas talvez
# de problema....
# solucao: caso nao ter nenhum ponto brilhante, subir n posicoes, se nao houver
# um ponto brilhante, acaba
# move window to the next line
y_pos_cur = y_pos_cur + 1
for lane in lanes_list:
min_p, max_p = lane.distance_y()
f = lane.get_function()
#for i in range(self.filter_param.roi_0_y, self.filter_param.roi_1_y):
for i in range(0, 399):
x = int(f(i))
y = i
cv2.circle(image, (x, y), 3, 255, 1)
cv2.rectangle(image,
(self.filter_param.roi_0_x, self.filter_param.roi_0_y),
(self.filter_param.roi_1_x, self.filter_param.roi_1_y),
255, 2)
cv2.imshow("rect", image)
'--image',
required=True,
help='Path to the input image containing the logo/image')
ap.add_argument('-M',
'--match',
help='Type of matching algorithm to use',
default='F')
args = vars(ap.parse_args())
#Getting the images ready
img1 = cv2.imread(args['logo'], cv2.IMREAD_GRAYSCALE)
img2 = cv2.imread(args['image'], cv2.IMREAD_GRAYSCALE)
match_type = args['match']
img_matches = None
if match_type == 'B':
#Matching the images using the Brute-Force algorithm.
img_matches = BruteForce(img1, img2)
elif match_type == 'R':
#Matching the images using BruteForce KNN algorithm with Ratio Test.
img_matches = BruteForceKNN(img1, img2)
elif match_type == 'F':
img_matches = FLANN(img1, img2)
elif match_type == 'H':
img_matches = Homography(img1, img2)
#Displaying the results and storing them
plt.imshow(img_matches)
plt.show()
cv2.imwrite('matched.png', img_matches)
size = 500
if __name__ == "__main__":
cap = cv2.VideoCapture(f'./{file}')
with open(f'calib_{file}.json') as f:
calib = json.load(f)
image_points = np.array(calib["image"])
map_points = np.array(calib["map"])
dist_coords = calib["dist"]
scale = calib["scale"]
homography = Homography(
image_points,
size * (scale / 2) + size * scale + map_points * size * scale)
dist_coords = [
homography.project(dist_coords[0]),
homography.project(dist_coords[1])
]
min_dist = np.linalg.norm(dist_coords[0] - dist_coords[1])
frame_idx = 0
while True:
r, img = cap.read()
img = cv2.resize(img, (1280, 720))
if frame_idx == 0:
def get_image_homography(image_file):
puntos = []
imagen = openCv.imread(image_file)
homography = Homography(puntos, imagen)
imagenH = homography.getHomography()
return homography.getPuntos()
del parameters
if __name__ == "__main__":
global homography
rospy.init_node('find_homography')
rospy.loginfo("Starting find_homography.py")
if len(sys.argv) < 2:
rospy.loginfo("Error in find_homography!")
rospy.loginfo("Cant find json file!")
exit(1)
file_path = sys.argv[1]
homography = Homography(file_path)
# create trackbar and set values
create_trackbar()
'''
# set world points
world_points = [
[000.0, 800.0],
[400.0, 800.0],
[400.0, 600.0],
[000.0, 600.0]]
'''
'''
#Gazebo
scale = 8
tcp = TCP_to_cam()
# img1 = cv2.imread(os.path.join(dirname, 'pose1.jpg'))
# pnts1 = tcp.read_image(img1, scale)
#TCP1
tcp0 = np.float32([[329, 288.9]])
pnts1 = np.float32([[177.5, 394]])
pnts2 = np.float32([[221, 386.5]])
pnts3 = np.float32([[294.5, 376.5]])
pnts4 = np.float32([[368, 354]])
pnts5 = np.float32([[431.5, 322]])
pnts6 = np.float32([[504.5, 290.5]])
pnts7 = np.float32([[562, 250.5]])
h = Homography()
# hom = np.float32([[0.341, -0.002, -2.588],
# [-0.002, 0.322, -2.407],
# [0.000, 0.000, 1.000]])
hom = np.float32([[0, -0.00814, 512*0.00814],
[0.00814, 0, 0],
[0, 0, 1.00000]])
pnts1 = h.transform(hom, pnts1)
pnts2 = h.transform(hom, pnts2)
pnts3 = h.transform(hom, pnts3)
pnts4 = h.transform(hom, pnts4)
pnts5 = h.transform(hom, pnts5)
pnts6 = h.transform(hom, pnts6)
pnts7 = h.transform(hom, pnts7)
if len(points) > 1:
_, speed_av = get_weighted_speed(frames, points, hom)
output = get_momentum_speeds(frames, points, hom)
speed_median = output[-1]
return speed_av, speed_median
else:
return 0, 0
if __name__ == "__main__":
# res_file = open('../video_cruise_control/second_experiment/temp/speed_file', 'w')
# with open(path_to_targets, 'r') as targets_file:
# lines = targets_file.read().splitlines()
# targets = dict((x.split(' ')[0], float(x.split(' ')[1])) for x in lines)
hom = Homography(np.array(pts_src_), np.array(pts_real_))
plate_cascade = cv.CascadeClassifier(path_to_cascade)
caffe.set_mode_cpu()
net = caffe.Net(path_to_model, path_to_weights, caffe.TEST)
# videos = []
# timestamps = []
# files = os.listdir(path_to_video_and_timestamp)
# for f in files:
# if not f.endswith('_timestamps') and f.endswith('mp4'):
# videos.append(f)
# timestamps.append(f + '_timestamps')
# v_t = dict(zip(videos, timestamps))
# v_t = {'1.mp4':'', '3.mp4':'', '5.mp4':'', '7.mp4':'', '9.mp4':'', '11.mp4':''}
# v_t = {'2.mp4':'', '4.mp4':'', '6.mp4':'', '8.mp4':'', '10.mp4':'', '12.mp4':''}
class LaneDetector():
POS_OFF_ROAD_RIGHT = 0
POS_ON_ROAD_RIGHT = 1
POS_ON_ROAD_LEFT = 2
POS_OFF_ROAD_LEFT = 3
def __init__(self, homography_file, filter_file, tune_param):
self.bridge = CvBridge()
# load homography matrix
self.homography_file = str(homography_file)
self.homography = Homography(self.homography_file)
self.homography_matrix = self.homography.get_homography_matrix()
if self.homography_matrix is None:
print("File doesnt have a homography matrix")
print("Run find_homography script")
# load parameters used in the filter process
self.filter_file = filter_file
self.filter_param = Parameters(filter_file)
self.tune_param = tune_param
# configure parameters
if tune_param:
self.filter_param.create_trackbar()
if os.path.exists(self.filter_file):
self.filter_param.load()
self.filter_param.set_trackbar_values()
# autonomous mode
else:
if os.path.exists(self.filter_file):
self.filter_param.load()
else:
print("Filter file doesnt exist")
exit(1)
self.init = True
#
self.filter_res = 0
self.image = None
self.warp_res = None
self.color_res = None
self.filter_res = None
self.lanes_list = None
def debug(self):
while True:
try:
#os.system('clear')
#print("Numero de linhas: " + str(len(self.lanes_list)))
for i, lane in enumerate(self.lanes_list):
distance = lane.distance_y()
#print("--> " + str(i) + " width: " + str(lane.get_avg_width()) + " len: " + str(lane.get_len_point()))
f = lane.get_function()
for j in range(self.filter_param.roi_0_y,
self.filter_param.roi_1_y):
#for j in range(0, 399):
x = int(f(j))
y = j
cv2.circle(self.warp_res, (x, y), 3, 0, 1)
cv2.rectangle(
self.warp_res,
(self.filter_param.roi_0_x, self.filter_param.roi_0_y),
(self.filter_param.roi_1_x, self.filter_param.roi_1_y),
255, 2)
cv2.imshow("image", self.image)
cv2.imshow("warp_res", self.warp_res)
cv2.imshow("color_res", self.color_res)
cv2.imshow("filter_res", self.filter_res)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cv2.destroyAllWindows()
break
elif key == ord('l'):
self.filter_param.load()
self.filter_param.set_trackbar_values()
elif key == ord('s'):
self.filter_param.save(self.filter_param)
self.filter_param.update_trackbar_values()
except:
pass
def process(self, compressed_image):
start = time.time()
self.image = self.bridge.compressed_imgmsg_to_cv2(
compressed_image, "bgr8")
#self.image = compressed_image.copy()
self.color_res = cv2.cvtColor(self.image.copy(), cv2.COLOR_BGR2GRAY)
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
contrast = clahe.apply(self.color_res)
self.warp_res = cv2.warpPerspective(contrast, self.homography_matrix,
(400, 400))
self.filter_res = self.filter(self.warp_res)
self.lanes_list = self.find_lanes(self.filter_res.copy())
good_lanes = []
# delete small lanes
for lane in self.lanes_list:
if lane.get_len_point() > 15:
good_lanes.append(lane)
self.lanes_list = good_lanes
# classify the road
lane_status = LaneInfo()
img_h, img_w = self.warp_res.shape
img_w_center = img_w / 2.0
y_react_point = self.filter_param.roi_1_y
num_lanes = len(self.lanes_list)
if num_lanes == 0:
lane_status.lane_type = 0
return lane_status
elif num_lanes == 1:
lane_status.lane_type = 255
lane = self.lanes_list[0]
lane.distance_y()
max_p = lane.get_min_vertical()
min_p = lane.get_max_vertical()
#print(min_p, max_p)
m_coef = -(min_p[1] - max_p[1]) / float((min_p[0] - max_p[0]))
angle = np.arctan(m_coef) * 180 / np.pi
#print(min_p, max_p)
print("angle: " + str(angle))
#raw_input()
x_react_point = min_p[0]
y_react_point = max_p[1]
#print(x_react_point, y_react_point)
#print(img_w_center-x_react_point)
# left side, off the road
if angle <= 80 and angle >= 0:
lane_status.lane_type = 0
s_angle = 90 - angle
s_offset = 80 - (img_w_center - x_react_point)
lane_status.lane_curvature = s_angle
lane_status.lane_offset = s_angle + s_offset
print(lane_status.lane_offset)
print("direeeeeeeeeeeeeeeeeeeeeeeita")
elif angle >= -80 and angle <= 0:
lane_status.lane_type = 0
s_angle = -(90 + angle)
s_offset = -(60 + (img_w_center - x_react_point))
lane_status.lane_curvature = s_angle
lane_status.lane_offset = s_angle + s_offset
print(lane_status.lane_offset)
print("esqueeeeeeeeeeeeeeeeeeeerda")
else:
print("reeeeeto")
pass
elif num_lanes == 2:
lane_status.lane_type = 1
angle_list = []
x_lane_position = 0
for lane in self.lanes_list:
lane.distance_y()
f = lane.get_function()
x_lane_position += f(self.filter_param.roi_1_y)
max_p = lane.get_min_vertical()
min_p = lane.get_max_vertical()
#print(min_p, max_p)
m_coef = -(min_p[1] - max_p[1]) / float((min_p[0] - max_p[0]))
angle = np.arctan(m_coef) * 180 / np.pi
angle_list.append(abs(angle))
lane_status.lane_curvature = min(angle_list)
lane_status.lane_offset = ((x_lane_position) / 2.0) - img_w_center
print(lane_status.lane_offset)
elif num_lanes == 3:
lane_status.lane_type = 2
good_lanes = []
for i in range(len(self.lanes_list) - 1):
f1 = self.lanes_list[i].get_function()
f2 = self.lanes_list[i + 1].get_function()
if abs(
f1(self.filter_param.roi_1_y) -
f2(self.filter_param.roi_1_y)) > 10:
good_lanes.append(self.lanes_list[i])
good_lanes.append(self.lanes_list[-1])
angle_list = []
x_lane_position = 0
for lane in good_lanes:
lane.distance_y()
f = lane.get_function()
x_lane_position += f(self.filter_param.roi_1_y)
max_p = lane.get_min_vertical()
min_p = lane.get_max_vertical()
#print(min_p, max_p)
m_coef = -(min_p[1] - max_p[1]) / float((min_p[0] - max_p[0]))
angle = np.arctan(m_coef) * 180 / np.pi
angle_list.append(abs(angle))
lane_status.lane_curvature = min(angle_list)
lane_status.lane_offset = ((x_lane_position) / 2.0) - img_w_center
print(lane_status.lane_offset)
elif num_lanes == 4:
lane_status.lane_type = 2
periodo = time.time() - start
fps = 1 / periodo
#print("FPS: " + str(fps))
def filter(self, image):
ret, thr = cv2.threshold(image, self.filter_param.gray_lower_bound,
255, cv2.THRESH_BINARY)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
filtered = cv2.morphologyEx(thr, cv2.MORPH_OPEN, kernel)
return filtered
def find_lanes(self, image):
'''
top-left = roi_0_x, roi_0_y
bot-right = roi_1_x, roi_1_y
'''
# image size
img_h, img_w = image.shape
# initial search position y
#y_pos_init = self.filter_param.roi_0_y
# random initial position of y
y_pos_init = random.randint(self.filter_param.roi_0_y,
self.filter_param.roi_1_y + 1)
# get entire horizontal line for the first scan of lanes
hor_line = image[y_pos_init, :]
# list of lanes... lane is a class with points, width ...
lanes_list = []
# store temporary points for a interesting area of the horizontal search
points_found = []
for index, pixel in enumerate(hor_line):
if pixel == 255:
points_found.append(index)
elif pixel == 0 and len(points_found) > 0:
# after to collect all sequence of bright pixels, get the median
new_lane_center_point = np.array(
(int(np.median(points_found)), y_pos_init))
# create a Lane obj and add to the list of lanes
lanes_list.append(
Lane(new_lane_center_point, len(points_found)))
points_found = []
# from the points found in the first search, find all points connected to this lane
# using sliding window with a box of fixed size
for lane in lanes_list:
# get last point found of a lane
last_point = lane.get_last_point()
x_lane_start = last_point[0]
y_lane_start = last_point[1]
# search up
x_pos_cur = x_lane_start
y_pos_cur = y_lane_start - self.filter_param.search_box_h
points_found = []
while y_pos_cur >= self.filter_param.roi_0_y:
x_box_start = x_pos_cur - int(
self.filter_param.search_box_w / 2)
x_box_end = x_pos_cur + int(self.filter_param.search_box_w / 2)
# get pixels from a search box
hor_line = image[y_pos_cur, x_box_start:x_box_end]
# real index of a hor_line pixel in the image
hor_line_image_index = [
i for i in range(x_box_start, x_box_end)
]
for index, pixel in enumerate(hor_line):
# store bright pixels
if pixel == 255:
points_found.append(index)
if len(points_found) > 0:
lane_center = hor_line_image_index[int(
np.median(points_found))]
new_lane_point = np.array((lane_center, y_pos_cur))
# append a new point to the list
lane.add_point(new_lane_point, len(points_found))
# x start point for the next search
x_pos_cur = lane_center
points_found = []
# "feature" existente aqui: como nao tem uma condicao de parada
# quando a linha acabar a baixa continuara procurando seguindo verticalmente
# no ultimo x. Talvez seja bom para a pista com faixa pontilhada, mas talvez
# de problema....
# solucao: caso nao ter nenhum ponto brilhante, subir n posicoes, se nao houver
# um ponto brilhante, acaba
# if not was found following the lane
else:
break
# move window to the next line
y_pos_cur = y_pos_cur - self.filter_param.search_box_h
# search down
x_pos_cur = x_lane_start
y_pos_cur = y_lane_start + self.filter_param.search_box_h
points_found = []
while y_pos_cur <= self.filter_param.roi_1_y:
x_box_start = x_pos_cur - int(
self.filter_param.search_box_w / 2)
x_box_end = x_pos_cur + int(self.filter_param.search_box_w / 2)
# get pixels from a search box
hor_line = image[y_pos_cur, x_box_start:x_box_end]
# real index of a hor_line pixel in the image
hor_line_image_index = [
i for i in range(x_box_start, x_box_end)
]
for index, pixel in enumerate(hor_line):
# store bright pixels
if pixel == 255:
points_found.append(index)
if len(points_found) > 0:
lane_center = hor_line_image_index[int(
np.median(points_found))]
new_lane_point = np.array((lane_center, y_pos_cur))
# append a new point to the list
lane.add_point(new_lane_point, len(points_found))
# x start point for the next search
x_pos_cur = lane_center
points_found = []
# "feature" existente aqui: como nao tem uma condicao de parada
# quando a linha acabar a baixa continuara procurando seguindo verticalmente
# no ultimo x. Talvez seja bom para a pista com faixa pontilhada, mas talvez
# de problema....
# solucao: caso nao ter nenhum ponto brilhante, subir n posicoes, se nao houver
# um ponto brilhante, acaba
# if not was found following the lane
else:
break
# move window to the next line
y_pos_cur = y_pos_cur + self.filter_param.search_box_h
return lanes_list
def __init__(self, name, roi, chessboard_size, flip_src, fps_ratio):
self.name = name
self.roi = roi
# raw videos
self.src_vid_paths = sorted(glob.glob(SRC_VID_FORMAT.format(name)))
self.dst_vid_paths = sorted(glob.glob(DST_VID_FORMAT.format(name)))
if len(self.src_vid_paths) != len(self.dst_vid_paths):
raise ValueError('Number of sharp and blur videos are not equal!')
print(f'{len(self.src_vid_paths)} videos found!')
# Flip the images of the left camera
self.flip_src = flip_src
# Ratio of sharp fps and blur fps
self.fps_ratio = fps_ratio
# crop center
self.cropper = Cropper(roi)
# Read and cache all calibration data
# homography data
hom_src_path = HOM_SRC_FORMAT.format(name)
hom_dst_path = HOM_DST_FORMAT.format(name)
if (not osp.isfile(hom_src_path)) or (not osp.isfile(hom_dst_path)):
raise ValueError('Missing homography data!')
hom_src = cv2.imread(hom_src_path)
hom_dst = cv2.imread(hom_dst_path)
# color correction data
cc_src_paths = sorted(glob.glob(COLOR_CORRECT_SRC_FORMAT.format(name)))
cc_dst_paths = sorted(glob.glob(COLOR_CORRECT_DST_FORMAT.format(name)))
if (len(cc_src_paths) != len(cc_dst_paths)) or (
len(cc_src_paths) != len(self.src_vid_paths)):
raise ValueError('Number of color correction frames is not valid!')
self.ccs_src = [
cv2.imread(cc_src_path) for cc_src_path in cc_src_paths
]
self.ccs_dst = [
cv2.imread(cc_dst_path) for cc_dst_path in cc_dst_paths
]
# Sharp videos captured from beamsplitter-based system are flipped
if self.flip:
hom_src = self.flip(hom_src)
self.ccs_src = [self.flip(ccs) for ccs in self.ccs_src]
# Initialize calibration tool
# Note:
# homography: sharp -> blur
# color correction: blur -> sharp
self.warper = Homography(hom_src, hom_dst, chessboard_size)
cv2.imwrite('debug1.png', self.crop(self.warp(self.ccs_src[0])))
cv2.imwrite('debug2.png', self.crop(self.ccs_dst[0]))
self.ccs_src = [self.warp(ccs) for ccs in self.ccs_src]
# remove black margins of the camera
self.ccs_src = [self.crop(ccs) for ccs in self.ccs_src]
self.ccs_dst = [self.crop(ccs) for ccs in self.ccs_dst]
self.color_transformer = [
MyColorCorrection(cc_dst, cc_src)
for cc_dst, cc_src in zip(self.ccs_dst, self.ccs_src)
]
def test_shift(self):
translation = np.array([2, 3])
h = Homography.translation(translation[0], translation[1])
shift = h.get_shift_at_point(np.array([0, 0]))
self.assertTrue(np.array_equal(shift, translation))
class LaneDetector():
POS_OFF_ROAD_RIGHT = 0
POS_ON_ROAD_RIGHT = 1
POS_ON_ROAD_LEFT = 2
POS_OFF_ROAD_LEFT = 3
def __init__(self, homography_file, filter_file, tune_param):
# load homography matrix
self.homography_file = str(homography_file)
self.homography = Homography(self.homography_file)
self.homography_matrix = self.homography.get_homography_matrix()
if self.homography_matrix is None:
print("File doesnt have a homography matrix")
print("Run find_homography script")
# load parameters used in the filter process
self.filter_file = filter_file
self.filter_param = Parameters(filter_file)
self.tune_param = tune_param
# configure parameters
if tune_param:
self.filter_param.create_trackbar()
if os.path.exists(self.filter_file):
self.filter_param.load()
self.filter_param.set_trackbar_values()
# autonomous mode
else:
if os.path.exists(self.filter_file):
self.filter_param.load()
else:
print("Filter file doesnt exist")
exit(1)
self.init = True
#
self.filter_res = 0
self.image = None
self.warp_res = None
self.color_res = None
self.filter_res = None
self.lanes_list = None
def process(self, compressed_image):
start = time.time()
#self.image = self.bridge.compressed_imgmsg_to_cv2(compressed_image, "bgr8")
self.image = compressed_image.copy()
self.color_res = cv2.cvtColor(self.image.copy(), cv2.COLOR_BGR2GRAY)
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
contrast = clahe.apply(self.color_res)
self.warp_res = cv2.warpPerspective(contrast, self.homography_matrix,
(400, 400))
self.filter_res = self.filter(self.warp_res)
self.lanes_list = self.find_lanes(self.filter_res.copy())
periodo = time.time() - start
fps = 1 / periodo
print("FPS: " + str(fps))
good_lanes = []
# delete small lanes
for lane in self.lanes_list:
if lane.get_len_point() > 15:
good_lanes.append(lane)
self.lanes_list = good_lanes
for i, lane in enumerate(self.lanes_list):
distance = lane.distance_y()
#print("--> " + str(i) + " width: " + str(lane.get_avg_width()) + " len: " + str(lane.get_len_point()))
f = lane.get_function()
#for j in range(self.filter_param.roi_0_y, self.filter_param.roi_1_y):
# x = int(f(j))
# y = j
# cv2.circle(self.warp_res, (x, y), 3, 255, 1)
points = lane.get_points()
for p in points:
cv2.circle(self.warp_res, (p[0], p[1]), 3, 0, 1)
pass
cv2.rectangle(
self.warp_res,
(self.filter_param.roi_0_x, self.filter_param.roi_0_y),
(self.filter_param.roi_1_x, self.filter_param.roi_1_y), 255, 2)
p1 = np.array((int(f(self.filter_param.roi_1_y)),
int(self.filter_param.roi_1_y)))
p2 = np.array(
(int(
f((self.filter_param.roi_1_y + self.filter_param.roi_0_y) /
2)),
int((self.filter_param.roi_1_y + self.filter_param.roi_0_y) /
2)))
p3 = np.array((int(f(self.filter_param.roi_0_y)),
int(self.filter_param.roi_0_y)))
#cv2.circle(self.warp_res, p2, 3, 0, 1)
d_x1 = (p2[1] - p1[1])
d_y1 = float(p2[0] -
p1[0]) if float(p2[0] - p1[0]) != 0 else 0.0000001
m1 = d_x1 / d_y1
d_x2 = (p3[1] - p2[1])
d_y2 = float(p3[0] -
p2[0]) if float(p3[0] - p2[0]) != 0 else 0.0000001
m2 = d_x2 / d_y2
dx_dy = (m1 + m2) / 2.0
x_mid1 = (p2[0] + p1[0]) / 2.0
x_mid2 = (p3[0] + p2[0]) / 2.0
delta_x_mid = float(x_mid1 -
x_mid2) if float(x_mid1 -
x_mid2) != 0 else 0.0000001
dx2_dy2 = (m2 - m1) / delta_x_mid
radius = ((1 + (dx_dy)**2)**(3 / 2.0)) / abs(dx2_dy2)
#print(p1, p2, p3)
#print(m1, m2)
print(radius)
cv2.imshow("image", self.image)
cv2.imshow("warp_res", self.warp_res)
cv2.imshow("color_res", self.color_res)
cv2.imshow("filter_res", self.filter_res)
cv2.imshow("contrast", contrast)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
exit(1)
elif key == ord('l'):
self.filter_param.load()
self.filter_param.set_trackbar_values()
elif key == ord('s'):
self.filter_param.save(self.filter_param)
self.filter_param.update_trackbar_values()
def filter(self, image):
ret, thr = cv2.threshold(image, self.filter_param.gray_lower_bound,
255, cv2.THRESH_BINARY)
#ret, thr = cv2.threshold(image, self.filter_param.gray_lower_bound, self.filter_param.gray_upper_bound, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
# talvez aplicar sobel(gradient) + threshould
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
opening = cv2.morphologyEx(thr, cv2.MORPH_OPEN, kernel)
filtered = opening
return filtered
def find_lanes(self, image):
'''
top-left = roi_0_x, roi_0_y
bot-right = roi_1_x, roi_1_y
'''
# image size
img_h, img_w = image.shape
# initial search position y
#y_pos_init = self.filter_param.roi_0_y
# random initial position of y
y_pos_init = random.randint(self.filter_param.roi_0_y,
self.filter_param.roi_1_y + 1)
# get entire horizontal line for the first scan of lanes
hor_line = image[y_pos_init, :]
# list of lanes... lane is a class with points, width ...
lanes_list = []
# store temporary points for a interesting area of the horizontal search
points_found = []
for index, pixel in enumerate(hor_line):
if pixel == 255:
points_found.append(index)
elif pixel == 0 and len(points_found) > 0:
# after to collect all sequence of bright pixels, get the median
new_lane_center_point = np.array(
(int(np.median(points_found)), y_pos_init))
# create a Lane obj and add to the list of lanes
lanes_list.append(
Lane(new_lane_center_point, len(points_found)))
points_found = []
# from the points found in the first search, find all points connected to this lane
# using sliding window with a box of fixed size
for lane in lanes_list:
# get last point found of a lane
last_point = lane.get_last_point()
x_lane_start = last_point[0]
y_lane_start = last_point[1]
# search up
x_pos_cur = x_lane_start
y_pos_cur = y_lane_start - self.filter_param.search_box_h
points_found = []
while y_pos_cur >= self.filter_param.roi_0_y:
x_box_start = x_pos_cur - int(
self.filter_param.search_box_w / 2)
x_box_end = x_pos_cur + int(self.filter_param.search_box_w / 2)
# get pixels from a search box
hor_line = image[y_pos_cur, x_box_start:x_box_end]
# real index of a hor_line pixel in the image
hor_line_image_index = [
i for i in range(x_box_start, x_box_end)
]
for index, pixel in enumerate(hor_line):
# store bright pixels
if pixel == 255:
points_found.append(index)
if len(points_found) > 0:
lane_center = hor_line_image_index[int(
np.median(points_found))]
new_lane_point = np.array((lane_center, y_pos_cur))
# append a new point to the list
lane.add_point(new_lane_point, len(points_found))
# x start point for the next search
x_pos_cur = lane_center
points_found = []
# "feature" existente aqui: como nao tem uma condicao de parada
# quando a linha acabar a baixa continuara procurando seguindo verticalmente
# no ultimo x. Talvez seja bom para a pista com faixa pontilhada, mas talvez
# de problema....
# solucao: caso nao ter nenhum ponto brilhante, subir n posicoes, se nao houver
# um ponto brilhante, acaba
# if not was found following the lane
else:
break
# move window to the next line
y_pos_cur = y_pos_cur - self.filter_param.search_box_h
# search down
x_pos_cur = x_lane_start
y_pos_cur = y_lane_start + self.filter_param.search_box_h
points_found = []
while y_pos_cur <= self.filter_param.roi_1_y:
x_box_start = x_pos_cur - int(
self.filter_param.search_box_w / 2)
x_box_end = x_pos_cur + int(self.filter_param.search_box_w / 2)
# get pixels from a search box
hor_line = image[y_pos_cur, x_box_start:x_box_end]
# real index of a hor_line pixel in the image
hor_line_image_index = [
i for i in range(x_box_start, x_box_end)
]
for index, pixel in enumerate(hor_line):
# store bright pixels
if pixel == 255:
points_found.append(index)
if len(points_found) > 0:
lane_center = hor_line_image_index[int(
np.median(points_found))]
new_lane_point = np.array((lane_center, y_pos_cur))
# append a new point to the list
lane.add_point(new_lane_point, len(points_found))
# x start point for the next search
x_pos_cur = lane_center
points_found = []
# "feature" existente aqui: como nao tem uma condicao de parada
# quando a linha acabar a baixa continuara procurando seguindo verticalmente
# no ultimo x. Talvez seja bom para a pista com faixa pontilhada, mas talvez
# de problema....
# solucao: caso nao ter nenhum ponto brilhante, subir n posicoes, se nao houver
# um ponto brilhante, acaba
# if not was found following the lane
else:
break
# move window to the next line
y_pos_cur = y_pos_cur + self.filter_param.search_box_h
return lanes_list
E = [3621, 2240]
F = [480, 2632]
K = [2404, 1294]
L = [2877, 1657]
M = [3440, 904]
AB = 10.473
BE = 16.88
BC = 11.940
CD = 11.120
DE = 16.983
EF = 7.266
FA = 11.472
dists = [AB, BC, CD, DE, EF, FA]
hom = Homography(np.array(pts_src_), np.array(pts_real_))
# print(hom.get_point_transform_2(hom.h, B, C))
# print(hom.get_point_transform_2(hom.h, C, D))
# print(hom.get_point_transform_2(hom.h, D, E))
# print(hom.get_point_transform_2(hom.h, B, E))
homs = hom.homs
errs = []
for h in homs:
err = 0
# err += AB - hom.get_point_transform_2(h[0], A, B)
# err += BE - hom.get_point_transform_2(h[0], B, E)
err += abs(BC - hom.get_point_transform_2(h[0], B, C))
err += abs(BE - hom.get_point_transform_2(h[0], B, E))
