add compare 2 cams (WIP)

compare_2_cams.py

+from colmap_util import read_model as rm
+import pandas as pd
+import numpy as np
+from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter
+from path import Path
+import matplotlib.pyplot as plt
+from scipy.spatial.transform import Rotation
+
+parser = ArgumentParser(description='Take all the drone videos of a folder and put the frame '
+                                    'location in a COLMAP file for vizualisation',
+                        formatter_class=ArgumentDefaultsHelpFormatter)
+
+parser.add_argument('--input_images_1', metavar='FILE', type=Path)
+parser.add_argument('--input_images_2', metavar='FILE', type=Path)
+parser.add_argument('--metadata_path_1', metavar='FILE', type=Path)
+parser.add_argument('--metadata_path_2', metavar='FILE', type=Path)
+parser.add_argument('--visualize', action="store_true")
+parser.add_argument('--interpolated_frames_list', type=Path)
+
+
+def colmap_to_world(tvec, qvec):
+    quats = Rotation.from_quat(qvec)
+    return -quats.apply(tvec, inverse=True)
+
+
+def qvec_diff(qvec1, qvec2):
+    rot1 = Rotation.from_quat(qvec1)
+    rot2 = Rotation.from_quat(qvec2)
+
+    diff = rot1 * rot2.inv()
+
+    return diff.as_quat()
+
+
+def open_video(images_colmap, metadata_path):
+    if images_colmap.ext == ".txt":
+        images_dict = rm.read_images_text(images_colmap)
+    elif images_colmap.ext == ".bin":
+        images_dict = rm.read_images_binary(images_colmap)
+    else:
+        print(images_colmap.ext)
+    image_df = pd.DataFrame.from_dict(images_dict, orient="index").set_index("id")
+    metadata = pd.read_csv(metadata_path).set_index("db_id", drop=False).sort_values("time")
+
+    tvec = np.stack(image_df["tvec"].values)
+    qvec = np.stack(image_df["qvec"].values)
+
+    # Check if quats are flipped by computing shifted dot product.
+    # If no flip (and continuous orientation), the dot product is around 1. Otherwise, it's around -1
+    # A quaternion is flipped if the dot product is negative
+
+    flips = list((np.sum(qvec[1:] * qvec[:-1], axis=1) < 0).nonzero()[0] + 1)
+    flips.append(qvec.shape[0])
+    for j, k in zip(flips[::2], flips[1::2]):
+        qvec[j:k] *= -1
+
+    tvec_columns = ["colmap_tx", "colmap_ty", "colmap_tz"]
+    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)")
+
+    qvec = metadata[["colmap_qx", "colmap_qy", "colmap_qz", "colmap_qw"]].values
+    tvec = metadata[["colmap_tx", "colmap_ty", "colmap_tz"]].values
+
+    world_tvec = colmap_to_world(tvec, qvec)
+
+    metadata["tx"] = world_tvec[:, 0]
+    metadata["ty"] = world_tvec[:, 1]
+    metadata["tz"] = world_tvec[:, 2]
+    metadata["qx"] = qvec[:, 0]
+    metadata["qy"] = qvec[:, 1]
+    metadata["qz"] = qvec[:, 2]
+    metadata["qw"] = qvec[:, 3]
+    return metadata
+
+
+def compare_metadata(metadata_1, metadata_2):
+    qvec_1 = metadata_1[["qx", "qy", "qz", "qw"]].values
+    qvec_2 = metadata_2[["qx", "qy", "qz", "qw"]].values
+
+    qvec = pd.DataFrame(qvec_diff(qvec_1, qvec_2), index=metadata_1.index)
+    tvec = metadata_1[["tx", "ty", "tz"]] - metadata_2[["tx", "ty", "tz"]]
+
+    return qvec, tvec
+
+
+def compare_cams(input_images_1, input_images_2, metadata_path_1, metadata_path_2,
+                 **env):
+
+    metadata_1 = open_video(input_images_1, metadata_path_1)
+    metadata_2 = open_video(input_images_2, metadata_path_2)
+    assert(len(metadata_1) == len(metadata_2))
+    metadata_1[["qw", "qx", "qy", "qz"]].plot()
+    metadata_2[["qw", "qx", "qy", "qz"]].plot()
+    metadata_1[["tx", "ty", "tz"]].plot()
+    metadata_2[["tx", "ty", "tz"]].plot()
+
+    qvec, tvec = compare_metadata(metadata_1, metadata_2)
+    qvec.plot()
+    tvec.plot()
+    plt.show()
+
+
+if __name__ == '__main__':
+    args = parser.parse_args()
+    env = vars(args)
+    compare_cams(**env)

