merge vizualisation and KITTI conversion in one step

anafi_metadata.py

@@ -44,17 +44,19 @@ def preprocess_metadata(metadata, proj, centroid):
             if metadata["location_valid"].iloc[0] == 0:
                 end = validity_start.pop(0)
                 positions[:end] = extrapolate_position(speed[:end], timestamps[:end], None, positions[end])
-                print(positions[:end], end)
             if metadata["location_valid"].iloc[-1] == 0:
                 start = invalidity_start.pop(-1) - 1
                 positions[start:] = extrapolate_position(speed[start:], timestamps[start:], positions[start], None)
 
             if(len(invalidity_start) != len(validity_start)):
-                print("error")
+                raise ValueError("error, invalidity_start ({}) and validity_start({})"
+                                 " are not the same length ({} vs {})".format(invalidity_start,
+                                                                              validity_start,
+                                                                              len(invalidity_start),
+                                                                              len(validity_start)))
 
             for start, end in zip(invalidity_start, validity_start):
                 positions[start:end] = extrapolate_position(speed[start:end], timestamps[start:end], positions[start], positions[end])
-        print(positions)
         positions -= centroid
 
     metadata["x"], metadata["y"], metadata["z"] = positions.transpose()

viz_depth.py → convert_dataset.py

@@ -3,6 +3,7 @@ from path import Path
 from imageio import imread, imwrite
 from skimage.transform import rescale
 from skimage.measure import block_reduce
+from colmap_util import read_model as rm
 import numpy as np
 from matplotlib import cm
 from matplotlib.colors import ListedColormap, LinearSegmentedColormap
@@ -10,6 +11,52 @@ from tqdm import tqdm
 from wrappers import FFMpeg
 import gzip
 from pebble import ProcessPool
+import pandas as pd
+
+
+def save_intrinsics(cameras, images, output_dir, downscale=1):
+    def construct_intrinsics(cam):
+        assert('PINHOLE' in cam.model)
+        if 'SIMPLE' in cam.model:
+            fx, cx, cy = cam.params
+            fy = fx
+        else:
+            fx, fy, cx, cy = cam.params
+
+        return np.array([[fx / downscale, 0, cx / downscale],
+                         [0, fy / downscale, cy / downscale],
+                         [0, 0, 1]])
+
+    if len(cameras) == 1:
+        cam = cameras[list(cameras.keys())[0]]
+        intrinsics = construct_intrinsics(cam)
+        np.savetxt(output_dir / 'intrinsics.txt', intrinsics)
+    else:
+        for _, img in images.items():
+            cam = cameras[img.camera_id]
+            intrinsics = construct_intrinsics(cam)
+            intrinsics_name = output_dir / Path(img.name).namebase + "_intrinsics.txt"
+            np.savetxt(intrinsics_name, intrinsics)
+
+
+def to_transform_matrix(q, t):
+    cam_R = rm.qvec2rotmat(q).T
+    cam_t = (- cam_R @ t).reshape(3, 1)
+    transform = np.vstack((np.hstack([cam_R, cam_t]), [0, 0, 0, 1]))
+    return transform
+
+
+def save_positions(images, output_dir):
+    starting_pos = None
+    positions = []
+    for _, img in images.items():
+        current_pos = to_transform_matrix(img.qvec, img.tvec)
+        if starting_pos is None:
+            starting_pos = current_pos
+        relative_position = np.linalg.inv(starting_pos) @ current_pos
+        positions.append(relative_position[:3])
+    positions = np.stack(positions)
+    np.savetxt(output_dir/'poses.txt', positions.reshape((len(images), -1)))
 
 
 def high_res_colormap(low_res_cmap, resolution=1000, max_value=1):
@@ -53,19 +100,29 @@ def apply_cmap_and_resize(depth, colormap, downscale):
 
     depth_viz = COLORMAPS[colormap](depth_norm)[:, :, :3]
     depth_viz[downscale_depth == np.inf] = 0
