Add main pipeline

add_video_to_db.py

+from colmap_util import database as db
+from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter
+from path import Path
+import pandas as pd
+import numpy as np
+from sqlite3 import IntegrityError
+from tqdm import tqdm
+parser = ArgumentParser(description='Create vizualisation for specified video',
+                        formatter_class=ArgumentDefaultsHelpFormatter)
+
+parser.add_argument('--frame_list', metavar='PATH',
+                    help='path to list with relative path to images', type=Path, default=None)
+parser.add_argument('--metadata', metavar='PATH',
+                    help='path to metadata csv file', type=Path)
+parser.add_argument('--database', metavar='DB', required=True,
+                    help='path to colmap database file, to get the image ids right')
+
+
+def add_to_db(db_path, metadata_path, frame_list_path, **env):
+    metadata = pd.read_csv(metadata_path)
+    database = db.COLMAPDatabase.connect(db_path)
+
+    frame_list = []
+    if frame_list_path is not None:
+        with open(frame_list_path, "r") as f:
+            frame_list = [line[:1] for line in f.readlines()]
+        metadata = metadata[metadata["image_path"].isin(frame_list)]
+
+    for image_id, row in tqdm(metadata.iterrows(), total=len(metadata)):
+        image_path = row["image_path"]
+        camera_id = row["camera_id"]
+        if row["location_valid"]:
+            frame_gps = row[["location_longitude", "location_latitude", "location_altitude"]]
+        else:
+            frame_gps = np.full(3, np.NaN)
+        try:
+            database.add_image(image_path, int(camera_id), prior_t=frame_gps)
+        except IntegrityError:
+            sql_string = "SELECT camera_id FROM images WHERE name='{}'".format(image_path)
+            existing_camera_id = print(next(database.execute(sql_string))[0])
+            assert(existing_camera_id == camera_id)
+
+
+def main():
+    args = parser.parse_args()
+    add_to_db(args.database, args.metadata, args.frame_list)
+
+    return
+
+
+if __name__ == '__main__':
+    main()

add_video_to_model.py

@@ -71,7 +71,7 @@ def main():
     print_cams(cameras)
     camera_id = int(input("which camera for the video ?\n"))
     images = rm.read_images_binary(args.input_model / "images.bin")
-    images = {}
+    # images = {}
     image_ids = get_id_from_db(db)
     for name in image_list:
         if name not in image_ids.keys():

anafi_metadata.py

+from subprocess import PIPE, call
+import pandas as pd
+import numpy as np
+from scipy import integrate
+import tempfile
+
+
+def extrapolate_position(speeds, timestamps, initial_position, final_position):
+    trapz_x = integrate.cumtrapz(speeds[:, 0], timestamps, initial=0)
+    trapz_y = integrate.cumtrapz(speeds[:, 1], timestamps, initial=0)
+    trapz_z = integrate.cumtrapz(speeds[:, 2], timestamps, initial=0)
+    trapz = np.stack([trapz_x, trapz_y, trapz_z], axis=-1)
+    if initial_position is None and final_position is None:
+        return trapz
+    elif initial_position is not None and final_position is None:
+        return trapz + initial_position
+    elif initial_position is None and final_position is not None:
+        return trapz + final_position - trapz[-1]
+    else:
+        interp = np.linspace(0, final_position - trapz[-1], speeds.shape[0])
+        return trapz + interp
+
+
+def preprocess_metadata(metadata, proj, centroid):
+    def lambda_fun(x):
+        return pd.Series(proj(*x), index=["x", "y"])
+    position_xy = metadata[["location_longitude", "location_latitude"]].apply(lambda_fun, axis=1)
+    metadata = metadata.join(position_xy)
+    # Extrapolate position from speed and previous frames
+
+    speed = metadata[["speed_east", "speed_north", "speed_down"]].values * np.array([1, 1, -1])
+    timestamps = metadata["time"].values * 1e-6
+    positions = metadata[["x", "y", "location_altitude"]].values
+    if metadata["location_valid"].unique().tolist() == [0]:
+        metadata["indoor"] = True
+        positions = extrapolate_position(speed, timestamps, None, None)
+    else:
+        metadata["indoor"] = False
+        if 0 in metadata["location_valid"].unique():
+            location_validity = metadata["location_valid"].diff()
+
+            invalidity_start = location_validity.index[location_validity == -1].tolist()
+            validity_start = location_validity.index[location_validity == 1].tolist()
+
+            if metadata["location_valid"].iloc[0] == 0:
+                end = validity_start.pop(0)
+                positions[:end] = extrapolate_position(speed[:end], timestamps[:end], None, positions[end-1])
+            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")
+
+            for start, end in zip(invalidity_start, validity_start):
+                positions[start:end] = extrapolate_position(speed[start:end], timestamps[start:end], positions[start], positions[end-1])
+        positions -= centroid
+        print(positions)
+
+    metadata["x"], metadata["y"], metadata["location_altitude"] = positions.transpose()
+
+    return metadata
+
+
+def extract_metadata(folder_path, file_path, native_wrapper, proj, w, h, f, centroid, save_path=None):
+    metadata = native_wrapper.vmeta_extract(file_path)
+    metadata = preprocess_metadata(metadata, proj, centroid)
+    video_quality = h * w / f
+    metadata["video_quality"] = video_quality
+    metadata["height"] = h
+    metadata["width"] = w
+    metadata["framerate"] = f
+    metadata["video"] = file_path
+    metadata['frame'] = metadata.index
+    if save_path is not None:
+        metadata.to_csv(save_path)
+    return metadata