convert_dataset.py

@@ -106,7 +106,11 @@ def apply_cmap_and_resize(depth, colormap, 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
+    if len(img.shape) == 3:
+        h, w, _ = img.shape
+    elif len(img.shape) == 2:
+        h, w = img.shape
+        img = img.reshape(h, w, 1)
     assert((h/downscale).is_integer() and (w/downscale).is_integer())
     output_img = np.zeros((2*(h//downscale), 2*(w//downscale), 3), dtype=np.uint8)
     rescaled_img = rescale(img, 1/downscale, multichannel=True)*255
@@ -148,7 +152,7 @@ parser.add_argument('--occ_dir', metavar='DIR', type=Path)
 parser.add_argument('--metadata_path', type=Path)
 parser.add_argument('--dataset_output_dir', metavar='DIR', default=None, type=Path)
 parser.add_argument('--video_output_dir', metavar='DIR', default=None, type=Path)
-parser.add_argument('--interpolated_frames_list', metavar='TXT', type=Path)
+parser.add_argument('--interpolated_frames_path', metavar='TXT', type=Path)
 parser.add_argument('--final_model', metavar='DIR', type=Path)
 parser.add_argument('--video', action='store_true')
 parser.add_argument('--downscale', type=int, default=1)

video_localization.py

@@ -20,13 +20,15 @@ def is_video_in_model(video_name, colmap_model, metadata):
 
 
 def sift_and_match(colmap, more_sift_features, indexed_vocab_tree, image_list, **env):
-    while True:
+    tries = 0
+    while tries < 10:
         try:
             colmap.extract_features(image_list=image_list, more=more_sift_features)
             colmap.match(method="sequential", vocab_tree=indexed_vocab_tree)
         except Exception:
             # If it failed, that's because sift gpu has failed.
             print("Error With colmap, will retry")
+            tries += 1
             pass
         else:
             return

videos_to_colmap.py

@@ -107,7 +107,7 @@ def optimal_sample(metadata, num_frames, orientation_weight, resolution_weight):
 
 def register_new_cameras(cameras_dataframe, database, camera_dict):
     camera_ids = []
-    for _, (w, h, f, hfov, vfov, camera_model) in cameras_dataframe.iterrows():
+    for _, (w, h, f, hfov, vfov, camera_model, *_) in cameras_dataframe.iterrows():
         if not (np.isnan(hfov) or np.isnan(vfov)):
             fx = w / (2 * np.tan(hfov * np.pi/360))
             fy = h / (2 * np.tan(vfov * np.pi/360))
@@ -209,16 +209,15 @@ def process_video_folder(videos_list, existing_pictures, output_video_folder, im
     if any(final_metadata["model"] == "generic"):
         print("Undefined remaining cameras, assigning generic models to them")
         generic_frames = final_metadata[final_metadata["model"] == "generic"]
-        generic_cameras_dataframe = generic_frames[cam_fields]
+        generic_cam_fields = cam_fields + ["video"]
+        generic_cameras_dataframe = generic_frames[generic_cam_fields]
         fixed_camera = True
         if fixed_camera:
             generic_cameras_dataframe = generic_cameras_dataframe.drop_duplicates()
         generic_cameras_dataframe = register_new_cameras(generic_cameras_dataframe, database, colmap_cameras)
         if fixed_camera:
             for cam_id, row in generic_cameras_dataframe.iterrows():
-                final_metadata.loc[(final_metadata[cam_fields] == row).all(axis=1), "camera_id"] = cam_id
-                print(cam_fields)
-                print(final_metadata.loc[(final_metadata[cam_fields] == row).all(axis=1)])
+                final_metadata.loc[(final_metadata[generic_cam_fields] == row).all(axis=1), "camera_id"] = cam_id
         else:
             final_metadata.loc[generic_frames.index, "camera_id"] = generic_cameras_dataframe.index
         cameras_dataframe = cameras_dataframe.append(generic_cameras_dataframe)