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))