Commit cd0bdaa7 authored by Clément Pinard's avatar Clément Pinard
Browse files

add compare 2 cams (WIP)

parent f40ac3e1
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)
......@@ -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)
......
......@@ -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
......
......@@ -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)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment