From e3110888a6c4c7f48cf739e4316d5e89dd678b3e Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sat, 23 Jan 2021 12:54:25 +0100 Subject: [PATCH 01/13] provaprova --- PROVA.txt | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 PROVA.txt diff --git a/PROVA.txt b/PROVA.txt new file mode 100644 index 0000000..0ea7595 --- /dev/null +++ b/PROVA.txt @@ -0,0 +1,2 @@ +x +provaprova From 0e04ddb08b455ff3cb6c180828924c93d298594e Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sat, 23 Jan 2021 13:06:10 +0100 Subject: [PATCH 02/13] provaproca2 --- GabHoo_settings.yaml | 28 +++ PROVA.txt | 2 - run2.py | 197 ++++++++++++++++ run_GabHoo.py | 211 ++++++++++++++++++ .../Settings/GabHoo_camera_settings.yaml | 64 ++++++ utils.py | 38 +++- 6 files changed, 537 insertions(+), 3 deletions(-) create mode 100644 GabHoo_settings.yaml delete mode 100644 PROVA.txt create mode 100644 run2.py create mode 100644 run_GabHoo.py create mode 100644 slam_method/Settings/GabHoo_camera_settings.yaml diff --git a/GabHoo_settings.yaml b/GabHoo_settings.yaml new file mode 100644 index 0000000..4eacff6 --- /dev/null +++ b/GabHoo_settings.yaml @@ -0,0 +1,28 @@ +SLAM.alg: "OrbSlam3" +#-------------------------------------------------------------------------------------------- +# SLAM Method: change with the name of the .py file in whitch there are the class SLAM +#-------------------------------------------------------------------------------------------- + +#-------------------------------------------------------------------------------------------- +# SLAM Params: add your own to run the algorithm +#-------------------------------------------------------------------------------------------- +SLAM.vocab_path: "slam_method/Settings/ORBvoc.bin" +SLAM.settings_path: "slam_method/Settings/GabHoo_camera_settings.yaml" + +#-------------------------------------------------------------------------------------------- +# Drawer Params: the extra params of trajectory_drawer class +# Params Guide https://slampy.readthedocs.io/en/latest/config.html +#-------------------------------------------------------------------------------------------- + +Drawer.eye.x: -18.0 +Drawer.eye.y: -13.0 +Drawer.eye.z: -55.0 +Drawer.center.x: -17.0 +Drawer.center.y: -8.0 +Drawer.scale_grade.x: 1.0 +Drawer.scale_grade.y: 1.0 +Drawer.scale_grade.z: 10.0 +Drawer.aspectratio.x: 50 +Drawer.aspectratio.y: 50 +Drawer.aspectratio.z: 100 +Drawer.point_size: 2 diff --git a/PROVA.txt b/PROVA.txt deleted file mode 100644 index 0ea7595..0000000 --- a/PROVA.txt +++ /dev/null @@ -1,2 +0,0 @@ -x -provaprova diff --git a/run2.py b/run2.py new file mode 100644 index 0000000..69a639c --- /dev/null +++ b/run2.py @@ -0,0 +1,197 @@ +import os +import cv2 +import numpy as np +import slampy +import argparse +from tqdm import tqdm +from utils import * +from kitti_odometry import KittiEvalOdom + + +def run(args): + """Run SLAM system for each frame in the dataset and save depths and poses. + + Args: + args: command line arguments + """ + setting_file = args.settings + if not os.path.exists(setting_file): + raise ValueError(f"Cannot find setting file at {setting_file}") + if args.pose_id < -1: + raise ValueError(f"Pose index must be -1 or >0") + + + app = slampy.System(setting_file, slampy.Sensor.MONOCULAR) + + + # TODO: generic loader an not KITTI one + + if args.data_type == "TUM": + image_filenames, timestamps = load_images_TUM(args.dataset, "rgb.txt") + elif args.data_type == "KITTI_VO": + image_filenames, timestamps = load_images_KITTI_VO(args.dataset) + num_images = len(image_filenames) + + dest_depth = os.path.join(args.dest, "depth") + dest_pose = os.path.join(args.dest, "pose") + + create_dir(dest_depth) + create_dir(dest_pose) + + states = [] + errors = [] + + with tqdm(total=num_images) as pbar: + for idx, image_name in enumerate(image_filenames): + # TODO: it is image loader duty to provide correct images + # image_name = image_name.replace(".png", ".jpg") + image = cv2.imread(image_name) + if image is None: + raise ValueError(f"failed to load image {image_name}") + + image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + + state = app.process_image_mono(image, timestamps[idx]) + + # NOTE: we buid a default invalid depth, in the case of system failure + if state == slampy.State.OK: + depth = app.get_depth() #calcola la depth del frame corrente + pose_past_frame_to_current = app.get_pose_to_target( + precedent_frame=args.pose_id + )#calcola la pose del frame corrente rispetto all'imput dell'utente + name = os.path.splitext(os.path.basename(image_name))[0] #prepara il nome del frame + + depth_path = os.path.join(dest_depth, name) #prepara il file diciamo, deppth_dest from line 33 + save_depth(depth_path, depth) #da arrray salva in formato .png + + pose_path = os.path.join(dest_pose, name) + save_pose(pose_path, pose_past_frame_to_current)#salva in formato numpy + + curr_pose = app.get_pose_to_target(-1) #pose rispetto all'inizio + if curr_pose is not None: + save_pose_txt(args, name, curr_pose)#salva courrent pose in file txt + + if args.is_evalute_depth: #depth rispetto al gt + gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) + err = get_error(args, name, depth, gt_file_path) + errors.append(err) + + states.append(state) + pbar.update(1) + + if args.is_evalute_depth: + mean_errors = np.array(errors).mean(0) #calcola la media di una matrice per colonna, risulta un erray con lo stesso numero di colonne i cui elementi sono la media di ogni clonna + save_results = os.path.join(args.dest, "results.txt") + save_depth_err_results(save_results, "mean values", mean_errors)#la stampa di un erray vuoto in quanto mean(0) di un array vuoto da 0 credo da un errore: format() argument after * must be an iterable, not numpy.float64 + + + # NOTE: final dump of log.txt file + with open(os.path.join(args.dest, "log.txt"), "w") as f: + for i, state in enumerate(states): + f.write(f"{i}: {state}\n") + + if args.is_evalute_pose: + print("Begin to evaluate predicted pose")#si prende il file pose di numpy array salvato prima in pose e lo confronta col gt + evaluate_pose(args) + eval_tool = KittiEvalOdom() + eval_tool.eval(args) + + +parser = argparse.ArgumentParser( + description="Run the SLAM system and save, at each frame, the current depth and pose" +) + +parser.add_argument( + "--dataset", + type=str, + default="/media/Datasets/KITTI_VO/dataset/sequences/10", + help="path to dataset", +) +parser.add_argument( + "--settings", + type=str, + default="./settings_kitty.yaml", + help="which configuration?", +) +parser.add_argument( + "--dest", + type=str, + default="./results_kitty_vo_10", + help="where do we save artefacts?", +) +parser.add_argument( + "--pose_id", + type=int, + default=-1, + help="between which frames do you want compute the pose? If pose_id==-1, get the pose between 0->T; \ + if pose_id >0, compute the pose between T-pose_id->T \ + For instance, if pose_id=2 then compute the pose between T-2->T", +) +parser.add_argument( + "--is_evalute_depth", + default=False, + action="store_true", + help="If set, will evalute the orb depth with the gt files ", +) + +parser.add_argument( + "--is_evalute_pose", + default=False, + action="store_true", + help="If set, will evalute the orb pose with the gt files", +) + +parser.add_argument( + "--is_bash", + # default=True, + action="store_true", + help="If set, means use bash shell to evaluate", +) + +parser.add_argument( + "--data_type", + type=str, + help="which dataset type", + default="KITTI_VO", + choices=["TUM", "KITTI_VO", "KITTI"], +) + +parser.add_argument( + "--gt_depth", + type=str, + help="the gt depth files of the dataset", + default="/media/Datasets/KITTI_VO_SGM/10/depth", +) +# /media/Datasets/TUM/freiburg3_convert/depth + +parser.add_argument( + "--gt_pose_dir", + type=str, + help="each frame's gt pose file, saved as previous to current, and filename as current.npy", + default="/media/Datasets/KITTI_VO_SGM/10/npy_pose", +) + +parser.add_argument( + "--gt_pose_txt", + type=str, + help="this is the gt pose file provided by kitty or tum.", + default="/media/Datasets/KITTI_VO/dataset/poses/10.txt", +) + +parser.add_argument( + "--align", + type=str, + choices=["scale", "scale_7dof", "7dof", "6dof"], + default="7dof", + help="alignment type", +) + +parser.add_argument( + "--named", type=str, help="the names for saving pose", default="kitty_vo_10" +) + + +if __name__ == "__main__": + + args = parser.parse_args() + run(args) diff --git a/run_GabHoo.py b/run_GabHoo.py new file mode 100644 index 0000000..6e44843 --- /dev/null +++ b/run_GabHoo.py @@ -0,0 +1,211 @@ +import os +import cv2 +import numpy as np +import slampy +import argparse +import yaml +from tqdm import tqdm +from utils import * +from kitti_odometry import KittiEvalOdom + + +def run(args): + """Run SLAM system for each frame in the dataset and save depths and poses. + + Args: + args: command line arguments + """ + setting_file = args.settings + if not os.path.exists(setting_file): + raise ValueError(f"Cannot find setting file at {setting_file}") + if args.pose_id < -1: + raise ValueError(f"Pose index must be -1 or >0") + + + with open(args.settings) as fs: + settings_yalm = yaml.safe_load(fs) + print("\nAlgorithm " + settings_yalm["SLAM.alg"] + " has been set\n") + + print("Dataset selected: " + os.path.basename(args.dataset) + "\n") + + app = slampy.System(setting_file, slampy.Sensor.MONOCULAR) + + print("\n") + + + # TODO: generic loader an not KITTI one + + if args.data_type == "TUM": + image_filenames, timestamps = load_images_TUM(args.dataset, "rgb.txt") + elif args.data_type == "KITTI_VO": + image_filenames, timestamps = load_images_KITTI_VO(args.dataset) + elif args.data_type == "OTHERS": + image_filenames, timestamps = load_images_OTHERS(args.dataset) + + + + num_images = len(image_filenames) + + dest_depth = os.path.join(args.dest, "depth") + dest_pose = os.path.join(args.dest, "pose") + + create_dir(dest_depth) + create_dir(dest_pose) + + states = [] + errors = [] + + with tqdm(total=num_images) as pbar: + for idx, image_name in enumerate(image_filenames): + # TODO: it is image loader duty to provide correct images + # image_name = image_name.replace(".png", ".jpg") + image = cv2.imread(image_name) + if image is None: + raise ValueError(f"failed to load image {image_name}") + + image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + + state = app.process_image_mono(image, timestamps[idx]) + + # NOTE: we buid a default invalid depth, in the case of system failure + if state == slampy.State.OK: + depth = app.get_depth() #calcola la depth del frame corrente + pose_past_frame_to_current = app.get_pose_to_target( + precedent_frame=args.pose_id + )#calcola la pose del frame corrente rispetto all'imput dell'utente + name = os.path.splitext(os.path.basename(image_name))[0] #prepara il nome del frame + + depth_path = os.path.join(dest_depth, name) #prepara il file diciamo, deppth_dest from line 33 + save_depth(depth_path, depth) #da arrray salva in formato .png + + pose_path = os.path.join(dest_pose, name) + save_pose(pose_path, pose_past_frame_to_current)#salva in formato numpy + + curr_pose = app.get_pose_to_target(-1) #pose rispetto all'inizio + if curr_pose is not None: + save_pose_txt(args, name, curr_pose)#salva courrent pose in file txt + + if args.is_evalute_depth: #depth rispetto al gt + gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) + err = get_error(args, name, depth, gt_file_path) + errors.append(err) + + states.append(state) + pbar.update(1) + + if args.is_evalute_depth: + mean_errors = np.array(errors).mean(0) #calcola la media di una matrice per colonna, risulta un erray con lo stesso numero di colonne i cui elementi sono la media di ogni clonna + save_results = os.path.join(args.dest, "results.txt") + save_depth_err_results(save_results, "mean values", mean_errors)#la stampa di un erray vuoto in quanto mean(0) di un array vuoto da 0 credo da un errore: format() argument after * must be an iterable, not numpy.float64 + + + # NOTE: final dump of log.txt file + with open(os.path.join(args.dest, "log.txt"), "w") as f: + for i, state in enumerate(states): + f.write(f"{i}: {state}\n") + + if args.is_evalute_pose: + print("Begin to evaluate predicted pose")#si prende il file pose di numpy array salvato prima in pose e lo confronta col gt + evaluate_pose(args) + eval_tool = KittiEvalOdom() + eval_tool.eval(args) + + +parser = argparse.ArgumentParser( + description="Run the SLAM system and save, at each frame, the current depth and pose" +) + +parser.add_argument( + "--dataset", + type=str, + default="/media/Datasets/KITTI_VO/dataset/sequences/10", + help="path to dataset", +) +parser.add_argument( + "--settings", + type=str, + default="./settings_kitty.yaml", + help="which configuration?", +) +parser.add_argument( + "--dest", + type=str, + default="./results_kitty_vo_10", + help="where do we save artefacts?", +) +parser.add_argument( + "--pose_id", + type=int, + default=-1, + help="between which frames do you want compute the pose? If pose_id==-1, get the pose between 0->T; \ + if pose_id >0, compute the pose between T-pose_id->T \ + For instance, if pose_id=2 then compute the pose between T-2->T", +) +parser.add_argument( + "--is_evalute_depth", + default=False, + action="store_true", + help="If set, will evalute the orb depth with the gt files ", +) + +parser.add_argument( + "--is_evalute_pose", + default=False, + action="store_true", + help="If set, will evalute the orb pose with the gt files", +) + +parser.add_argument( + "--is_bash", + # default=True, + action="store_true", + help="If set, means use bash shell to evaluate", +) + +parser.add_argument( + "--data_type", + type=str, + help="which dataset type", + default="KITTI_VO", + choices=["TUM", "KITTI_VO", "KITTI","OTHERS"], +) + +parser.add_argument( + "--gt_depth", + type=str, + help="the gt depth files of the dataset", + default="/media/Datasets/KITTI_VO_SGM/10/depth", +) +# /media/Datasets/TUM/freiburg3_convert/depth + +parser.add_argument( + "--gt_pose_dir", + type=str, + help="each frame's gt pose file, saved as previous to current, and filename as current.npy", + default="/media/Datasets/KITTI_VO_SGM/10/npy_pose", +) + +parser.add_argument( + "--gt_pose_txt", + type=str, + help="this is the gt pose file provided by kitty or tum.", + default="/media/Datasets/KITTI_VO/dataset/poses/10.txt", +) + +parser.add_argument( + "--align", + type=str, + choices=["scale", "scale_7dof", "7dof", "6dof"], + default="7dof", + help="alignment type", +) + +parser.add_argument( + "--named", type=str, help="the names for saving pose", default="kitty_vo_10" +) + + +if __name__ == "__main__": + + args = parser.parse_args() + run(args) diff --git a/slam_method/Settings/GabHoo_camera_settings.yaml b/slam_method/Settings/GabHoo_camera_settings.yaml new file mode 100644 index 0000000..b75b340 --- /dev/null +++ b/slam_method/Settings/GabHoo_camera_settings.yaml @@ -0,0 +1,64 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 960.21601970854658 +Camera.fy: 960.21601970854658 +Camera.cx: 359.50 +Camera.cy: 640.5 + +Camera.k1: 0.15227141764846494 +Camera.k2: -0.45960964020556544 +Camera.p1: -6.0511094544970037e-04 +Camera.p2: -8.8257777165142386e-05 +Camera.p3: 3.6787038305221986e-01 + + +# Camera frames per second +Camera.fps: 30.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Camera resolution +Camera.width: 720 +Camera.height: 1282 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 3000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 14 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 + diff --git a/utils.py b/utils.py index d8c4348..49a2be6 100644 --- a/utils.py +++ b/utils.py @@ -232,7 +232,7 @@ def save_pose_txt(args, name, pose): """ pose_file_path = os.path.join(args.dest, "pose.txt") fp = open(pose_file_path, "a") - pose34 = pose[:3] + pose34 = pose[:3] #prende dal primo al terzo elemento (partendo da 0) dell'array fp.write(name) for row in pose34: @@ -374,6 +374,7 @@ def get_error(args, filename, points, gt_filename): return err + def load_images_KITTI_VO(path_to_sequence): """Return the sequence of the images found in the path and the corrispondent timestamp @@ -416,3 +417,38 @@ def load_images_TUM(path_to_sequence, file_name): timestamps.append(float(t)) return [os.path.join(path_to_sequence, name) for name in rgb_filenames], timestamps + + +def load_images_OTHERS(path_to_sequence): + + """Return the sequence of the images found in the path and the corrispondent timestamp + + Args: + path_to_sequence : the sequence in witch we can found the image sequences + + Returns : + two array : one contains the sequence of the image filename and the second the timestamp in whitch they are acquired + + Inside of path_to_sequence must be: +data + xxxxxxxx.png + xxxxxxxy.png + .... + -times.txt + where times.txt simply contains timestamps of every frame + + """ + + timestamps = [] + framenames = [] + + with open(os.path.join(path_to_sequence, "times.txt")) as times_file: + for line in times_file: + if len(line) > 0 and not line.startswith("#"): + timestamps.append(float(line)) + + for framename in sorted(os.listdir(os.path.join(path_to_sequence,"data"))): + if framename.endswith(".png"): + framenames.append(framename) + + return [os.path.join(path_to_sequence,"data",name) for name in framenames], timestamps + From 325aab74b356d59b793957b5f98a448cb4e35826 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sat, 23 Jan 2021 13:08:54 +0100 Subject: [PATCH 03/13] modified: run.py deleted: run_GabHoo.py --- run.py | 55 ++++++++----- run_GabHoo.py | 211 -------------------------------------------------- 2 files changed, 37 insertions(+), 229 deletions(-) delete mode 100644 run_GabHoo.py diff --git a/run.py b/run.py index edcb94a..6e44843 100644 --- a/run.py +++ b/run.py @@ -3,6 +3,7 @@ import numpy as np import slampy import argparse +import yaml from tqdm import tqdm from utils import * from kitti_odometry import KittiEvalOdom @@ -19,15 +20,30 @@ def run(args): raise ValueError(f"Cannot find setting file at {setting_file}") if args.pose_id < -1: raise ValueError(f"Pose index must be -1 or >0") + + + with open(args.settings) as fs: + settings_yalm = yaml.safe_load(fs) + print("\nAlgorithm " + settings_yalm["SLAM.alg"] + " has been set\n") + + print("Dataset selected: " + os.path.basename(args.dataset) + "\n") app = slampy.System(setting_file, slampy.Sensor.MONOCULAR) + print("\n") + + # TODO: generic loader an not KITTI one if args.data_type == "TUM": image_filenames, timestamps = load_images_TUM(args.dataset, "rgb.txt") elif args.data_type == "KITTI_VO": image_filenames, timestamps = load_images_KITTI_VO(args.dataset) + elif args.data_type == "OTHERS": + image_filenames, timestamps = load_images_OTHERS(args.dataset) + + + num_images = len(image_filenames) dest_depth = os.path.join(args.dest, "depth") @@ -48,37 +64,40 @@ def run(args): raise ValueError(f"failed to load image {image_name}") image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - + state = app.process_image_mono(image, timestamps[idx]) # NOTE: we buid a default invalid depth, in the case of system failure if state == slampy.State.OK: - depth = app.get_depth() + depth = app.get_depth() #calcola la depth del frame corrente pose_past_frame_to_current = app.get_pose_to_target( precedent_frame=args.pose_id - ) - name = os.path.splitext(os.path.basename(image_name))[0] - depth_path = os.path.join(dest_depth, name) - save_depth(depth_path, depth) + )#calcola la pose del frame corrente rispetto all'imput dell'utente + name = os.path.splitext(os.path.basename(image_name))[0] #prepara il nome del frame + + depth_path = os.path.join(dest_depth, name) #prepara il file diciamo, deppth_dest from line 33 + save_depth(depth_path, depth) #da arrray salva in formato .png pose_path = os.path.join(dest_pose, name) - save_pose(pose_path, pose_past_frame_to_current) + save_pose(pose_path, pose_past_frame_to_current)#salva in formato numpy - curr_pose = app.get_pose_to_target(-1) + curr_pose = app.get_pose_to_target(-1) #pose rispetto all'inizio if curr_pose is not None: - save_pose_txt(args, name, curr_pose) + save_pose_txt(args, name, curr_pose)#salva courrent pose in file txt - if args.is_evalute_depth: + if args.is_evalute_depth: #depth rispetto al gt gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) err = get_error(args, name, depth, gt_file_path) errors.append(err) states.append(state) pbar.update(1) - - mean_errors = np.array(errors).mean(0) - save_results = os.path.join(args.dest, "results.txt") - save_depth_err_results(save_results, "mean values", mean_errors) + + if args.is_evalute_depth: + mean_errors = np.array(errors).mean(0) #calcola la media di una matrice per colonna, risulta un erray con lo stesso numero di colonne i cui elementi sono la media di ogni clonna + save_results = os.path.join(args.dest, "results.txt") + save_depth_err_results(save_results, "mean values", mean_errors)#la stampa di un erray vuoto in quanto mean(0) di un array vuoto da 0 credo da un errore: format() argument after * must be an iterable, not numpy.float64 + # NOTE: final dump of log.txt file with open(os.path.join(args.dest, "log.txt"), "w") as f: @@ -86,7 +105,7 @@ def run(args): f.write(f"{i}: {state}\n") if args.is_evalute_pose: - print("Begin to evaluate predicted pose") + print("Begin to evaluate predicted pose")#si prende il file pose di numpy array salvato prima in pose e lo confronta col gt evaluate_pose(args) eval_tool = KittiEvalOdom() eval_tool.eval(args) @@ -124,14 +143,14 @@ def run(args): ) parser.add_argument( "--is_evalute_depth", - default=True, + default=False, action="store_true", help="If set, will evalute the orb depth with the gt files ", ) parser.add_argument( "--is_evalute_pose", - default=True, + default=False, action="store_true", help="If set, will evalute the orb pose with the gt files", ) @@ -148,7 +167,7 @@ def run(args): type=str, help="which dataset type", default="KITTI_VO", - choices=["TUM", "KITTI_VO", "KITTI"], + choices=["TUM", "KITTI_VO", "KITTI","OTHERS"], ) parser.add_argument( diff --git a/run_GabHoo.py b/run_GabHoo.py deleted file mode 100644 index 6e44843..0000000 --- a/run_GabHoo.py +++ /dev/null @@ -1,211 +0,0 @@ -import os -import cv2 -import numpy as np -import slampy -import argparse -import yaml -from tqdm import tqdm -from utils import * -from kitti_odometry import KittiEvalOdom - - -def run(args): - """Run SLAM system for each frame in the dataset and save depths and poses. - - Args: - args: command line arguments - """ - setting_file = args.settings - if not os.path.exists(setting_file): - raise ValueError(f"Cannot find setting file at {setting_file}") - if args.pose_id < -1: - raise ValueError(f"Pose index must be -1 or >0") - - - with open(args.settings) as fs: - settings_yalm = yaml.safe_load(fs) - print("\nAlgorithm " + settings_yalm["SLAM.alg"] + " has been set\n") - - print("Dataset selected: " + os.path.basename(args.dataset) + "\n") - - app = slampy.System(setting_file, slampy.Sensor.MONOCULAR) - - print("\n") - - - # TODO: generic loader an not KITTI one - - if args.data_type == "TUM": - image_filenames, timestamps = load_images_TUM(args.dataset, "rgb.txt") - elif args.data_type == "KITTI_VO": - image_filenames, timestamps = load_images_KITTI_VO(args.dataset) - elif args.data_type == "OTHERS": - image_filenames, timestamps = load_images_OTHERS(args.dataset) - - - - num_images = len(image_filenames) - - dest_depth = os.path.join(args.dest, "depth") - dest_pose = os.path.join(args.dest, "pose") - - create_dir(dest_depth) - create_dir(dest_pose) - - states = [] - errors = [] - - with tqdm(total=num_images) as pbar: - for idx, image_name in enumerate(image_filenames): - # TODO: it is image loader duty to provide correct images - # image_name = image_name.replace(".png", ".jpg") - image = cv2.imread(image_name) - if image is None: - raise ValueError(f"failed to load image {image_name}") - - image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - - state = app.process_image_mono(image, timestamps[idx]) - - # NOTE: we buid a default invalid depth, in the case of system failure - if state == slampy.State.OK: - depth = app.get_depth() #calcola la depth del frame corrente - pose_past_frame_to_current = app.get_pose_to_target( - precedent_frame=args.pose_id - )#calcola la pose del frame corrente rispetto all'imput dell'utente - name = os.path.splitext(os.path.basename(image_name))[0] #prepara il nome del frame - - depth_path = os.path.join(dest_depth, name) #prepara il file diciamo, deppth_dest from line 33 - save_depth(depth_path, depth) #da arrray salva in formato .png - - pose_path = os.path.join(dest_pose, name) - save_pose(pose_path, pose_past_frame_to_current)#salva in formato numpy - - curr_pose = app.get_pose_to_target(-1) #pose rispetto all'inizio - if curr_pose is not None: - save_pose_txt(args, name, curr_pose)#salva courrent pose in file txt - - if args.is_evalute_depth: #depth rispetto al gt - gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) - err = get_error(args, name, depth, gt_file_path) - errors.append(err) - - states.append(state) - pbar.update(1) - - if args.is_evalute_depth: - mean_errors = np.array(errors).mean(0) #calcola la media di una matrice per colonna, risulta un erray con lo stesso numero di colonne i cui elementi sono la media di ogni clonna - save_results = os.path.join(args.dest, "results.txt") - save_depth_err_results(save_results, "mean values", mean_errors)#la stampa di un erray vuoto in quanto mean(0) di un array vuoto da 0 credo da un errore: format() argument after * must be an iterable, not numpy.float64 - - - # NOTE: final dump of log.txt file - with open(os.path.join(args.dest, "log.txt"), "w") as f: - for i, state in enumerate(states): - f.write(f"{i}: {state}\n") - - if args.is_evalute_pose: - print("Begin to evaluate predicted pose")#si prende il file pose di numpy array salvato prima in pose e lo confronta col gt - evaluate_pose(args) - eval_tool = KittiEvalOdom() - eval_tool.eval(args) - - -parser = argparse.ArgumentParser( - description="Run the SLAM system and save, at each frame, the current depth and pose" -) - -parser.add_argument( - "--dataset", - type=str, - default="/media/Datasets/KITTI_VO/dataset/sequences/10", - help="path to dataset", -) -parser.add_argument( - "--settings", - type=str, - default="./settings_kitty.yaml", - help="which configuration?", -) -parser.add_argument( - "--dest", - type=str, - default="./results_kitty_vo_10", - help="where do we save artefacts?", -) -parser.add_argument( - "--pose_id", - type=int, - default=-1, - help="between which frames do you want compute the pose? If pose_id==-1, get the pose between 0->T; \ - if pose_id >0, compute the pose between T-pose_id->T \ - For instance, if pose_id=2 then compute the pose between T-2->T", -) -parser.add_argument( - "--is_evalute_depth", - default=False, - action="store_true", - help="If set, will evalute the orb depth with the gt files ", -) - -parser.add_argument( - "--is_evalute_pose", - default=False, - action="store_true", - help="If set, will evalute the orb pose with the gt files", -) - -parser.add_argument( - "--is_bash", - # default=True, - action="store_true", - help="If set, means use bash shell to evaluate", -) - -parser.add_argument( - "--data_type", - type=str, - help="which dataset type", - default="KITTI_VO", - choices=["TUM", "KITTI_VO", "KITTI","OTHERS"], -) - -parser.add_argument( - "--gt_depth", - type=str, - help="the gt depth files of the dataset", - default="/media/Datasets/KITTI_VO_SGM/10/depth", -) -# /media/Datasets/TUM/freiburg3_convert/depth - -parser.add_argument( - "--gt_pose_dir", - type=str, - help="each frame's gt pose file, saved as previous to current, and filename as current.npy", - default="/media/Datasets/KITTI_VO_SGM/10/npy_pose", -) - -parser.add_argument( - "--gt_pose_txt", - type=str, - help="this is the gt pose file provided by kitty or tum.", - default="/media/Datasets/KITTI_VO/dataset/poses/10.txt", -) - -parser.add_argument( - "--align", - type=str, - choices=["scale", "scale_7dof", "7dof", "6dof"], - default="7dof", - help="alignment type", -) - -parser.add_argument( - "--named", type=str, help="the names for saving pose", default="kitty_vo_10" -) - - -if __name__ == "__main__": - - args = parser.parse_args() - run(args) From 2b7194135e38b607e8d21e273c794f475aac797c Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sat, 23 Jan 2021 13:14:59 +0100 Subject: [PATCH 04/13] yeye tolto commenti cancellato evalueate --- run.py | 32 ++++----- run2.py | 197 -------------------------------------------------------- 2 files changed, 16 insertions(+), 213 deletions(-) delete mode 100644 run2.py diff --git a/run.py b/run.py index 6e44843..ce54b01 100644 --- a/run.py +++ b/run.py @@ -69,23 +69,23 @@ def run(args): # NOTE: we buid a default invalid depth, in the case of system failure if state == slampy.State.OK: - depth = app.get_depth() #calcola la depth del frame corrente + depth = app.get_depth() pose_past_frame_to_current = app.get_pose_to_target( precedent_frame=args.pose_id - )#calcola la pose del frame corrente rispetto all'imput dell'utente - name = os.path.splitext(os.path.basename(image_name))[0] #prepara il nome del frame + ) + name = os.path.splitext(os.path.basename(image_name))[0] - depth_path = os.path.join(dest_depth, name) #prepara il file diciamo, deppth_dest from line 33 - save_depth(depth_path, depth) #da arrray salva in formato .png + depth_path = os.path.join(dest_depth, name) + save_depth(depth_path, depth) pose_path = os.path.join(dest_pose, name) - save_pose(pose_path, pose_past_frame_to_current)#salva in formato numpy + save_pose(pose_path, pose_past_frame_to_current) - curr_pose = app.get_pose_to_target(-1) #pose rispetto all'inizio + curr_pose = app.get_pose_to_target(-1) if curr_pose is not None: - save_pose_txt(args, name, curr_pose)#salva courrent pose in file txt + save_pose_txt(args, name, curr_pose) - if args.is_evalute_depth: #depth rispetto al gt + if args.is_evaluate_depth: gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) err = get_error(args, name, depth, gt_file_path) errors.append(err) @@ -93,10 +93,10 @@ def run(args): states.append(state) pbar.update(1) - if args.is_evalute_depth: - mean_errors = np.array(errors).mean(0) #calcola la media di una matrice per colonna, risulta un erray con lo stesso numero di colonne i cui elementi sono la media di ogni clonna + if args.is_evaluate_depth: + mean_errors = np.array(errors).mean(0) save_results = os.path.join(args.dest, "results.txt") - save_depth_err_results(save_results, "mean values", mean_errors)#la stampa di un erray vuoto in quanto mean(0) di un array vuoto da 0 credo da un errore: format() argument after * must be an iterable, not numpy.float64 + save_depth_err_results(save_results, "mean values", mean_errors) # NOTE: final dump of log.txt file @@ -104,8 +104,8 @@ def run(args): for i, state in enumerate(states): f.write(f"{i}: {state}\n") - if args.is_evalute_pose: - print("Begin to evaluate predicted pose")#si prende il file pose di numpy array salvato prima in pose e lo confronta col gt + if args.is_evaluate_pose: + print("Begin to evaluate predicted pose") evaluate_pose(args) eval_tool = KittiEvalOdom() eval_tool.eval(args) @@ -142,14 +142,14 @@ def run(args): For instance, if pose_id=2 then compute the pose between T-2->T", ) parser.add_argument( - "--is_evalute_depth", + "--is_evaluate_depth", default=False, action="store_true", help="If set, will evalute the orb depth with the gt files ", ) parser.add_argument( - "--is_evalute_pose", + "--is_evaluate_pose", default=False, action="store_true", help="If set, will evalute the orb pose with the gt files", diff --git a/run2.py b/run2.py deleted file mode 100644 index 69a639c..0000000 --- a/run2.py +++ /dev/null @@ -1,197 +0,0 @@ -import os -import cv2 -import numpy as np -import slampy -import argparse -from tqdm import tqdm -from utils import * -from kitti_odometry import KittiEvalOdom - - -def run(args): - """Run SLAM system for each frame in the dataset and save depths and poses. - - Args: - args: command line arguments - """ - setting_file = args.settings - if not os.path.exists(setting_file): - raise ValueError(f"Cannot find setting file at {setting_file}") - if args.pose_id < -1: - raise ValueError(f"Pose index must be -1 or >0") - - - app = slampy.System(setting_file, slampy.Sensor.MONOCULAR) - - - # TODO: generic loader an not KITTI one - - if args.data_type == "TUM": - image_filenames, timestamps = load_images_TUM(args.dataset, "rgb.txt") - elif args.data_type == "KITTI_VO": - image_filenames, timestamps = load_images_KITTI_VO(args.dataset) - num_images = len(image_filenames) - - dest_depth = os.path.join(args.dest, "depth") - dest_pose = os.path.join(args.dest, "pose") - - create_dir(dest_depth) - create_dir(dest_pose) - - states = [] - errors = [] - - with tqdm(total=num_images) as pbar: - for idx, image_name in enumerate(image_filenames): - # TODO: it is image loader duty to provide correct images - # image_name = image_name.replace(".png", ".jpg") - image = cv2.imread(image_name) - if image is None: - raise ValueError(f"failed to load image {image_name}") - - image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - - state = app.process_image_mono(image, timestamps[idx]) - - # NOTE: we buid a default invalid depth, in the case of system failure - if state == slampy.State.OK: - depth = app.get_depth() #calcola la depth del frame corrente - pose_past_frame_to_current = app.get_pose_to_target( - precedent_frame=args.pose_id - )#calcola la pose del frame corrente rispetto all'imput dell'utente - name = os.path.splitext(os.path.basename(image_name))[0] #prepara il nome del frame - - depth_path = os.path.join(dest_depth, name) #prepara il file diciamo, deppth_dest from line 33 - save_depth(depth_path, depth) #da arrray salva in formato .png - - pose_path = os.path.join(dest_pose, name) - save_pose(pose_path, pose_past_frame_to_current)#salva in formato numpy - - curr_pose = app.get_pose_to_target(-1) #pose rispetto all'inizio - if curr_pose is not None: - save_pose_txt(args, name, curr_pose)#salva courrent pose in file txt - - if args.is_evalute_depth: #depth rispetto al gt - gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) - err = get_error(args, name, depth, gt_file_path) - errors.append(err) - - states.append(state) - pbar.update(1) - - if args.is_evalute_depth: - mean_errors = np.array(errors).mean(0) #calcola la media di una matrice per colonna, risulta un erray con lo stesso numero di colonne i cui elementi sono la media di ogni clonna - save_results = os.path.join(args.dest, "results.txt") - save_depth_err_results(save_results, "mean values", mean_errors)#la stampa di un erray vuoto in quanto mean(0) di un array vuoto da 0 credo da un errore: format() argument after * must be an iterable, not numpy.float64 - - - # NOTE: final dump of log.txt file - with open(os.path.join(args.dest, "log.txt"), "w") as f: - for i, state in enumerate(states): - f.write(f"{i}: {state}\n") - - if args.is_evalute_pose: - print("Begin to evaluate predicted pose")#si prende il file pose di numpy array salvato prima in pose e lo confronta col gt - evaluate_pose(args) - eval_tool = KittiEvalOdom() - eval_tool.eval(args) - - -parser = argparse.ArgumentParser( - description="Run the SLAM system and save, at each frame, the current depth and pose" -) - -parser.add_argument( - "--dataset", - type=str, - default="/media/Datasets/KITTI_VO/dataset/sequences/10", - help="path to dataset", -) -parser.add_argument( - "--settings", - type=str, - default="./settings_kitty.yaml", - help="which configuration?", -) -parser.add_argument( - "--dest", - type=str, - default="./results_kitty_vo_10", - help="where do we save artefacts?", -) -parser.add_argument( - "--pose_id", - type=int, - default=-1, - help="between which frames do you want compute the pose? If pose_id==-1, get the pose between 0->T; \ - if pose_id >0, compute the pose between T-pose_id->T \ - For instance, if pose_id=2 then compute the pose between T-2->T", -) -parser.add_argument( - "--is_evalute_depth", - default=False, - action="store_true", - help="If set, will evalute the orb depth with the gt files ", -) - -parser.add_argument( - "--is_evalute_pose", - default=False, - action="store_true", - help="If set, will evalute the orb pose with the gt files", -) - -parser.add_argument( - "--is_bash", - # default=True, - action="store_true", - help="If set, means use bash shell to evaluate", -) - -parser.add_argument( - "--data_type", - type=str, - help="which dataset type", - default="KITTI_VO", - choices=["TUM", "KITTI_VO", "KITTI"], -) - -parser.add_argument( - "--gt_depth", - type=str, - help="the gt depth files of the dataset", - default="/media/Datasets/KITTI_VO_SGM/10/depth", -) -# /media/Datasets/TUM/freiburg3_convert/depth - -parser.add_argument( - "--gt_pose_dir", - type=str, - help="each frame's gt pose file, saved as previous to current, and filename as current.npy", - default="/media/Datasets/KITTI_VO_SGM/10/npy_pose", -) - -parser.add_argument( - "--gt_pose_txt", - type=str, - help="this is the gt pose file provided by kitty or tum.", - default="/media/Datasets/KITTI_VO/dataset/poses/10.txt", -) - -parser.add_argument( - "--align", - type=str, - choices=["scale", "scale_7dof", "7dof", "6dof"], - default="7dof", - help="alignment type", -) - -parser.add_argument( - "--named", type=str, help="the names for saving pose", default="kitty_vo_10" -) - - -if __name__ == "__main__": - - args = parser.parse_args() - run(args) From 1b77b035e07f95e8d286dcf23192f7103c139f23 Mon Sep 17 00:00:00 2001 From: GabHoo <56197287+GabHoo@users.noreply.github.com> Date: Sat, 23 Jan 2021 13:32:17 +0100 Subject: [PATCH 05/13] Update utils.py --- utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utils.py b/utils.py index 49a2be6..d3526ef 100644 --- a/utils.py +++ b/utils.py @@ -232,7 +232,7 @@ def save_pose_txt(args, name, pose): """ pose_file_path = os.path.join(args.dest, "pose.txt") fp = open(pose_file_path, "a") - pose34 = pose[:3] #prende dal primo al terzo elemento (partendo da 0) dell'array + pose34 = pose[:3] fp.write(name) for row in pose34: From 5702cb32da06370a611e01683dcc2cc7c4885f48 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Sat, 23 Jan 2021 13:35:53 +0100 Subject: [PATCH 06/13] small fixes --- GabHoo_settings.yaml | 28 ---------------------------- run.py | 3 --- utils.py | 3 +-- 3 files changed, 1 insertion(+), 33 deletions(-) delete mode 100644 GabHoo_settings.yaml diff --git a/GabHoo_settings.yaml b/GabHoo_settings.yaml deleted file mode 100644 index 4eacff6..0000000 --- a/GabHoo_settings.yaml +++ /dev/null @@ -1,28 +0,0 @@ -SLAM.alg: "OrbSlam3" -#-------------------------------------------------------------------------------------------- -# SLAM Method: change with the name of the .py file in whitch there are the class SLAM -#-------------------------------------------------------------------------------------------- - -#-------------------------------------------------------------------------------------------- -# SLAM Params: add your own to run the algorithm -#-------------------------------------------------------------------------------------------- -SLAM.vocab_path: "slam_method/Settings/ORBvoc.bin" -SLAM.settings_path: "slam_method/Settings/GabHoo_camera_settings.yaml" - -#-------------------------------------------------------------------------------------------- -# Drawer Params: the extra params of trajectory_drawer class -# Params Guide https://slampy.readthedocs.io/en/latest/config.html -#-------------------------------------------------------------------------------------------- - -Drawer.eye.x: -18.0 -Drawer.eye.y: -13.0 -Drawer.eye.z: -55.0 -Drawer.center.x: -17.0 -Drawer.center.y: -8.0 -Drawer.scale_grade.x: 1.0 -Drawer.scale_grade.y: 1.0 -Drawer.scale_grade.z: 10.0 -Drawer.aspectratio.x: 50 -Drawer.aspectratio.y: 50 -Drawer.aspectratio.z: 100 -Drawer.point_size: 2 diff --git a/run.py b/run.py index ce54b01..7df0e2b 100644 --- a/run.py +++ b/run.py @@ -32,7 +32,6 @@ def run(args): print("\n") - # TODO: generic loader an not KITTI one if args.data_type == "TUM": @@ -41,8 +40,6 @@ def run(args): image_filenames, timestamps = load_images_KITTI_VO(args.dataset) elif args.data_type == "OTHERS": image_filenames, timestamps = load_images_OTHERS(args.dataset) - - num_images = len(image_filenames) diff --git a/utils.py b/utils.py index 49a2be6..dc68040 100644 --- a/utils.py +++ b/utils.py @@ -232,8 +232,7 @@ def save_pose_txt(args, name, pose): """ pose_file_path = os.path.join(args.dest, "pose.txt") fp = open(pose_file_path, "a") - pose34 = pose[:3] #prende dal primo al terzo elemento (partendo da 0) dell'array - + pose34 = pose[:3] fp.write(name) for row in pose34: fp.write(" ") From 7b95403f9a3b2f1bffe559e81ab30d61f1a89725 Mon Sep 17 00:00:00 2001 From: GabHoo <56197287+GabHoo@users.noreply.github.com> Date: Tue, 26 Jan 2021 19:20:49 +0100 Subject: [PATCH 07/13] Update and rename GabHoo_camera_settings.yaml to P20_camera_settings.yaml --- .../{GabHoo_camera_settings.yaml => P20_camera_settings.yaml} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename slam_method/Settings/{GabHoo_camera_settings.yaml => P20_camera_settings.yaml} (92%) diff --git a/slam_method/Settings/GabHoo_camera_settings.yaml b/slam_method/Settings/P20_camera_settings.yaml similarity index 92% rename from slam_method/Settings/GabHoo_camera_settings.yaml rename to slam_method/Settings/P20_camera_settings.yaml index b75b340..e321a19 100644 --- a/slam_method/Settings/GabHoo_camera_settings.yaml +++ b/slam_method/Settings/P20_camera_settings.yaml @@ -1,7 +1,7 @@ %YAML:1.0 #-------------------------------------------------------------------------------------------- -# Camera Parameters. Adjust them! +# Camera Parameters. This settings refer to a Huawei P20 lite main camera, manually calibrated with opencv [resolution fixed to 720x1280] #-------------------------------------------------------------------------------------------- Camera.type: "PinHole" @@ -26,7 +26,7 @@ Camera.RGB: 1 # Camera resolution Camera.width: 720 -Camera.height: 1282 +Camera.height: 1280 #-------------------------------------------------------------------------------------------- # ORB Parameters From 590adb6773bf80b32bb41053adaa47c29f172654 Mon Sep 17 00:00:00 2001 From: GabHoo <56197287+GabHoo@users.noreply.github.com> Date: Tue, 26 Jan 2021 19:35:08 +0100 Subject: [PATCH 08/13] Update utils.py --- utils.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/utils.py b/utils.py index e13ab33..8e842f4 100644 --- a/utils.py +++ b/utils.py @@ -232,11 +232,7 @@ def save_pose_txt(args, name, pose): """ pose_file_path = os.path.join(args.dest, "pose.txt") fp = open(pose_file_path, "a") - pose34 = pose[:3] -<<<<<<< HEAD -======= ->>>>>>> 1b77b035e07f95e8d286dcf23192f7103c139f23 fp.write(name) for row in pose34: fp.write(" ") From cf6a3ac765b029ebb01b32d48f71709b617fe910 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Tue, 2 Feb 2021 13:33:15 +0100 Subject: [PATCH 09/13] Changes to be committed: new file: eval_orb_tum_vi.sh new file: run_MONO_IMU.py new file: settings_TUM_VI.yaml modified: slam_method/OrbSlam3.py new file: slam_method/Settings/TUM_512_cameraIMU.yaml modified: utils.py --- eval_orb_tum_vi.sh | 25 +++ run_MONO_IMU.py | 226 ++++++++++++++++++++ settings_TUM_VI.yaml | 28 +++ slam_method/OrbSlam3.py | 5 +- slam_method/Settings/TUM_512_cameraIMU.yaml | 91 ++++++++ utils.py | 44 +++- 6 files changed, 413 insertions(+), 6 deletions(-) create mode 100755 eval_orb_tum_vi.sh create mode 100644 run_MONO_IMU.py create mode 100644 settings_TUM_VI.yaml create mode 100755 slam_method/Settings/TUM_512_cameraIMU.yaml diff --git a/eval_orb_tum_vi.sh b/eval_orb_tum_vi.sh new file mode 100755 index 0000000..57775c6 --- /dev/null +++ b/eval_orb_tum_vi.sh @@ -0,0 +1,25 @@ + +#!bin/bash +data="Dataset/TUM_VI/dataset-corridor4_512_16" +settings="./settings_TUM_VI.yaml" +save_path="./results_tum_vi_corrido4" +poseid=1 +type='TUM_VI' +gt_depth_path= +gt_pose_dir="/Dataset/TUM_VI/dataset-corridor4_512_16/dso/gt_imu.csv" +gt_pose_txt= +names_to_plot="TUM_VI_corridor4" + + +python3 run_MONO_IMU.py --dataset $data \ + --settings $settings \ + --dest $save_path \ + --pose_id $poseid \ + --data_type $type \ + + --gt_pose_dir $gt_pose_dir\ + --named $names_to_plot\ + + --is_bash \ + + diff --git a/run_MONO_IMU.py b/run_MONO_IMU.py new file mode 100644 index 0000000..f762186 --- /dev/null +++ b/run_MONO_IMU.py @@ -0,0 +1,226 @@ +import os +import cv2 +import numpy as np +import slampy +import argparse +import yaml +from tqdm import tqdm +from utils import * +from kitti_odometry import KittiEvalOdom + + +def run(args): + """Run SLAM system for each frame in the dataset and save depths and poses. + + Args: + args: command line arguments + """ + setting_file = args.settings + if not os.path.exists(setting_file): + raise ValueError(f"Cannot find setting file at {setting_file}") + if args.pose_id < -1: + raise ValueError(f"Pose index must be -1 or >0") + + + with open(args.settings) as fs: + settings_yalm = yaml.safe_load(fs) + print("\nAlgorithm " + settings_yalm["SLAM.alg"] + " has been set\n") + + print("Dataset selected: " + os.path.basename(args.dataset) + "\n") + + app = slampy.System(setting_file, slampy.Sensor.MONOCULAR_IMU) + + print("\n") + + if args.data_type == "TUM_VI": + image_filenames, timestamps = load_images_TUM_VI(args.dataset) + elif args.data_type == "OTHERS": + image_filenames, timestamps = load_images_OTHERS(args.dataset) + + num_images = len(image_filenames) + + if args.data_type == "TUM_VI": + acc_data, gyro_data, IMUtimestamps = load_IMU_datas_TUM_VI(args.dataset) + + dest_depth = os.path.join(args.dest, "depth") + dest_pose = os.path.join(args.dest, "pose") + + create_dir(dest_depth) + create_dir(dest_pose) + + states = [] + errors = [] + + + #finds first useful imu data, assuming imu starts recording way before camera + firstIMU=0 + while(IMUtimestamps[firstIMU]<=timestamps[0]): + firstIMU += 1 + firstIMU -= 1 + + imu = [] # array of valid imu measurments: one imu measure is 7 floats [acc x, acc y, acc z, gyro x, gyro y, gyro z, timestamp] + + with tqdm(total=num_images) as pbar: + for idx, image_name in enumerate(image_filenames): + # TODO: it is image loader duty to provide correct images + image = cv2.imread(image_name) + if image is None: + raise ValueError(f"failed to load image {image_name}") + + image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + + imu.clear() #clear imu measures from last frame + + + + if idx > 0: #select only those imu meas that occour in time before the current frame + while(IMUtimestamps[firstIMU]<=timestamps[idx]): + imu_valid_meas = (acc_data[firstIMU] + gyro_data[firstIMU]) + imu_valid_meas.append(IMUtimestamps[firstIMU]) + imu.append(imu_valid_meas) + firstIMU += 1 + + + state = app.process_image_imu_mono(image, timestamps[idx], np.array(imu)) + + # NOTE: we buid a default invalid depth, in the case of system failure + if state == slampy.State.OK: + depth = app.get_depth() + pose_past_frame_to_current = app.get_pose_to_target( + precedent_frame=args.pose_id + ) + name = os.path.splitext(os.path.basename(image_name))[0] + + depth_path = os.path.join(dest_depth, name) + save_depth(depth_path, depth) + + pose_path = os.path.join(dest_pose, name) + save_pose(pose_path, pose_past_frame_to_current) + + curr_pose = app.get_pose_to_target(-1) + if curr_pose is not None: + save_pose_txt(args, name, curr_pose) + + if args.is_evaluate_depth: + gt_file_path = os.path.join(args.gt_depth, "{}.png".format(name)) + err = get_error(args, name, depth, gt_file_path) + errors.append(err) + + states.append(state) + pbar.update(1) + + if args.is_evaluate_depth: + mean_errors = np.array(errors).mean(0) + save_results = os.path.join(args.dest, "results.txt") + save_depth_err_results(save_results, "mean values", mean_errors) + + + # NOTE: final dump of log.txt file + with open(os.path.join(args.dest, "log.txt"), "w") as f: + for i, state in enumerate(states): + f.write(f"{i}: {state}\n") + + if args.is_evaluate_pose: + print("Begin to evaluate predicted pose") + evaluate_pose(args) + eval_tool = KittiEvalOdom() + eval_tool.eval(args) + + +parser = argparse.ArgumentParser( + description="Run the SLAM system and save, at each frame, the current depth and pose" +) + +parser.add_argument( + "--dataset", + type=str, + default="/media/Datasets/KITTI_VO/dataset/sequences/10", + help="path to dataset", +) +parser.add_argument( + "--settings", + type=str, + default="./settings_kitty.yaml", + help="which configuration?", +) +parser.add_argument( + "--dest", + type=str, + default="./results_kitty_vo_10", + help="where do we save artefacts?", +) +parser.add_argument( + "--pose_id", + type=int, + default=-1, + help="between which frames do you want compute the pose? If pose_id==-1, get the pose between 0->T; \ + if pose_id >0, compute the pose between T-pose_id->T \ + For instance, if pose_id=2 then compute the pose between T-2->T", +) +parser.add_argument( + "--is_evaluate_depth", + default=False, + action="store_true", + help="If set, will evalute the orb depth with the gt files ", +) + +parser.add_argument( + "--is_evaluate_pose", + default=False, + action="store_true", + help="If set, will evalute the orb pose with the gt files", +) + +parser.add_argument( + "--is_bash", + # default=True, + action="store_true", + help="If set, means use bash shell to evaluate", +) + +parser.add_argument( + "--data_type", + type=str, + help="which dataset type", + default="KITTI_VO", + choices=["TUM", "KITTI_VO", "KITTI","OTHERS","TUM_VI"], +) + +parser.add_argument( + "--gt_depth", + type=str, + help="the gt depth files of the dataset", + default="/media/Datasets/KITTI_VO_SGM/10/depth", +) + +parser.add_argument( + "--gt_pose_dir", + type=str, + help="each frame's gt pose file, saved as previous to current, and filename as current.npy", + default="/media/Datasets/KITTI_VO_SGM/10/npy_pose", +) + +parser.add_argument( + "--gt_pose_txt", + type=str, + help="this is the gt pose file provided by kitty or tum.", + default="/media/Datasets/KITTI_VO/dataset/poses/10.txt", +) + +parser.add_argument( + "--align", + type=str, + choices=["scale", "scale_7dof", "7dof", "6dof"], + default="7dof", + help="alignment type", +) + +parser.add_argument( + "--named", type=str, help="the names for saving pose", default="kitty_vo_10" +) + + +if __name__ == "__main__": + + args = parser.parse_args() + run(args) diff --git a/settings_TUM_VI.yaml b/settings_TUM_VI.yaml new file mode 100644 index 0000000..a602dfb --- /dev/null +++ b/settings_TUM_VI.yaml @@ -0,0 +1,28 @@ +SLAM.alg: "OrbSlam3" +#-------------------------------------------------------------------------------------------- +# SLAM Method: change with the name of the .py file in whitch there are the class SLAM +#-------------------------------------------------------------------------------------------- + +#-------------------------------------------------------------------------------------------- +# SLAM Params: add your own to run the algorithm +#-------------------------------------------------------------------------------------------- +SLAM.vocab_path: "slam_method/Settings/ORBvoc.bin" +SLAM.settings_path: "slam_method/Settings/TUM_512_cameraIMU.yaml" + +#-------------------------------------------------------------------------------------------- +# Drawer Params: the extra params of trajectory_drawer class +# Params Guide https://slampy.readthedocs.io/en/latest/config.html +#-------------------------------------------------------------------------------------------- + +Drawer.eye.x: -18.0 +Drawer.eye.y: -13.0 +Drawer.eye.z: -55.0 +Drawer.center.x: -17.0 +Drawer.center.y: -8.0 +Drawer.scale_grade.x: 1.0 +Drawer.scale_grade.y: 1.0 +Drawer.scale_grade.z: 10.0 +Drawer.aspectratio.x: 50 +Drawer.aspectratio.y: 50 +Drawer.aspectratio.z: 100 +Drawer.point_size: 2 diff --git a/slam_method/OrbSlam3.py b/slam_method/OrbSlam3.py index 936719a..e9de60f 100644 --- a/slam_method/OrbSlam3.py +++ b/slam_method/OrbSlam3.py @@ -68,15 +68,14 @@ def process_image_stereo(self, image_left, image_right, tframe): def process_image_imu_mono(self, image, tframe, imu): if self.sensor_type == Sensor.MONOCULAR_IMU: - self.slam.process_image_imu_mono(image, tframe, imu, "0") + self.slam.process_image_imu_mono(image, tframe, "0", imu) else: raise Exception("The sensor type is not MONOCULAR_IMU") def process_image_imu_stereo(self, image_left, image_right, tframe, imu): if self.sensor_type == Sensor.STEREO_IMU: self.slam.process_image_imu_stereo( - image_left, image_right, tframe, imu, "0" - ) + image_left, image_right, tframe, "0", imu) else: raise Exception("The sensor type is not STREO_IMU") diff --git a/slam_method/Settings/TUM_512_cameraIMU.yaml b/slam_method/Settings/TUM_512_cameraIMU.yaml new file mode 100755 index 0000000..cd782fc --- /dev/null +++ b/slam_method/Settings/TUM_512_cameraIMU.yaml @@ -0,0 +1,91 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +Camera.type: "KannalaBrandt8" +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 190.978477 # 190.97847715128717 +Camera.fy: 190.973307 # 190.9733070521226 +Camera.cx: 254.931706 # 254.93170605935475 +Camera.cy: 256.897442 # 256.8974428996504 + +# Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182 +#Camera.bFishEye: 1 +Camera.k1: 0.003482389402 # 0.0034823894022493434 +Camera.k2: 0.000715034845 # 0.0007150348452162257 +Camera.k3: -0.002053236141 # -0.0020532361418706202 +Camera.k4: 0.000202936736 # 0.00020293673591811182 + +# Camera resolution +Camera.width: 512 +Camera.height: 512 + +# Camera frames per second +Camera.fps: 20.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Transformation from body-frame (imu) to camera +Tbc: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [-0.9995250378696743, 0.0075019185074052044, -0.02989013031643309, 0.045574835649698026, + 0.029615343885863205, -0.03439736061393144, -0.998969345370175, -0.071161801837997044, + -0.008522328211654736, -0.9993800792498829, 0.03415885127385616, -0.044681254117144367, + 0.0, 0.0, 0.0, 1.0] +# Tbc: !!opencv-matrix # from vins mono calibration file +# rows: 4 +# cols: 4 +# dt: f +# data: [-0.9995250378696743, 0.0075842033363785165, -0.030214670573904204, 0.044511917113940799, +# 0.029940114644659861, -0.034023430206013172, -0.99897246995704592, -0.073197096234105752, +# -0.0086044170750674241, -0.99939225835343004, 0.033779845322755464, -0.047972907300764499, +# 0.0, 0.0, 0.0, 1.0] + + +# IMU noise (Use those from VINS-mono) +IMU.NoiseGyro: 0.00016 # rad/s^0.5 +IMU.NoiseAcc: 0.0028 # m/s^1.5 +IMU.GyroWalk: 0.000022 # rad/s^1.5 +IMU.AccWalk: 0.00086 # m/s^2.5 +IMU.Frequency: 200 + + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1500 # Tested with 1250 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +# ORBextractor.iniThFAST: 20 +# ORBextractor.minThFAST: 7 +ORBextractor.iniThFAST: 20 # 20 +ORBextractor.minThFAST: 7 # 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 # -1.8 +Viewer.ViewpointF: 500 diff --git a/utils.py b/utils.py index e13ab33..2121800 100644 --- a/utils.py +++ b/utils.py @@ -233,10 +233,7 @@ def save_pose_txt(args, name, pose): pose_file_path = os.path.join(args.dest, "pose.txt") fp = open(pose_file_path, "a") pose34 = pose[:3] -<<<<<<< HEAD -======= ->>>>>>> 1b77b035e07f95e8d286dcf23192f7103c139f23 fp.write(name) for row in pose34: fp.write(" ") @@ -455,3 +452,44 @@ def load_images_OTHERS(path_to_sequence): return [os.path.join(path_to_sequence,"data",name) for name in framenames], timestamps + +def load_images_TUM_VI(path_to_sequence): + + """ This loader is created for Visual Inertial TUM datasets. Format of such datasets is: + path_to_sequence/mav0/cam0/+data/xxxx.png + /-times.txt + """ + timestamps = [] + framenames = [] + + with open(os.path.join(path_to_sequence,"mav0/cam0/times.txt")) as times_file: + for line in times_file: + if len(line) > 0 and not line.startswith("#"): + framenames.append(line.split()[0] + ".png") + timestamps.append(float(line.split()[1])) + + return [os.path.join(path_to_sequence,"mav0/cam0/data",name) for name in framenames], timestamps + + + + +def load_IMU_datas_TUM_VI(path_to_sequence): + timestamp = [] + gyro_data = [] + acc_data = [] + with open(os.path.join(path_to_sequence,"mav0/imu0/data.csv")) as imu_file: + for line in imu_file: + if len(line) > 0 and not line.startswith("#"): + imu_line=line.split(",") + timestamp.append(float(imu_line[0])*10e-9) + gyro_data.append([float(imu_line[1]),float(imu_line[2]),float(imu_line[3])]) + acc_data.append([float(imu_line[4]),float(imu_line[5]),float(imu_line[6])]) + + return acc_data,gyro_data,timestamp + + + + + + + From e69a6325188b20cf111c2af3b11ba1b127d9c8e2 Mon Sep 17 00:00:00 2001 From: GabHoo <56197287+GabHoo@users.noreply.github.com> Date: Tue, 9 Feb 2021 15:24:34 +0100 Subject: [PATCH 10/13] Update utils.py small fix: imu timestamp to sec --- utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utils.py b/utils.py index f8cc436..3396dd1 100644 --- a/utils.py +++ b/utils.py @@ -480,7 +480,7 @@ def load_IMU_datas_TUM_VI(path_to_sequence): for line in imu_file: if len(line) > 0 and not line.startswith("#"): imu_line=line.split(",") - timestamp.append(float(imu_line[0])*10e-9) + timestamp.append(float(imu_line[0])*1e-9) gyro_data.append([float(imu_line[1]),float(imu_line[2]),float(imu_line[3])]) acc_data.append([float(imu_line[4]),float(imu_line[5]),float(imu_line[6])]) From 749ce992c042555562c5ef056ffda1d5f6f9f1a1 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Thu, 11 Feb 2021 01:27:48 +0100 Subject: [PATCH 11/13] Added loaders and settings for Euroc Inertial Vicon_Room2 dataset --- run_MONO_IMU.py | 9 ++- settings_EuRoC.yaml | 28 ++++++++ slam_method/Settings/EuRoC_ViconRoom2.yaml | 78 ++++++++++++++++++++++ utils.py | 22 +++++- 4 files changed, 131 insertions(+), 6 deletions(-) create mode 100644 settings_EuRoC.yaml create mode 100755 slam_method/Settings/EuRoC_ViconRoom2.yaml diff --git a/run_MONO_IMU.py b/run_MONO_IMU.py index f762186..a9e53a9 100644 --- a/run_MONO_IMU.py +++ b/run_MONO_IMU.py @@ -34,12 +34,14 @@ def run(args): if args.data_type == "TUM_VI": image_filenames, timestamps = load_images_TUM_VI(args.dataset) + elif args.data_type == "EUROC": + image_filenames, timestamps = load_images_EuRoC(args.dataset) elif args.data_type == "OTHERS": image_filenames, timestamps = load_images_OTHERS(args.dataset) num_images = len(image_filenames) - if args.data_type == "TUM_VI": + if args.data_type == "TUM_VI" or args.data_type =="EUROC": acc_data, gyro_data, IMUtimestamps = load_IMU_datas_TUM_VI(args.dataset) dest_depth = os.path.join(args.dest, "depth") @@ -80,7 +82,6 @@ def run(args): imu.append(imu_valid_meas) firstIMU += 1 - state = app.process_image_imu_mono(image, timestamps[idx], np.array(imu)) # NOTE: we buid a default invalid depth, in the case of system failure @@ -125,6 +126,8 @@ def run(args): evaluate_pose(args) eval_tool = KittiEvalOdom() eval_tool.eval(args) + + app.shutdown() parser = argparse.ArgumentParser( @@ -183,7 +186,7 @@ def run(args): type=str, help="which dataset type", default="KITTI_VO", - choices=["TUM", "KITTI_VO", "KITTI","OTHERS","TUM_VI"], + choices=["TUM", "KITTI_VO", "KITTI","OTHERS","TUM_VI","EUROC"], ) parser.add_argument( diff --git a/settings_EuRoC.yaml b/settings_EuRoC.yaml new file mode 100644 index 0000000..74636a2 --- /dev/null +++ b/settings_EuRoC.yaml @@ -0,0 +1,28 @@ +SLAM.alg: "OrbSlam3" +#-------------------------------------------------------------------------------------------- +# SLAM Method: change with the name of the .py file in whitch there are the class SLAM +#-------------------------------------------------------------------------------------------- + +#-------------------------------------------------------------------------------------------- +# SLAM Params: add your own to run the algorithm +#-------------------------------------------------------------------------------------------- +SLAM.vocab_path: "slam_method/Settings/ORBvoc.bin" +SLAM.settings_path: "slam_method/Settings/EuRoC_ViconRoom2.yaml" + +#-------------------------------------------------------------------------------------------- +# Drawer Params: the extra params of trajectory_drawer class +# Params Guide https://slampy.readthedocs.io/en/latest/config.html +#-------------------------------------------------------------------------------------------- + +Drawer.eye.x: -18.0 +Drawer.eye.y: -13.0 +Drawer.eye.z: -55.0 +Drawer.center.x: -17.0 +Drawer.center.y: -8.0 +Drawer.scale_grade.x: 1.0 +Drawer.scale_grade.y: 1.0 +Drawer.scale_grade.z: 10.0 +Drawer.aspectratio.x: 50 +Drawer.aspectratio.y: 50 +Drawer.aspectratio.z: 100 +Drawer.point_size: 2 diff --git a/slam_method/Settings/EuRoC_ViconRoom2.yaml b/slam_method/Settings/EuRoC_ViconRoom2.yaml new file mode 100755 index 0000000..6b1be4f --- /dev/null +++ b/slam_method/Settings/EuRoC_ViconRoom2.yaml @@ -0,0 +1,78 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 458.654 +Camera.fy: 457.296 +Camera.cx: 367.215 +Camera.cy: 248.375 + +Camera.k1: -0.28340811 +Camera.k2: 0.07395907 +Camera.p1: 0.00019359 +Camera.p2: 1.76187114e-05 + +# Camera resolution +Camera.width: 752 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 20.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Transformation from camera to body-frame (imu) +Tbc: !!opencv-matrix + rows: 4 + cols: 4 + dt: f + data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, + 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, + -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, + 0.0, 0.0, 0.0, 1.0] + +# IMU noise +IMU.NoiseGyro: 1.7e-4 #1.6968e-04 +IMU.NoiseAcc: 2.0000e-3 #2.0e-3 +IMU.GyroWalk: 1.9393e-05 +IMU.AccWalk: 3.0000e-03 # 3e-03 +IMU.Frequency: 200 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 2000 # 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -3.5 # -1.8 +Viewer.ViewpointF: 500 diff --git a/utils.py b/utils.py index f8cc436..edcd41a 100644 --- a/utils.py +++ b/utils.py @@ -470,17 +470,33 @@ def load_images_TUM_VI(path_to_sequence): return [os.path.join(path_to_sequence,"mav0/cam0/data",name) for name in framenames], timestamps - +def load_images_EuRoC(path_to_sequence): + + """ This loader is created for Visual Inertial EuRoC datasets. Format of such datasets is: + path_to_sequence/mav0/cam0/+data/xxxx.png + /-times.txt + """ + timestamps = [] + framenames = [] + + with open(os.path.join(path_to_sequence,"mav0/cam0/data.txt")) as times_file: + for line in times_file: + if len(line) > 0 and not line.startswith("#"): + framenames.append(line.split(",")[1].rstrip()) + timestamps.append(float(line.split(",")[0])*1e-9) + + return [os.path.join(path_to_sequence,"mav0/cam0/data",name) for name in framenames], timestamps + def load_IMU_datas_TUM_VI(path_to_sequence): timestamp = [] gyro_data = [] acc_data = [] - with open(os.path.join(path_to_sequence,"mav0/imu0/data.csv")) as imu_file: + with open(os.path.join(path_to_sequence,"mav0/imu0/data.txt")) as imu_file: for line in imu_file: if len(line) > 0 and not line.startswith("#"): imu_line=line.split(",") - timestamp.append(float(imu_line[0])*10e-9) + timestamp.append(float(imu_line[0])*1e-9) gyro_data.append([float(imu_line[1]),float(imu_line[2]),float(imu_line[3])]) acc_data.append([float(imu_line[4]),float(imu_line[5]),float(imu_line[6])]) From 63d4df2579e40d316cd34eb314d5e654a0c4c0f2 Mon Sep 17 00:00:00 2001 From: Gabriel Date: Fri, 19 Feb 2021 19:20:05 +0100 Subject: [PATCH 12/13] Add save_pose_and_time_txt --- utils.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/utils.py b/utils.py index edcd41a..fb27b10 100644 --- a/utils.py +++ b/utils.py @@ -242,6 +242,31 @@ def save_pose_txt(args, name, pose): fp.close() + + +def save_pose_and_times_txt(args, name, pose): + """Save pose and time in two different txt files.""" + + time_file_path = os.path.join(args.dest, "times.txt") + pose_file_path = os.path.join(args.dest, "pose.txt") + fd = open(time_file_path, "a") + fp = open(pose_file_path, "a") + pose34 = pose[:3] + fd.write(name) + fd.write("\n") + fd.close() + line="" + for row in pose34: + line+=(" ".join(str(round(i, 10)) for i in row)) + line+=(" ") + line=line[:-1] + line+=("\n") + fp.write(line) + # fp.write('\n') + fp.close() + + + def evaluate_pose(args): """Evaluate odometry on the KITTI dataset""" orb_pose_dir = os.path.join(args.dest, "pose") From 5049f4a2c9ad5f9ae398f8a8f98d199eafb1f919 Mon Sep 17 00:00:00 2001 From: GabHoo <56197287+GabHoo@users.noreply.github.com> Date: Mon, 1 Mar 2021 00:56:22 +0100 Subject: [PATCH 13/13] Update utils.py fix format for proper loading --- utils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utils.py b/utils.py index fb27b10..b71b3ee 100644 --- a/utils.py +++ b/utils.py @@ -504,7 +504,7 @@ def load_images_EuRoC(path_to_sequence): timestamps = [] framenames = [] - with open(os.path.join(path_to_sequence,"mav0/cam0/data.txt")) as times_file: + with open(os.path.join(path_to_sequence,"mav0/cam0/data.csv")) as times_file: for line in times_file: if len(line) > 0 and not line.startswith("#"): framenames.append(line.split(",")[1].rstrip()) @@ -517,7 +517,7 @@ def load_IMU_datas_TUM_VI(path_to_sequence): timestamp = [] gyro_data = [] acc_data = [] - with open(os.path.join(path_to_sequence,"mav0/imu0/data.txt")) as imu_file: + with open(os.path.join(path_to_sequence,"mav0/imu0/data.csv")) as imu_file: for line in imu_file: if len(line) > 0 and not line.startswith("#"): imu_line=line.split(",")