colmap/database.py

-# Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-#     * Redistributions of source code must retain the above copyright
-#       notice, this list of conditions and the following disclaimer.
-#
-#     * Redistributions in binary form must reproduce the above copyright
-#       notice, this list of conditions and the following disclaimer in the
-#       documentation and/or other materials provided with the distribution.
-#
-#     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
-#       its contributors may be used to endorse or promote products derived
-#       from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
-
-# This script is based on an original implementation by True Price.
-
-import sys
-import sqlite3
-import numpy as np
-
-
-IS_PYTHON3 = sys.version_info[0] >= 3
-
-MAX_IMAGE_ID = 2**31 - 1
-
-CREATE_CAMERAS_TABLE = """CREATE TABLE IF NOT EXISTS cameras (
-    camera_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL,
-    model INTEGER NOT NULL,
-    width INTEGER NOT NULL,
-    height INTEGER NOT NULL,
-    params BLOB,
-    prior_focal_length INTEGER NOT NULL)"""
-
-CREATE_DESCRIPTORS_TABLE = """CREATE TABLE IF NOT EXISTS descriptors (
-    image_id INTEGER PRIMARY KEY NOT NULL,
-    rows INTEGER NOT NULL,
-    cols INTEGER NOT NULL,
-    data BLOB,
-    FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE)"""
-
-CREATE_IMAGES_TABLE = """CREATE TABLE IF NOT EXISTS images (
-    image_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL,
-    name TEXT NOT NULL UNIQUE,
-    camera_id INTEGER NOT NULL,
-    prior_qw REAL,
-    prior_qx REAL,
-    prior_qy REAL,
-    prior_qz REAL,
-    prior_tx REAL,
-    prior_ty REAL,
-    prior_tz REAL,
-    CONSTRAINT image_id_check CHECK(image_id >= 0 and image_id < {}),
-    FOREIGN KEY(camera_id) REFERENCES cameras(camera_id))
-""".format(MAX_IMAGE_ID)
-
-CREATE_TWO_VIEW_GEOMETRIES_TABLE = """
-CREATE TABLE IF NOT EXISTS two_view_geometries (
-    pair_id INTEGER PRIMARY KEY NOT NULL,
-    rows INTEGER NOT NULL,
-    cols INTEGER NOT NULL,
-    data BLOB,
-    config INTEGER NOT NULL,
-    F BLOB,
-    E BLOB,
-    H BLOB)
-"""
-
-CREATE_KEYPOINTS_TABLE = """CREATE TABLE IF NOT EXISTS keypoints (
-    image_id INTEGER PRIMARY KEY NOT NULL,
-    rows INTEGER NOT NULL,
-    cols INTEGER NOT NULL,
-    data BLOB,
-    FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE)
-"""
-
-CREATE_MATCHES_TABLE = """CREATE TABLE IF NOT EXISTS matches (
-    pair_id INTEGER PRIMARY KEY NOT NULL,
-    rows INTEGER NOT NULL,
-    cols INTEGER NOT NULL,
-    data BLOB)"""
-
-CREATE_NAME_INDEX = \
-    "CREATE UNIQUE INDEX IF NOT EXISTS index_name ON images(name)"
-
-CREATE_ALL = "; ".join([
-    CREATE_CAMERAS_TABLE,
-    CREATE_IMAGES_TABLE,
-    CREATE_KEYPOINTS_TABLE,
-    CREATE_DESCRIPTORS_TABLE,
-    CREATE_MATCHES_TABLE,
-    CREATE_TWO_VIEW_GEOMETRIES_TABLE,
-    CREATE_NAME_INDEX
-])
-
-
-def image_ids_to_pair_id(image_id1, image_id2):
-    if image_id1 > image_id2:
-        image_id1, image_id2 = image_id2, image_id1
-    return image_id1 * MAX_IMAGE_ID + image_id2
-
-
-def pair_id_to_image_ids(pair_id):
-    image_id2 = pair_id % MAX_IMAGE_ID
-    image_id1 = (pair_id - image_id2) / MAX_IMAGE_ID
-    return image_id1, image_id2
-
-
-def array_to_blob(array):
-    if IS_PYTHON3:
-        return array.tostring()
-    else:
-        return np.getbuffer(array)
-
-
-def blob_to_array(blob, dtype, shape=(-1,)):
-    if IS_PYTHON3:
-        return np.fromstring(blob, dtype=dtype).reshape(*shape)
-    else:
-        return np.frombuffer(blob, dtype=dtype).reshape(*shape)
-
-
-class COLMAPDatabase(sqlite3.Connection):
-
-    @staticmethod
-    def connect(database_path):
-        return sqlite3.connect(database_path, factory=COLMAPDatabase)
-
-
-    def __init__(self, *args, **kwargs):
-        super(COLMAPDatabase, self).__init__(*args, **kwargs)
-
-        self.create_tables = lambda: self.executescript(CREATE_ALL)
-        self.create_cameras_table = \
-            lambda: self.executescript(CREATE_CAMERAS_TABLE)
-        self.create_descriptors_table = \
-            lambda: self.executescript(CREATE_DESCRIPTORS_TABLE)
-        self.create_images_table = \
-            lambda: self.executescript(CREATE_IMAGES_TABLE)
-        self.create_two_view_geometries_table = \
-            lambda: self.executescript(CREATE_TWO_VIEW_GEOMETRIES_TABLE)
-        self.create_keypoints_table = \
-            lambda: self.executescript(CREATE_KEYPOINTS_TABLE)
-        self.create_matches_table = \
-            lambda: self.executescript(CREATE_MATCHES_TABLE)
-        self.create_name_index = lambda: self.executescript(CREATE_NAME_INDEX)
-
-    def add_camera(self, model, width, height, params,
-                   prior_focal_length=False, camera_id=None):
-        params = np.asarray(params, np.float64)
-        cursor = self.execute(
-            "INSERT INTO cameras VALUES (?, ?, ?, ?, ?, ?)",
-            (camera_id, model, width, height, array_to_blob(params),
-             prior_focal_length))
-        return cursor.lastrowid
-
-    def add_image(self, name, camera_id,
-                  prior_q=np.zeros(4), prior_t=np.zeros(3), image_id=None):
-        cursor = self.execute(
-            "INSERT INTO images VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?)",
-            (image_id, name, camera_id, prior_q[0], prior_q[1], prior_q[2],
-             prior_q[3], prior_t[0], prior_t[1], prior_t[2]))
-        return cursor.lastrowid
-
-    def add_keypoints(self, image_id, keypoints):
-        assert(len(keypoints.shape) == 2)
-        assert(keypoints.shape[1] in [2, 4, 6])
-
-        keypoints = np.asarray(keypoints, np.float32)
-        self.execute(
-            "INSERT INTO keypoints VALUES (?, ?, ?, ?)",
-            (image_id,) + keypoints.shape + (array_to_blob(keypoints),))
-
-    def add_descriptors(self, image_id, descriptors):
-        descriptors = np.ascontiguousarray(descriptors, np.uint8)
-        self.execute(
-            "INSERT INTO descriptors VALUES (?, ?, ?, ?)",
-            (image_id,) + descriptors.shape + (array_to_blob(descriptors),))
-
-    def add_matches(self, image_id1, image_id2, matches):
-        assert(len(matches.shape) == 2)
-        assert(matches.shape[1] == 2)
-
-        if image_id1 > image_id2:
-            matches = matches[:,::-1]
-
-        pair_id = image_ids_to_pair_id(image_id1, image_id2)
-        matches = np.asarray(matches, np.uint32)
-        self.execute(
-            "INSERT INTO matches VALUES (?, ?, ?, ?)",
-            (pair_id,) + matches.shape + (array_to_blob(matches),))
-
-    def add_two_view_geometry(self, image_id1, image_id2, matches,
-                              F=np.eye(3), E=np.eye(3), H=np.eye(3), config=2):
-        assert(len(matches.shape) == 2)
-        assert(matches.shape[1] == 2)
-
-        if image_id1 > image_id2:
-            matches = matches[:,::-1]
-
-        pair_id = image_ids_to_pair_id(image_id1, image_id2)
-        matches = np.asarray(matches, np.uint32)
-        F = np.asarray(F, dtype=np.float64)
-        E = np.asarray(E, dtype=np.float64)
-        H = np.asarray(H, dtype=np.float64)
-        self.execute(
-            "INSERT INTO two_view_geometries VALUES (?, ?, ?, ?, ?, ?, ?, ?)",
-            (pair_id,) + matches.shape + (array_to_blob(matches), config,
-             array_to_blob(F), array_to_blob(E), array_to_blob(H)))
-
-
-def example_usage():
-    import os
-    import argparse
-
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--database_path", default="database.db")
-    args = parser.parse_args()
-
-    if os.path.exists(args.database_path):
-        print("ERROR: database path already exists -- will not modify it.")
-        return
-
-    # Open the database.
-
-    db = COLMAPDatabase.connect(args.database_path)
-
-    # For convenience, try creating all the tables upfront.
-
-    db.create_tables()
-
-    # Create dummy cameras.
-
-    model1, width1, height1, params1 = \
-        0, 1024, 768, np.array((1024., 512., 384.))
-    model2, width2, height2, params2 = \
-        2, 1024, 768, np.array((1024., 512., 384., 0.1))
-
-    camera_id1 = db.add_camera(model1, width1, height1, params1)
-    camera_id2 = db.add_camera(model2, width2, height2, params2)
-
-    # Create dummy images.
-
-    image_id1 = db.add_image("image1.png", camera_id1)
-    image_id2 = db.add_image("image2.png", camera_id1)
-    image_id3 = db.add_image("image3.png", camera_id2)
-    image_id4 = db.add_image("image4.png", camera_id2)
-
-    # Create dummy keypoints.
-    #
-    # Note that COLMAP supports:
-    #      - 2D keypoints: (x, y)
-    #      - 4D keypoints: (x, y, theta, scale)
-    #      - 6D affine keypoints: (x, y, a_11, a_12, a_21, a_22)
-
-    num_keypoints = 1000
-    keypoints1 = np.random.rand(num_keypoints, 2) * (width1, height1)
-    keypoints2 = np.random.rand(num_keypoints, 2) * (width1, height1)
-    keypoints3 = np.random.rand(num_keypoints, 2) * (width2, height2)
-    keypoints4 = np.random.rand(num_keypoints, 2) * (width2, height2)
-
-    db.add_keypoints(image_id1, keypoints1)
-    db.add_keypoints(image_id2, keypoints2)
-    db.add_keypoints(image_id3, keypoints3)
-    db.add_keypoints(image_id4, keypoints4)
-
-    # Create dummy matches.
-
-    M = 50
-    matches12 = np.random.randint(num_keypoints, size=(M, 2))
-    matches23 = np.random.randint(num_keypoints, size=(M, 2))
-    matches34 = np.random.randint(num_keypoints, size=(M, 2))
-
-    db.add_matches(image_id1, image_id2, matches12)
-    db.add_matches(image_id2, image_id3, matches23)
-    db.add_matches(image_id3, image_id4, matches34)
-
-    # Commit the data to the file.
-
-    db.commit()
-
-    # Read and check cameras.
-
-    rows = db.execute("SELECT * FROM cameras")
-
-    camera_id, model, width, height, params, prior = next(rows)
-    params = blob_to_array(params, np.float64)
-    assert camera_id == camera_id1
-    assert model == model1 and width == width1 and height == height1
-    assert np.allclose(params, params1)
-
-    camera_id, model, width, height, params, prior = next(rows)
-    params = blob_to_array(params, np.float64)
-    assert camera_id == camera_id2
-    assert model == model2 and width == width2 and height == height2
-    assert np.allclose(params, params2)
-
-    # Read and check keypoints.
-
-    keypoints = dict(
-        (image_id, blob_to_array(data, np.float32, (-1, 2)))
-        for image_id, data in db.execute(
-            "SELECT image_id, data FROM keypoints"))
-
-    assert np.allclose(keypoints[image_id1], keypoints1)
-    assert np.allclose(keypoints[image_id2], keypoints2)
-    assert np.allclose(keypoints[image_id3], keypoints3)
-    assert np.allclose(keypoints[image_id4], keypoints4)
-
-    # Read and check matches.
-
-    pair_ids = [image_ids_to_pair_id(*pair) for pair in
-                ((image_id1, image_id2),
-                 (image_id2, image_id3),
-                 (image_id3, image_id4))]
-
-    matches = dict(
-        (pair_id_to_image_ids(pair_id),
-         blob_to_array(data, np.uint32, (-1, 2)))
-        for pair_id, data in db.execute("SELECT pair_id, data FROM matches")
-    )
-
-    assert np.all(matches[(image_id1, image_id2)] == matches12)
-    assert np.all(matches[(image_id2, image_id3)] == matches23)
-    assert np.all(matches[(image_id3, image_id4)] == matches34)
-
-    # Clean up.
-
-    db.close()
-
-    if os.path.exists(args.database_path):
-        os.remove(args.database_path)
-
-
-if __name__ == "__main__":
-    example_usage()

