from logic.inverse_perspective import InversePerspective import pandas as pd import os import cv2 import numpy as np import quaternion from logic.mapper import Mapper, calculate_position def row_to_quaternion(row): return np.quaternion(row.q4, row.q1, row.q2, row.q3) if __name__ == '__main__': cal_data = r"C:\repositories\autonomic_car\selfie_car\src\pc\settings\camera_calibration\calib.json" cal = VideoCalibration(cal_data, (320, 240)) dirpath = r"C:\repositories\autonomic_car\selfie_car\data_intake4\v1.011" perspective = InversePerspective(img_shape=(250, 250), desired_shape=(80, 160), marigin=0.25) mapper = Mapper(map_size=(1200, 1200), scale=300.0, type='additive') df = pd.read_csv(os.path.join(dirpath, 'vis_v1.csv')) df = df[~df['filenames'].isnull()] columns = ['time', 'x', 'y', 'z', 'q1', 'q2', 'q3', 'q4'] slam_df = pd.read_csv(os.path.join(dirpath, 'slam_trajectory.csv'), sep=' ', names=columns) # imu_df = calculate_position(df, dirpath) for i, row in slam_df.iterrows(): #if pd.isnull(row['filenames']): # continue
#proc = cv2.fastNlMeansDenoising(proc, None, 2, 4, 5) #cv2.imshow('denoise', proc) #proc = cv2.equalizeHist(proc,) #proc = gamma_correction(proc, 0.9) #kernel = np.array([[-1, -1, -1], [-1, 9, -1], [-1, -1, -1]]) #proc = cv2.filter2D(proc, -1, kernel) #cv2.imshow('equal', proc) #proc = cv2.adaptiveThreshold(proc, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 15, 2) #cv2.imshow('adap', proc) #proc = cv2.Canny(proc, 20, 150, apertureSize=3) #cv2.imshow('edge', proc) return proc if __name__=='__main__': cal_data = r"C:\repositories\autonomic_car\selfie_car\src\pc\settings\camera_calibration\calib.json" cal = VideoCalibration(cal_data, (320, 240)) dirpath = r"C:\Users\hawker\Dropbox\Public\selfie_car\data_intake3\v1.23" perspective = InversePerspective(img_shape=(250,250), desired_shape=(80,160), marigin=0.25) import os import pandas as pd pd.read_csv(os.path.join(dirpath,'vis_v1.csv')) images = os.listdir(dirpath) images = filter(lambda x: ".jpg" in x, images) images = map(lambda x: os.path.join(dirpath, x), images) images = list(images) bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) orb = cv2.ORB_create(nfeatures=5000, scaleFactor=2., )#, edgeThreshold=17, patchSize=64) vx_list = [] vy_list = [] for i in range(1,len(images)): img_path_1 = images[i]
if __name__ == '__main__': ## @MentalMap # Calibration is needed to undistort # Perspective reverses the image parser = ArgumentParser() parser.add_argument('calibration', help="path to json calibration file") parser.add_argument('map_path', help="path to maps yaml file") parser.add_argument('images_path', help="path to folder with images") #'/home/mwm/Desktop/datadumps/01-07-19-drive-v1_22' parser.add_argument('assoc_file', help="path to assoc.json file") #'./assets/maps/testmap1/assoc.json' args = parser.parse_args() img_shape = 320,240 calibration = VideoCalibration(args.calibration, img_shape, new_shape=img_shape) mapper = Mapper2(map_shape=(800,800,3), type='additive') perspective = InversePerspective(img_shape=(250, 250), desired_shape=(100, 130), marigin=0.25) id_and_pose = get_poses(args.map_path) xy_transformation = Pose3Dto2D([idp[1] for idp in id_and_pose]) for kf_id, pose, ts, filename, img in data_iterator(id_and_pose, args.images_path, args.assoc_file): if True: cv2.imshow('img',img) undi = calibration.undistort(img) undi[:100,:,:] = 0 img = perspective.inverse(undi) cv2.imshow('undi', img) cv2.waitKey(0) new_pose = xy_transformation.transform.dot(pose) new_angles = transform.Rotation.from_dcm(new_pose[:3,:3]).as_euler('xyz') position = new_pose[:2,3]
).astype(np.float32) if __name__ == "__main__": ## @MentalMap # Calibration is needed to undistort # Perspective reverses the image parser = ArgumentParser() parser.add_argument("calibration", help="path to json calibration file") parser.add_argument("map_path", help="path to maps yaml file") parser.add_argument("images_path", help="path to folder with images") #'/home/mwm/Desktop/datadumps/01-07-19-drive-v1_22' parser.add_argument("assoc_file", help="path to assoc.json file") #'./assets/maps/testmap1/assoc.json' args = parser.parse_args() img_shape = 320, 240 calibration = VideoCalibration(args.calibration, img_shape, new_shape=img_shape) mapper = Mapper2( map_shape=(800, 800, 3), coord_coef=((50, 400), (150, 400)), rot_cent_coef=((0.5, 0), (1.1, 0)), type="additive", ) perspective = InversePerspective( perspective_area=HAXOR_SRC3, img_shape=(250, 250), desired_shape=(100, 130), marigin=0.25, ) perspective.calculate_roi_mask() my_osmap = OsmapData.from_map_path(args.map_path) id_and_pose = my_osmap.id_and_pose()