diff --git a/run.py b/run.py index edcb94a..7df0e2b 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,27 @@ 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,44 +61,47 @@ 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() 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) + 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) + curr_pose = app.get_pose_to_target(-1) if curr_pose is not None: save_pose_txt(args, name, curr_pose) - if args.is_evalute_depth: + 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) - - 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_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_evalute_pose: + if args.is_evaluate_pose: print("Begin to evaluate predicted pose") evaluate_pose(args) eval_tool = KittiEvalOdom() @@ -123,15 +139,15 @@ def run(args): For instance, if pose_id=2 then compute the pose between T-2->T", ) parser.add_argument( - "--is_evalute_depth", - default=True, + "--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", - default=True, + "--is_evaluate_pose", + default=False, action="store_true", help="If set, will evalute the orb pose with the gt files", ) @@ -148,7 +164,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/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..e13ab33 100644 --- a/utils.py +++ b/utils.py @@ -232,8 +232,11 @@ 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] +<<<<<<< HEAD +======= +>>>>>>> 1b77b035e07f95e8d286dcf23192f7103c139f23 fp.write(name) for row in pose34: fp.write(" ") @@ -374,6 +377,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 +420,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 +