euroc2colmap.py

+import pandas as pd
+import numpy as np
+from path import Path
+import yaml
+from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter
+from colmap.read_model import Image, Camera, Point3D, write_model, qvec2rotmat, rotmat2qvec
+from tqdm import tqdm
+from pyntcloud import PyntCloud
+from itertools import islice
+
+parser = ArgumentParser(description='create a vizualisation from ground truth created',
+                        formatter_class=ArgumentDefaultsHelpFormatter)
+
+parser.add_argument('--root', metavar='DIR', type=Path)
+parser.add_argument('--output_dir', metavar='DIR', default=None, type=Path)
+parser.add_argument('--img_root', metavar='DIR', default=None, type=Path)
+parser.add_argument('--load_pointcloud', action='store_true')
+parser.add_argument('--format', choices=['.txt', '.bin'], default='.txt')
+
+
+def get_cam(yaml_path, cam_id):
+    with open(yaml_path) as f:
+        cam_dict = yaml.load(f, Loader=yaml.SafeLoader)
+
+    calib = cam_dict["T_BS"]
+    calib_matrix = np.array(calib["data"]).reshape((calib["rows"], calib["cols"]))
+    assert cam_dict["distortion_model"] == "radial-tangential"
+    w, h = cam_dict["resolution"]
+    cam = Camera(id=cam_id,
+                 model="OPENCV",
+                 width=w,
+                 height=h,
+                 params=np.array(cam_dict["intrinsics"] + cam_dict["distortion_coefficients"]))
+
+    return cam, calib_matrix
+
+
+def get_vicon_calib(yaml_path):
+    with open(yaml_path) as f:
+        vicon_dict = yaml.load(f, Loader=yaml.SafeLoader)
+
+    calib = vicon_dict["T_BS"]
+    return np.array(calib["data"]).reshape((calib["rows"], calib["cols"]))
+
+
+def create_image(img_id, cam_id, file_path, drone_pose, image_calib, vicon_calib):
+    t_prefix = " p_RS_R_{} [m]"
+    q_prefix = " q_RS_{} []"
+    drone_tvec = drone_pose[[t_prefix.format(dim) for dim in 'xyz']].to_numpy().reshape(3, 1)
+    drone_qvec = drone_pose[[q_prefix.format(dim) for dim in 'wxyz']].to_numpy()
+    drone_R = qvec2rotmat(drone_qvec)
+    drone_matrix = np.concatenate((np.hstack((drone_R, drone_tvec)), np.array([0, 0, 0, 1]).reshape(1, 4)))
+    image_matrix = drone_matrix @ np.linalg.inv(vicon_calib) @ image_calib
+    colmap_matrix = np.linalg.inv(image_matrix)
+    colmap_qvec = rotmat2qvec(colmap_matrix[:3, :3])
+    colmap_tvec = colmap_matrix[:3, -1]
+
+    return Image(id=img_id, qvec=colmap_qvec, tvec=colmap_tvec,
+                 camera_id=cam_id, name=file_path,
+                 xys=[], point3D_ids=[]), image_matrix[:3, -1]
+
+
+def main():
+    args = parser.parse_args()
+    cam_dirs = [args.root/"cam0", args.root/"cam1"]
+    vicon_dir = args.root/"state_groundtruth_estimate0"
+    cloud_file = args.root/"pointcloud0"/"data.ply"
+    if args.img_root is None:
+        args.img_root = args.root
+
+    vicon_poses = pd.read_csv(vicon_dir/"data.csv")
+    vicon_poses = vicon_poses.set_index("#timestamp")
+    vicon_calib = get_vicon_calib(vicon_dir/"sensor.yaml")
+    cameras = {}
+    images = {}
+    image_list = []
+    image_georef = []
+    for cam_id, cam in enumerate(cam_dirs):
+        print("Converting camera {} ...".format(cam))
+        if len(images.keys()) == 0:
+            last_image_id = 0
+        else:
+            last_image_id = max(images.keys())
+        cameras[cam_id], cam_calib = get_cam(cam/"sensor.yaml", cam_id)
+
+        image_names = pd.read_csv(cam/"data.csv")
+        image_root = cam/"data"
+        step = 1
+        for img_id, (_, (ts, filename)) in tqdm(enumerate(islice(image_names.iterrows(), 0, None, step)), total=len(image_names.index)//step):
+            final_path = (image_root/filename).relpath(args.img_root)
+            image_list.append(final_path)
+            row_index = vicon_poses.index.get_loc(ts, method='nearest')
+            current_drone_pose = vicon_poses.iloc[row_index]
+            images[1 + img_id + last_image_id], georef = create_image(1 + img_id + last_image_id, cam_id,
+                                                                      final_path, current_drone_pose,
+                                                                      cam_calib, vicon_calib)
+            image_georef.append(georef)
+
+    points = {}
+    if args.load_pointcloud:
+        subsample = 1
+        print("Loading point cloud {}...".format(cloud_file))
+        cloud = PyntCloud.from_file(cloud_file)
+        print("Converting ...")
+        npy_points = cloud.points[['x', 'y', 'z', 'intensity']].values[::subsample]
+        for id_point, row in tqdm(enumerate(npy_points), total=len(npy_points)):
+            xyz = row[:3]
+            gray_level