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

merge vizualisation and KITTI conversion in one step

parent 8c2c4881
...@@ -44,17 +44,19 @@ def preprocess_metadata(metadata, proj, centroid): ...@@ -44,17 +44,19 @@ def preprocess_metadata(metadata, proj, centroid):
if metadata["location_valid"].iloc[0] == 0: if metadata["location_valid"].iloc[0] == 0:
end = validity_start.pop(0) end = validity_start.pop(0)
positions[:end] = extrapolate_position(speed[:end], timestamps[:end], None, positions[end]) positions[:end] = extrapolate_position(speed[:end], timestamps[:end], None, positions[end])
print(positions[:end], end)
if metadata["location_valid"].iloc[-1] == 0: if metadata["location_valid"].iloc[-1] == 0:
start = invalidity_start.pop(-1) - 1 start = invalidity_start.pop(-1) - 1
positions[start:] = extrapolate_position(speed[start:], timestamps[start:], positions[start], None) positions[start:] = extrapolate_position(speed[start:], timestamps[start:], positions[start], None)
if(len(invalidity_start) != len(validity_start)): 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): for start, end in zip(invalidity_start, validity_start):
positions[start:end] = extrapolate_position(speed[start:end], timestamps[start:end], positions[start], positions[end]) positions[start:end] = extrapolate_position(speed[start:end], timestamps[start:end], positions[start], positions[end])
print(positions)
positions -= centroid positions -= centroid
metadata["x"], metadata["y"], metadata["z"] = positions.transpose() metadata["x"], metadata["y"], metadata["z"] = positions.transpose()
......
...@@ -3,6 +3,7 @@ from path import Path ...@@ -3,6 +3,7 @@ from path import Path
from imageio import imread, imwrite from imageio import imread, imwrite
from skimage.transform import rescale from skimage.transform import rescale
from skimage.measure import block_reduce from skimage.measure import block_reduce
from colmap_util import read_model as rm
import numpy as np import numpy as np
from matplotlib import cm from matplotlib import cm
from matplotlib.colors import ListedColormap, LinearSegmentedColormap from matplotlib.colors import ListedColormap, LinearSegmentedColormap
...@@ -10,6 +11,52 @@ from tqdm import tqdm ...@@ -10,6 +11,52 @@ from tqdm import tqdm
from wrappers import FFMpeg from wrappers import FFMpeg
import gzip import gzip
from pebble import ProcessPool 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): def high_res_colormap(low_res_cmap, resolution=1000, max_value=1):
...@@ -53,19 +100,29 @@ def apply_cmap_and_resize(depth, colormap, downscale): ...@@ -53,19 +100,29 @@ def apply_cmap_and_resize(depth, colormap, downscale):
depth_viz = COLORMAPS[colormap](depth_norm)[:, :, :3] depth_viz = COLORMAPS[colormap](depth_norm)[:, :, :3]
depth_viz[downscale_depth == np.inf] = 0 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) img = imread(img_path)
h, w, _ = img.shape h, w, _ = img.shape
assert((h/downscale).is_integer() and (w/downscale).is_integer()) 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 = 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: if depth_path is not None:
with gzip.open(depth_path, "rb") as f: with gzip.open(depth_path, "rb") as f:
depth = np.frombuffer(f.read(), np.float32).reshape(h, w) 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] = \
output_img[:h//downscale, :w//downscale]//2 + \ output_img[:h//downscale, :w//downscale]//2 + \
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): ...@@ -73,9 +130,13 @@ def process_one_frame(img_path, depth_path, occ_path, output_dir, downscale):
if occ_path is not None: if occ_path is not None:
with gzip.open(occ_path, "rb") as f: with gzip.open(occ_path, "rb") as f:
occ = np.frombuffer(f.read(), np.float32).reshape(h, w) 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', parser = ArgumentParser(description='create a vizualisation from ground truth created',
...@@ -84,36 +145,65 @@ parser = ArgumentParser(description='create a vizualisation from ground truth cr ...@@ -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('--depth_dir', metavar='DIR', type=Path)
parser.add_argument('--img_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('--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('--output_dir', metavar='DIR', default=None, type=Path)
parser.add_argument('--video', action='store_true') parser.add_argument('--video', action='store_true')
parser.add_argument('--fps', default='1') parser.add_argument('--fps', default='1')
parser.add_argument('--downscale', type=int, 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): def convert_dataset(final_model, depth_dir, images_root_folder, occ_dir, dataset_output_dir, video_output_dir, metadata_path, interpolated_frames_path,
imgs = sorted(img_dir.files('*.jpg')) + sorted(img_dir.files('*.JPG')) 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 = [] depth_maps = []
occ_maps = [] occ_maps = []
for i in imgs: interpolated = []
fname = i.basename() 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" depth_path = depth_dir / fname + ".gz"
if depth_path.isfile(): if depth_path.isfile():
depth_maps.append(depth_path) depth_maps.append(depth_path)
else: else:
print("Image {} was not registered".format(fname)) print("Image {} was not registered".format(fname))
depth_maps.append(None) 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" occ_path = occ_dir / fname + ".gz"
if occ_path.isfile(): if occ_path.isfile():
occ_maps.append(occ_path) occ_maps.append(occ_path)
else: else:
occ_maps.append(None) occ_maps.append(None)
output_dir.makedirs_p()
if threads == 1: if threads == 1:
for i, d, o in tqdm(zip(imgs, depth_maps, occ_maps), total=len(imgs)): for i, d, o, n in tqdm(zip(imgs, depth_maps, occ_maps, interpolated), total=len(imgs)):
process_one_frame(i, d, o, output_dir, downscale) process_one_frame(i, d, o, dataset_output_dir, video_output_dir, downscale, n)
else: else:
with ProcessPool(max_workers=threads) as pool: 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: try:
for _ in tqdm(tasks.result(), total=len(imgs)): for _ in tqdm(tasks.result(), total=len(imgs)):
pass pass
...@@ -122,8 +212,8 @@ def process_viz(depth_dir, img_dir, occ_dir, output_dir, video, fps, downscale, ...@@ -122,8 +212,8 @@ def process_viz(depth_dir, img_dir, occ_dir, output_dir, video, fps, downscale,
raise e raise e
if video: if video:
video_path = str(output_dir/'{}_groundtruth_viz.mp4'.format(output_dir.namebase)) video_path = str(video_output_dir/'{}_groundtruth_viz.mp4'.format(video_output_dir.namebase))
glob_pattern = str(output_dir/'*.png') glob_pattern = str(video_output_dir/'*.png')
ffmpeg.create_video(video_path, glob_pattern, fps) ffmpeg.create_video(video_path, glob_pattern, fps)
...@@ -131,4 +221,4 @@ if __name__ == '__main__': ...@@ -131,4 +221,4 @@ if __name__ == '__main__':
args = parser.parse_args() args = parser.parse_args()
env = vars(args) env = vars(args)
env["ffmpeg"] = FFMpeg() env["ffmpeg"] = FFMpeg()
process_viz(**env) convert_dataset(**env)
...@@ -11,30 +11,13 @@ parser = ArgumentParser(description='Take all the drone videos of a folder and p ...@@ -11,30 +11,13 @@ parser = ArgumentParser(description='Take all the drone videos of a folder and p
'location in a COLMAP file for vizualisation', 'location in a COLMAP file for vizualisation',
formatter_class=ArgumentDefaultsHelpFormatter) formatter_class=ArgumentDefaultsHelpFormatter)
parser.add_argument('--colmap_model_folder', metavar='DIR', type=Path) parser.add_argument('--input_images_colmap', metavar='FILE', type=Path)
parser.add_argument('--output_folder_model', metavar='DIR', type=Path) parser.add_argument('--output_images_colmap', metavar='FILE', type=Path)
parser.add_argument('--interpolate', action="store_true") 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") 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): def colmap_to_world(tvec, qvec):
if any(np.isnan(qvec)): if any(np.isnan(qvec)):
...@@ -51,6 +34,11 @@ def colmap_to_world(tvec, qvec): ...@@ -51,6 +34,11 @@ def colmap_to_world(tvec, qvec):
return -quats.apply(tvec, inverse=True) 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): def NEDtoworld(qvec):
world2NED = np.float32([[0, 1, 0], world2NED = np.float32([[0, 1, 0],
[1, 0, 0], [1, 0, 0],
...@@ -77,8 +65,8 @@ def get_outliers(series, threshold): ...@@ -77,8 +65,8 @@ def get_outliers(series, threshold):
def slerp_quats(quat_df, prefix=""): def slerp_quats(quat_df, prefix=""):
valid_df = quat_df[~quat_df["outlier"]] valid_df = quat_df[~quat_df["outlier"]]
valid_index = valid_df["time"] valid_index = valid_df.index
total_index = quat_df["time"] total_index = quat_df.index
# Note that scipy uses a different order convention than colmap # 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) 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=""): ...@@ -88,12 +76,17 @@ def slerp_quats(quat_df, prefix=""):
return pd.DataFrame(slerp(total_index).as_quat(), index=quat_df.index) 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, filter_degree=3, filter_time=0.1,
threshold_t=0.01, threshold_q=5e-3, threshold_t=0.01, threshold_q=5e-3,
visualize=False, **env): visualize=False, max_interpolate=2, **env):
images_dict = rm.read_images_text(input / "images.txt") if input_images_colmap.ext == ".txt":
metadata = pd.read_csv(metadata_path).set_index("db_id").sort_values("time") 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] framerate = metadata["framerate"].iloc[0]
filter_length = 2*int(filter_time * framerate) + 1 filter_length = 2*int(filter_time * framerate) + 1
...@@ -101,8 +94,8 @@ def filter_colmap_model(input, output, metadata_path, ...@@ -101,8 +94,8 @@ def filter_colmap_model(input, output, metadata_path,
image_df = image_df.reindex(metadata.index) image_df = image_df.reindex(metadata.index)
metadata["outlier"] = image_df.isna().any(axis="columns") metadata["outlier"] = image_df.isna().any(axis="columns")
colmap_outliers = sum(metadata["outlier"]) colmap_outliers = sum(metadata["outlier"])
total_frames = len(metadata)
image_df = image_df.dropna() image_df = image_df.dropna()
tvec = np.stack(image_df["tvec"].values) tvec = np.stack(image_df["tvec"].values)
qvec = np.stack(image_df["qvec"].values) qvec = np.stack(image_df["qvec"].values)
...@@ -111,7 +104,6 @@ def filter_colmap_model(input, output, metadata_path, ...@@ -111,7 +104,6 @@ def filter_colmap_model(input, output, metadata_path,
# A quaternion is flipped if the dot product is negative # 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 = list((np.sum(qvec[1:] * qvec[:-1], axis=1) < 0).nonzero()[0] + 1)
print(flips)
flips.append(qvec.shape[0]) flips.append(qvec.shape[0])
for j, k in zip(flips[::2], flips[1::2]): for j, k in zip(flips[::2], flips[1::2]):
qvec[j:k] *= -1 qvec[j:k] *= -1
...@@ -120,8 +112,15 @@ def filter_colmap_model(input, output, metadata_path, ...@@ -120,8 +112,15 @@ def filter_colmap_model(input, output, metadata_path,
quat_columns = ["colmap_qw", "colmap_qx", "colmap_qy", "colmap_qz"] quat_columns = ["colmap_qw", "colmap_qx", "colmap_qy", "colmap_qz"]
metadata[tvec_columns] = pd.DataFrame(tvec, index=image_df.index) metadata[tvec_columns] = pd.DataFrame(tvec, index=image_df.index)
metadata[quat_columns] = pd.DataFrame(qvec, 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 # 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[tvec_columns] = metadata[tvec_columns].interpolate()
metadata[["colmap_qx", "colmap_qy", "colmap_qz", "colmap_qw"]] = slerp_quats(metadata, prefix="colmap_") 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, ...@@ -183,24 +182,46 @@ def filter_colmap_model(input, output, metadata_path,
colmap_speeds.plot() colmap_speeds.plot()
plt.gcf().canvas.set_window_title('speeds from colmap (noisy)') plt.gcf().canvas.set_window_title('speeds from colmap (noisy)')
metadata[["x", "y", "z", "tx", "ty", "tz"]].plot() 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() metadata[["ref_qw", "ref_qx", "ref_qy", "ref_qz"]].plot()
plt.gcf().canvas.set_window_title('quaternions from sensor') 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"] | \ metadata["outlier"] = metadata["outlier"] | \
get_outliers(metadata["outliers_pos"], threshold_t) | \ get_outliers(metadata["outliers_pos"], threshold_t) | \
get_outliers(metadata["outlier_rot"], threshold_q) 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 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_qvec_interp = slerp_quats(metadata).values
world_tvec_smoothed = savgol_filter(world_tvec_interp, filter_length, filter_degree, axis=0) 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 = savgol_filter(world_qvec_interp, filter_length, filter_degree, axis=0)
qvec_smoothed /= np.linalg.norm(qvec_smoothed, axis=1, keepdims=True) qvec_smoothed /= np.linalg.norm(qvec_smoothed, axis=1, keepdims=True)
metadata["tx_smoothed"] = world_tvec_smoothed[:, 0] colmap_tvec_smoothed = world_to_colmap(world_tvec_smoothed, qvec_smoothed)
metadata["ty_smoothed"] = world_tvec_smoothed[:, 1]
metadata["tz_smoothed"] = world_tvec_smoothed[:, 2] 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["qx_smoothed"] = qvec_smoothed[:, 0]
metadata["qy_smoothed"] = qvec_smoothed[:, 1] metadata["qy_smoothed"] = qvec_smoothed[:, 1]
...@@ -208,54 +229,50 @@ def filter_colmap_model(input, output, metadata_path, ...@@ -208,54 +229,50 @@ def filter_colmap_model(input, output, metadata_path,
metadata["qw_smoothed"] = qvec_smoothed[:, 3] metadata["qw_smoothed"] = qvec_smoothed[:, 3]
if visualize: 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') plt.gcf().canvas.set_window_title('speed from filtered and smoothed')
metadata[["qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].plot(ax=ax_q)
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[["q{}2_smoothed".format(col) for col in list('wxyz')]] = metadata[['qw_smoothed', metadata[["q{}2_smoothed".format(col) for col in list('wxyz')]] = metadata[['qw_smoothed',
'qx_smoothed', 'qx_smoothed',
'qy_smoothed', 'qy_smoothed',
'qz_smoothed']].shift() '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 = metadata.apply(quaternion_distances(suffix="_smoothed"), axis='columns')
metadata[["colmap_qdist", "qdist_filtered", "qdist_smoothed"]].plot() metadata[["qdist_smoothed"]].plot(ax=ax_qdist)
plt.gcf().canvas.set_window_title('GPS(xyz) vs colmap position')
metadata[["outlier"]].astype(float).plot() metadata[["outlier"]].astype(float).plot()
plt.gcf().canvas.set_window_title('outliers indices') plt.gcf().canvas.set_window_title('outliers indices')
print("number of not localized by colmap : {}/{} ({:.2f}%)".format(colmap_outliers, print("number of not localized by colmap : {}/{} ({:.2f}%)".format(colmap_outliers,
len(metadata), total_frames,
100 * colmap_outliers/len(metadata))) 100 * colmap_outliers/total_frames))
print("Total number of outliers : {} / {} ({:.2f}%)".format(sum(metadata["outlier"]), print("Total number of outliers : {} / {} ({:.2f}%)".format(sum(metadata["outlier"]),
len(metadata), total_frames,
100 * sum(metadata["outlier"])/len(metadata))) 100 * sum(metadata["outlier"])/total_frames))
if visualize: if visualize:
plt.show() plt.show()
smoothed_images_dict = {} if output_images_colmap is not None:
for db_id, row in metadata.iterrows(): smoothed_images_dict = {}
smoothed_images_dict[db_id] = rm.Image(id=db_id, interpolated_frames = []
qvec=row[["qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].values, for _, row in metadata.iterrows():
tvec=row[["tx_smoothed", "ty_smoothed", "tz_smoothed"]].values, db_id = row["db_id"]
camera_id=row["camera_id"], if row["outlier"]:
name=row["image_path"], interpolated_frames.append(row["image_path"])
xys=[], point3D_ids=[]) smoothed_images_dict[db_id] = rm.Image(id=db_id,
rm.write_images_text(smoothed_images_dict, output / "images.txt") qvec=row[["qw_smoothed", "qx_smoothed", "qy_smoothed", "qz_smoothed"]].values,