Merge pull request GiordanoLaminetti#3 from GabHoo/master

generic loader and small fixes

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(

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
+

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
+