-    return depth_viz*255
+    return downscale_depth, depth_viz*255
 
 
-def process_one_frame(img_path, depth_path, occ_path, output_dir, downscale):
+def process_one_frame(img_path, depth_path, occ_path,
+                      dataset_output_dir, video_output_dir, downscale, interpolated):
     img = imread(img_path)
     h, w, _ = img.shape
     assert((h/downscale).is_integer() and (w/downscale).is_integer())
     output_img = np.zeros((2*(h//downscale), 2*(w//downscale), 3), dtype=np.uint8)
-    output_img[:h//downscale, :w//downscale] = rescale(img, 1/downscale, multichannel=True)*255
+    rescaled_img = rescale(img, 1/downscale, multichannel=True)*255
+    # Img goes to upper left corner of vizualisation
+    output_img[:h//downscale, :w//downscale] = rescaled_img
+    imwrite(dataset_output_dir / img_path.basename(), rescaled_img.astype(np.uint8))
     if depth_path is not None:
         with gzip.open(depth_path, "rb") as f:
             depth = np.frombuffer(f.read(), np.float32).reshape(h, w)
-        output_img[:h//downscale, w//downscale:] = apply_cmap_and_resize(depth, 'rainbow', downscale)
+        output_depth_name = dataset_output_dir / img_path.basename() + '.npy'
+        downscaled_depth, viz = apply_cmap_and_resize(depth, 'rainbow', downscale)
+        if not interpolated:
+            np.save(output_depth_name, downscaled_depth)
+        # Depth colormap goes to upper right corner
+        output_img[:h//downscale, w//downscale:] = viz
+        # Mix Depth / image goest to lower left corner
         output_img[h//downscale:, :w//downscale] = \
             output_img[:h//downscale, :w//downscale]//2 + \
             output_img[:h//downscale, w//downscale:]//2
@@ -73,9 +130,13 @@ def process_one_frame(img_path, depth_path, occ_path, output_dir, downscale):
     if occ_path is not None:
         with gzip.open(occ_path, "rb") as f:
             occ = np.frombuffer(f.read(), np.float32).reshape(h, w)
-        output_img[h//downscale:, w//downscale:] = apply_cmap_and_resize(occ, 'bone', downscale)
+        _, occ_viz = apply_cmap_and_resize(occ, 'bone', downscale)
+        # Occlusion depthmap vizualisation goes to lower right corner
+        output_img[h//downscale:, w//downscale:] = occ_viz
+    if interpolated:
+        output_img[:5] = output_img[-5:] = output_img[:, :5] = output_img[:, -5:] = [255, 128, 0]
 
-    imwrite(output_dir/img_path.namebase + '.png', output_img)
+    imwrite(video_output_dir/img_path.namebase + '.png', output_img)
 
 
 parser = ArgumentParser(description='create a vizualisation from ground truth created',
@@ -84,36 +145,65 @@ parser = ArgumentParser(description='create a vizualisation from ground truth cr
 parser.add_argument('--depth_dir', metavar='DIR', type=Path)
 parser.add_argument('--img_dir', metavar='DIR', type=Path)
 parser.add_argument('--occ_dir', metavar='DIR', type=Path)
+parser.add_argument('--metdata', type=Path)
 parser.add_argument('--output_dir', metavar='DIR', default=None, type=Path)
 parser.add_argument('--video', action='store_true')
 parser.add_argument('--fps', default='1')
 parser.add_argument('--downscale', type=int, default=1)
 
 
-def process_viz(depth_dir, img_dir, occ_dir, output_dir, video, fps, downscale, ffmpeg, threads=8, **env):
-    imgs = sorted(img_dir.files('*.jpg')) + sorted(img_dir.files('*.JPG'))
+def convert_dataset(final_model, depth_dir, images_root_folder, occ_dir, dataset_output_dir, video_output_dir, metadata_path, interpolated_frames_path,
+                    fps, downscale, ffmpeg, threads=8, video=False, **env):
+    dataset_output_dir.makedirs_p()
+    video_output_dir.makedirs_p()
+    cameras, images, _ = rm.read_model(final_model, '.txt')
+    save_intrinsics(cameras, images, dataset_output_dir, downscale)
+    save_positions(images, dataset_output_dir)
+    if interpolated_frames_path is None:
+        interpolated_frames = []
+    else:
+        with open(interpolated_frames_path, "r") as f:
+            interpolated_frames = [line[:-1] for line in f.readlines()]
+
+    metadata = pd.read_csv(metadata_path).set_index("db_id", drop=False).sort_values("time")
+    image_df = pd.DataFrame.from_dict(images, orient="index").set_index("id")
+    image_df = image_df.reindex(metadata.index)
     depth_maps = []
     occ_maps = []
-    for i in imgs:
-        fname = i.basename()
+    interpolated = []
+    imgs = []
+    cameras = []
+
+    for i in metadata["image_path"]:
+        img_path = images_root_folder / i
+        imgs.append(img_path)
+
+        fname = img_path.basename()
         depth_path = depth_dir / fname + ".gz"
         if depth_path.isfile():
             depth_maps.append(depth_path)
         else:
             print("Image {} was not registered".format(fname))
             depth_maps.append(None)
+        if i in interpolated_frames:
+            interpolated.append(True)
+            print("Image {} was interpolated".format(fname))
+        else:
+            interpolated.append(False)
+
         occ_path = occ_dir / fname + ".gz"
         if occ_path.isfile():
             occ_maps.append(occ_path)
         else:
             occ_maps.append(None)
-    output_dir.makedirs_p()
     if threads == 1:
-        for i, d, o in tqdm(zip(imgs, depth_maps, occ_maps), total=len(imgs)):
-            process_one_frame(i, d, o, output_dir, downscale)
+        for i, d, o, n in tqdm(zip(imgs, depth_maps, occ_maps, interpolated), total=len(imgs)):
+            process_one_frame(i, d, o, dataset_output_dir, video_output_dir, downscale, n)
     else:
         with ProcessPool(max_workers=threads) as pool:
-            tasks = pool.map(process_one_frame, imgs, depth_maps, occ_maps, [output_dir]*len(imgs), [downscale]*len(imgs))
+            tasks = pool.map(process_one_frame, imgs, depth_maps, occ_maps,
+                             [dataset_output_dir]*len(imgs), [video_output_dir]*len(imgs),
+                             [downscale]*len(imgs), interpolated)
             try:
                 for _ in tqdm(tasks.result(), total=len(imgs)):
                     pass
@@ -122,8 +212,8 @@ def process_viz(depth_dir, img_dir, occ_dir, output_dir, video, fps, downscale,
                 raise e
 
     if video:
-        video_path = str(output_dir/'{}_groundtruth_viz.mp4'.format(output_dir.namebase))
-        glob_pattern = str(output_dir/'*.png')
+        video_path = str(video_output_dir/'{}_groundtruth_viz.mp4'.format(video_output_dir.namebase))
+        glob_pattern = str(video_output_dir/'*.png')
         ffmpeg.create_video(video_path, glob_pattern, fps)
 
 
@@ -131,4 +221,4 @@ if __name__ == '__main__':
     args = parser.parse_args()
     env = vars(args)
     env["ffmpeg"] = FFMpeg()
-    process_viz(**env)
+    convert_dataset(**env)

filter_colmap_model.py

@@ -11,30 +11,13 @@ parser = ArgumentParser(description='Take all the drone videos of a folder and p
                                     'location in a COLMAP file for vizualisation',
                         formatter_class=ArgumentDefaultsHelpFormatter)
 
-parser.add_argument('--colmap_model_folder', metavar='DIR', type=Path)
-parser.add_argument('--output_folder_model', metavar='DIR', type=Path)
+parser.add_argument('--input_images_colmap', metavar='FILE', type=Path)
+parser.add_argument('--output_images_colmap', metavar='FILE', type=Path)
 parser.add_argument('--interpolate', action="store_true")
-parser.add_argument('--metadata', metavar='DIR', type=Path)
+parser.add_argument('--metadata', metavar='FILE', type=Path)
 parser.add_argument('--visualize', action="store_true")
 
 
-def world_to_colmap(frame_qvec, frame_tvec):
-    '''
-    frame_qvec is written in the NED system (north east down)
-    frame_tvec is already is the world system (east norht up)
-    '''
-    world2NED = np.float32([[0, 1, 0],
-                            [1, 0, 0],
-                            [0, 0, -1]])
-    NED2cam = np.float32([[0, 1, 0],
-                          [0, 0, 1],
-                          [1, 0, 0]])
-    world2cam = NED2cam @ rm.qvec2rotmat(frame_qvec).T @ world2NED
-    cam_tvec = - world2cam  @ frame_tvec
-    cam_qvec = rm.rotmat2qvec(world2cam)
-    return cam_qvec, cam_tvec
-
-
 '''
 def colmap_to_world(tvec, qvec):
     if any(np.isnan(qvec)):
@@ -51,6 +34,11 @@ def colmap_to_world(tvec, qvec):
     return -quats.apply(tvec, inverse=True)
 
 
+def world_to_colmap(tvec, qvec):
+    quats = Rotation.from_quat(qvec)
+    return -quats.apply(tvec, inverse=False)
+
+
 def NEDtoworld(qvec):
     world2NED = np.float32([[0, 1, 0],
                             [1, 0, 0],
@@ -77,8 +65,8 @@ def get_outliers(series, threshold):
 
 def slerp_quats(quat_df, prefix=""):
     valid_df = quat_df[~quat_df["outlier"]]
-    valid_index = valid_df["time"]
-    total_index = quat_df["time"]
+    valid_index = valid_df.index
+    total_index = quat_df.index
 
     # Note that scipy uses a different order convention than colmap
     quats = Rotation.from_quat(valid_df[["{}q{}".format(prefix, col) for col in list("xyzw")]].values)
@@ -88,12 +76,17 @@ def slerp_quats(quat_df, prefix=""):
     return pd.DataFrame(slerp(total_index).as_quat(), index=quat_df.index)
 
 
-def filter_colmap_model(input, output, metadata_path,
+def filter_colmap_model(input_images_colmap, output_images_colmap, metadata_path,
                         filter_degree=3, filter_time=0.1,
                         threshold_t=0.01, threshold_q=5e-3,
-                        visualize=False, **env):
-    images_dict = rm.read_images_text(input / "images.txt")
-    metadata = pd.read_csv(metadata_path).set_index("db_id").sort_values("time")
+                        visualize=False, max_interpolate=2, **env):
+    if input_images_colmap.ext == ".txt":
+        images_dict = rm.read_images_text(input_images_colmap)
+    elif input_images_colmap.ext == ".bin":
+        images_dict = rm.read_images_binary(input_images_colmap)
+    else:
+        print(input_images_colmap.ext)
+    metadata = pd.read_csv(metadata_path).set_index("db_id", drop=False).sort_values("time")
     framerate = metadata["framerate"].iloc[0]
     filter_length = 2*int(filter_time * framerate) + 1
 
@@ -101,8 +94,8 @@ def filter_colmap_model(input, output, metadata_path,
     image_df = image_df.reindex(metadata.index)
     metadata["outlier"] = image_df.isna().any(axis="columns")
     colmap_outliers = sum(metadata["outlier"])
+    total_frames = len(metadata)
     image_df = image_df.dropna()
-
     tvec = np.stack(image_df["tvec"].values)
     qvec = np.stack(image_df["qvec"].values)
 
@@ -111,7 +104,6 @@ def filter_colmap_model(input, output, metadata_path,
     # A quaternion is flipped if the dot product is negative
 
     flips = list((np.sum(qvec[1:] * qvec[:-1], axis=1) < 0).nonzero()[0] + 1)
-    print(flips)
     flips.append(qvec.shape[0])
     for j, k in zip(flips[::2], flips[1::2]):
         qvec[j:k] *= -1
@@ -120,8 +112,15 @@ def filter_colmap_model(input, output, metadata_path,
     quat_columns = ["colmap_qw", "colmap_qx", "colmap_qy", "colmap_qz"]
     metadata[tvec_columns] = pd.DataFrame(tvec, index=image_df.index)
     metadata[quat_columns] = pd.DataFrame(qvec, index=image_df.index)
+    metadata["time (s)"] = metadata["time"] / 1e6
+    metadata = metadata.set_index("time (s)")
 
     # Interpolate missing values for tvec and quat
+    # In order to avoid extrapolation, we get rid of outlier at the beginning and the end of the sequence
+
+    first_valid = metadata["outlier"].idxmin()
+    last_valid = metadata["outlier"][::-1].idxmin()
+    metadata = metadata.loc[first_valid:last_valid]
 
     metadata[tvec_columns] = metadata[tvec_columns].interpolate()
     metadata[["colmap_qx", "colmap_qy", "colmap_qz", "colmap_qw"]] = slerp_quats(metadata, prefix="colmap_")
@@ -183,24 +182,46 @@ def filter_colmap_model(input, output, metadata_path,
         colmap_speeds.plot()
         plt.gcf().canvas.set_window_title('speeds from colmap (noisy)')
         metadata[["x", "y", "z", "tx", "ty", "tz"]].plot()
-        plt.gcf().canvas.set_window_title('GPS(xyz) vs colmap position (tx,ty,tz')
+        plt.gcf().canvas.set_window_title('GPS(xyz) vs colmap position (tx,ty,tz)')
         metadata[["ref_qw", "ref_qx", "ref_qy", "ref_qz"]].plot()
         plt.gcf().canvas.set_window_title('quaternions from sensor')
+        ax_q = metadata[["qw", "qx", "qy", "qz"]].plot()
+        plt.gcf().canvas.set_window_title('quaternions from colmap vs from smoothed')
+
+        metadata[["colmap_q{}2".format(col) for col in list('wxyz')]] = metadata[['colmap_qw',
+                                                                                  'colmap_qx',
+                                                                                  'colmap_qy',
+                                                                                  'colmap_qz']].shift()
+        metadata[["q{}2_filtered".format(col) for col in list('wxyz')]] = metadata[['qw_filtered',
+                                                                                    'qx_filtered',
+                                                                                    'qy_filtered',
+                                                                                    'qz_filtered']].shift()
+        metadata = metadata.apply(quaternion_distances(prefix="colmap_"), axis='columns')
+        metadata = metadata.apply(quaternion_distances(suffix="_filtered"), axis='columns')
+        ax_qdist = metadata[["colmap_qdist", "qdist_filtered"]].plot()
+        plt.gcf().canvas.set_window_title('quaternions variations colmap vs filtered vs smoothed')
 
     metadata["outlier"] = metadata["outlier"] | \
         get_outliers(metadata["outliers_pos"], threshold_t) | \
         get_outliers(metadata["outlier_rot"], threshold_q)
+
+    first_valid = metadata["outlier"].idxmin()
+    last_valid = metadata["outlier"][::-1].idxmin()
+    metadata = metadata.loc[first_valid:last_valid]
+
     metadata.loc[metadata["outlier"], ["tx", "ty", "tz", "qw", "qx", "qy", "qz"]] = np.NaN
-    world_tvec_interp = metadata[["tx", "ty", "tz"]].interpolate().values
+    world_tvec_interp = metadata[["tx", "ty", "tz"]].interpolate(method="polynomial", order=3).values
     world_qvec_interp = slerp_quats(metadata).values
 
     world_tvec_smoothed = savgol_filter(world_tvec_interp, filter_length, filter_degree, axis=0)
     qvec_smoothed = savgol_filter(world_qvec_interp, filter_length, filter_degree, axis=0)
     qvec_smoothed /= np.linalg.norm(qvec_smoothed, axis=1, keepdims=True)
 
-    metadata["tx_smoothed"] = world_tvec_smoothed[:, 0]
-    metadata["ty_smoothed"] = world_tvec_smoothed[:, 1]
-    metadata["tz_smoothed"] = world_tvec_smoothed[:, 2]
+    colmap_tvec_smoothed = world_to_colmap(world_tvec_smoothed, qvec_smoothed)
+
+    metadata["tx_smoothed"] = colmap_tvec_smoothed[:, 0]
+    metadata["ty_smoothed"] = colmap_tvec_smoothed[:, 1]
+    metadata["tz_smoothed"] = colmap_tvec_smoothed[:, 2]
 
     metadata["qx_smoothed"] = qvec_smoothed[:, 0]
     metadata["qy_smoothed"] = qvec_smoothed[:, 1]
@@ -208,54 +229,50 @@ def filter_colmap_model(input, output, metadata_path,
     metadata["qw_smoothed"] = qvec_smoothed[:, 3]
 
     if visualize:
-        metadata[["tx_smoothed", "ty_smoothed", "tz_smoothed"]].diff().plot()
+        metadata["world_tx_smoothed"] = world_tvec_smoothed[:, 0]
+        metadata["world_ty_smoothed"] = world_tvec_smoothed[:, 1]
+        metadata["world_tz_smoothed"] = world_tvec_smoothed[:, 2]
+        metadata[["world_tx_smoothed", "world_ty_smoothed", "world_tz_smoothed"]].diff().plot()
         plt.gcf().canvas.set_window_title('speed from filtered and smoothed')
-
-        metadata[["qw", "qx", "qy", "qz",
-                  "qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].plot()
-        plt.gcf().canvas.set_window_title('quaternions from colmap vs from smoothed')
-        metadata[["colmap_q{}2".format(col) for col in list('wxyz')]] = metadata[['colmap_qw',
-                                                                                  'colmap_qx',
-                                                                                  'colmap_qy',
-                                                                                  'colmap_qz']].shift()
-        metadata[["q{}2_filtered".format(col) for col in list('wxyz')]] = metadata[['qw_filtered',
-                                                                                    'qx_filtered',
-                                                                                    'qy_filtered',
-                                                                                    'qz_filtered']].shift()
+        metadata[["qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].plot(ax=ax_q)
         metadata[["q{}2_smoothed".format(col) for col in list('wxyz')]] = metadata[['qw_smoothed',
                                                                                     'qx_smoothed',
                                                                                     'qy_smoothed',
                                                                                     'qz_smoothed']].shift()
-        metadata = metadata.apply(quaternion_distances(prefix="colmap_"), axis='columns')
-        metadata = metadata.apply(quaternion_distances(suffix="_filtered"), axis='columns')
         metadata = metadata.apply(quaternion_distances(suffix="_smoothed"), axis='columns')
-        metadata[["colmap_qdist", "qdist_filtered", "qdist_smoothed"]].plot()
-        plt.gcf().canvas.set_window_title('GPS(xyz) vs colmap position')
+        metadata[["qdist_smoothed"]].plot(ax=ax_qdist)
         metadata[["outlier"]].astype(float).plot()
         plt.gcf().canvas.set_window_title('outliers indices')
 
     print("number of not localized by colmap : {}/{} ({:.2f}%)".format(colmap_outliers,
-                                                                       len(metadata),
-                                                                       100 * colmap_outliers/len(metadata)))
+                                                                       total_frames,
+                                                                       100 * colmap_outliers/total_frames))
     print("Total number of outliers : {} / {} ({:.2f}%)".format(sum(metadata["outlier"]),
-                                                                len(metadata),
-                                                                100 * sum(metadata["outlier"])/len(metadata)))
+                                                                total_frames,
+                                                                100 * sum(metadata["outlier"])/total_frames))
 
     if visualize:
         plt.show()
 
-    smoothed_images_dict = {}
-    for db_id, row in metadata.iterrows():
-        smoothed_images_dict[db_id] = rm.Image(id=db_id,
-                                               qvec=row[["qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].values,
-                                               tvec=row[["tx_smoothed", "ty_smoothed", "tz_smoothed"]].values,
-                                               camera_id=row["camera_id"],
-                                               name=row["image_path"],
-                                               xys=[], point3D_ids=[])
-        rm.write_images_text(smoothed_images_dict, output / "images.txt")
+    if output_images_colmap is not None:
+        smoothed_images_dict = {}
+        interpolated_frames = []
+        for _, row in metadata.iterrows():
+            db_id = row["db_id"]
+            if row["outlier"]:
+                interpolated_frames.append(row["image_path"])
+            smoothed_images_dict[db_id] = rm.Image(id=db_id,
+                                                   qvec=row[["qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].values,
+                                                   tvec=row[["tx_smoothed", "ty_smoothed", "tz_smoothed"]].values,
+                                                   camera_id=row["camera_id"],
+                                                   name=row["image_path"],
+                                                   xys=[], point3D_ids=[])
+        rm.write_images_text(smoothed_images_dict, output_images_colmap)
+
+    return interpolated_frames
 
 
 if __name__ == '__main__':
     args = parser.parse_args()
     env = vars(args)
-    filter_colmap_model(input=args.colmap_model_folder, output=args.output_folder_model, metadata_path=args.metadata, **env)
+    filter_colmap_model(metadata_path=args.metadata, **env)

main_pipeline.py

@@ -118,16 +118,19 @@ def prepare_workspace(path, env):
     env["georefrecon_ply"] = env["georef_recon"] / "georef_reconstruction.ply"
     env["matrix_path"] = env["workspace"] / "matrix_thorough.txt"
     env["indexed_vocab_tree"] = env["workspace"] / "vocab_tree_thorough.bin"
+    env["dense_workspace"] = env["thorough_recon"].parent/"dense"
 
 
-def prepare_video_workspace(video_name, video_frames_folder, output_folder, video_recon, image_path, **env):
+def prepare_video_workspace(video_name, video_frames_folder,
+                            raw_output_folder, converted_output_folder,
+                            video_recon, video_path, **env):
     video_env = {video_name: video_name,
                  video_frames_folder: video_frames_folder}
-    relative_path_folder = video_frames_folder.relpath(image_path)
+    relative_path_folder = video_frames_folder.relpath(video_path)
     video_env["lowfps_db"] = video_frames_folder / "video_low_fps.db"
     video_env["metadata"] = video_frames_folder / "metadata.csv"
     video_env["lowfps_image_list_path"] = video_frames_folder / "lowfps.txt"
-    video_env["chunk_image_list_paths"] = video_frames_folder.files("full_chunk_*.txt")
+    video_env["chunk_image_list_paths"] = sorted(video_frames_folder.files("full_chunk_*.txt"))
     video_env["chunk_dbs"] = [video_frames_folder / fp.namebase + ".db" for fp in video_env["chunk_image_list_paths"]]
     colmap_root = video_recon / relative_path_folder
     video_env["colmap_models_root"] = colmap_root
@@ -137,15 +140,17 @@ def prepare_video_workspace(video_name, video_frames_folder, output_folder, vide
     video_env["chunk_models"] = [colmap_root / "chunk_{}".format(index) for index in range(num_chunks)]
     video_env["final_model"] = colmap_root / "final"
     output = {}
-    output["images_root_folder"] = output_folder / "images"
+    output["images_root_folder"] = raw_output_folder / "images"
     output["video_frames_folder"] = output["images_root_folder"] / relative_path_folder
-    output["viz_folder"] = output_folder / "video" / relative_path_folder
-    output["model_folder"] = output_folder / "models" / relative_path_folder
+    output["model_folder"] = raw_output_folder / "models" / relative_path_folder
+    output["interpolated_frames_list"] = output["model_folder"] / "interpolated_frames.txt"
     output["final_model"] = output["model_folder"] / "final"
     output["video_fps"] = pd.read_csv(video_env["metadata"])["framerate"].values[0]
+    output["kitti_format_folder"] = converted_output_folder / "KITTI" / relative_path_folder
+    output["viz_folder"] = converted_output_folder / "video" / relative_path_folder
     video_env["output_env"] = output
     video_env["already_localized"] = env["resume_work"] and output["model_folder"].isdir()
-    video_env["GT_already_done"] = env["resume_work"] and (output_folder / "groundtruth_depth" / video_name.namebase).isdir()
+    video_env["GT_already_done"] = env["resume_work"] and (raw_output_folder / "groundtruth_depth" / video_name.namebase).isdir()
     return video_env
 
 
@@ -154,6 +159,7 @@ def main():
     env = vars(args)
     if args.show_steps:
         print_workflow()
+        return
     if args.add_new_videos:
         args.skip_step += [1, 2, 4, 5, 6]
     if args.begin_step is not None:
@@ -164,6 +170,7 @@ def main():
     colmap = Colmap(db=env["thorough_db"],
                     image_path=env["image_path"],
                     mask_path=env["mask_path"],
+                    dense_workspace=env["dense_workspace"],
                     binary=args.colmap,
                     verbose=args.verbose,
                     logfile=args.log)
@@ -172,7 +179,8 @@ def main():
     env["ffmpeg"] = ffmpeg
     pdraw = PDraw(args.nw, verbose=args.verbose, logfile=args.log)
     env["pdraw"] = pdraw
-    eth3d = ETH3D(args.eth3d, args.output_folder / "Images", verbose=args.verbose, logfile=args.log)
+    eth3d = ETH3D(args.eth3d, args.raw_output_folder / "Images", args.max_occlusion_depth,
+                  verbose=args.verbose, logfile=args.log)
     env["eth3d"] = eth3d
     pcl_util = PCLUtil(args.pcl_util, verbose=args.verbose, logfile=args.log)
     env["pcl_util"] = pcl_util
@@ -191,13 +199,6 @@ def main():
         centroid_path = sorted(env["lidar_path"].files("*_centroid.txt"))[0]
         env["centroid"] = np.loadtxt(centroid_path)
 
-    i += 1
-    if i not in args.skip_step:
-        print_step(i, "Occlusion Mesh computing")
-        eth3d.compute_normals(env["with_normals_path"], env["lidar_mlp"], neighbor_radius=args.normal_radius)
-        pcl_util.triangulate_mesh(env["occlusion_ply"], env["with_normals_path"], resolution=args.mesh_resolution)
-        eth3d.create_splats(env["splats_ply"], env["with_normals_path"], env["occlusion_ply"], threshold=args.splat_threshold)
-
     i += 1
     if i not in args.skip_step:
         print_step(i, "Pictures preparation")
@@ -244,8 +245,10 @@ def main():
         gsm.process_folder(folder_to_process=env["video_path"], **env)
         colmap.extract_features(image_list=env["video_frame_list_thorough"], more=args.more_sift_features)
         colmap.index_images(vocab_tree_output=env["indexed_vocab_tree"], vocab_tree_input=args.vocab_tree)
-        colmap.match()
+        colmap.match(method="vocab_tree", vocab_tree=env["indexed_vocab_tree"])
         colmap.map(output=env["thorough_recon"].parent)
+        colmap.adjust_bundle(output=env["thorough_recon"], input=env["thorough_recon"],
+                             num_iter=100, refine_extra_params=True)
 
     i += 1
     if i not in args.skip_step:
@@ -255,16 +258,9 @@ def main():
                            input=env["thorough_recon"],
                            ref_images=env["georef_frames_list"])
         env["georef_recon"].merge_tree(env["georef_full_recon"])
-        if args.dense:
-            print_step("{} (bis)".format(i), "Point cloud densificitation")
-            dense_workspace = env["thorough_recon"].parent/"dense"
-            colmap.undistort(input=env["georef_recon"], output=dense_workspace)
-            colmap.dense_stereo(workspace=dense_workspace)
-            colmap.stereo_fusion(workspace=dense_workspace, output=env["georefrecon_ply"])
-        else:
-            colmap.export_model(output=env["georefrecon_ply"],
-                                input=env["georef_recon"])
     if args.inspect_dataset:
+        colmap.export_model(output=env["georef_recon"] / "georef_sparse.ply",
+                            input=env["georef_recon"])
         georef_mlp = env["georef_recon"]/"georef_recon.mlp"
         mxw.create_project(georef_mlp, [env["georefrecon_ply"]])
         colmap.export_model(output=env["georef_recon"],
@@ -274,6 +270,25 @@ def main():
                               colmap_model=env["georef_recon"],
                               image_path=env["image_path"])
 
+    i += 1
+    if i not in args.skip_step:
+        print_step(i, "Video localization with respect to reconstruction")