From 61d58fd5d5fc3e78d455c68f6b12a836a4376b83 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 16 Sep 2020 09:48:50 +0200 Subject: [PATCH 01/64] New features for the annotation UI Press the 'Auto GCP' button in any of the views to visualize tracks. The tracks are color-coded, brighter tracks are longer. Now click on any track to automatically create a GCP from it. Now you only have to annotate the GCP on the images that didn't get reconstructed. Press the 'Analyze' button to get some metrics about the positional accuracy of each frame and the reprojection error of each GCP. This information will show up in the GCP list and frame lists of each view. It also enables the 'jump to worst GCP' function (press Q) Press Q (after having analyzed the annotations) to jump to the GCP that has the largest average reprojection error. Every view that has at least one annotation for this GCP will jump to the frame within the sequence that has the worst reprojection error. Use this to find wrongly annotated points. Smart zoom behaviour upon displaying a new frame (From scrolling or clicking on a frame list): - If sticky zoom is set and a GCP is selected: + If the GCP is visible, zoom to the GCP + otherwise, zoom to the last clicked x, y - If sticky zoom is not set or there is no selected GCP, the view will show all the image. Regardless of the 'sticky zoom' mode, the view will instantly zoom in after clicking to add a GCP. - w/s to move forwards/backwards on all sequences (linked or not) - x as a hotkey to enable / disable sticky zoom. A list next to each view displays a list of all the frames in the sequence. Clicking on a row in this list immediately jumps to that frame. Each row contains: - Frame index. - Count of annotated points. - Positional uncertainty of each frame. Only available if the annotations were analyzed (click 'Analyze') So there is no wasted time arranging windows before starting the annotation. --- annotation_gui_gcp/Database.py | 15 +- annotation_gui_gcp/GUI.py | 513 +++++++++++++++------- annotation_gui_gcp/apps.py | 7 +- annotation_gui_gcp/epipolarCalculation.py | 23 - annotation_gui_gcp/geometry.py | 53 +++ annotation_gui_gcp/main.py | 3 +- annotation_gui_gcp/run_ba.py | 306 +++++++++++++ 7 files changed, 734 insertions(+), 186 deletions(-) delete mode 100644 annotation_gui_gcp/epipolarCalculation.py create mode 100644 annotation_gui_gcp/geometry.py create mode 100644 annotation_gui_gcp/run_ba.py diff --git a/annotation_gui_gcp/Database.py b/annotation_gui_gcp/Database.py index 20826b992..11cf903a8 100644 --- a/annotation_gui_gcp/Database.py +++ b/annotation_gui_gcp/Database.py @@ -23,7 +23,7 @@ def __init__(self, seqs, path, preload_images=True): for k in keys: self.get_image(k) - p_gcp_errors = self.get_path() + '/gcp_reprojections.json' + p_gcp_errors = self.path + '/gcp_reprojections.json' if os.path.exists(p_gcp_errors): self.load_gcp_reprojections(p_gcp_errors) else: @@ -46,9 +46,6 @@ def go_to_image_path(self): img_folder = self.path + "/images/" return img_folder - def get_path(self): - return self.path - def init_points(self, points): for point in points: point_id, observations = point['id'], point['observations'] @@ -80,7 +77,6 @@ def get_image_size(self, img_name): def get_visible_points_coords(self, main_image): visible_points_coords = OrderedDict() for point_id, observations in self.points.items(): - pair_images = [obs["shot_id"] for obs in observations] for observation in observations: if observation["shot_id"] == main_image: visible_points_coords[point_id] = observation["projection"] @@ -136,6 +132,8 @@ def compute_gcp_errors(self): for gcp_id in self.points: error_avg[gcp_id] = 0 for gcp_id in self.gcp_reprojections: + if gcp_id not in self.points: + continue for shot_id in self.gcp_reprojections[gcp_id]: err = self.gcp_reprojections[gcp_id][shot_id]['error'] error_avg[gcp_id] += err @@ -147,8 +145,11 @@ def compute_gcp_errors(self): return worst_gcp, shot_worst_gcp, worst_gcp_error, error_avg - def get_worst_gcp(self): - return self.compute_gcp_errors()[:3] + def shot_with_max_gcp_error(self, image_keys, gcp): + # Return they key with most reprojection error for this GCP + annotated_images = set(self.gcp_reprojections[gcp]).intersection(set(image_keys)) + errors = {k: self.gcp_reprojections[gcp][k]['error'] for k in annotated_images} + return max(errors, key=lambda k: errors[k]) def remove_gcp(self, point_id): if self.point_exists(point_id): diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 8540f92d6..6b017bbab 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -4,8 +4,11 @@ import time import matplotlib import os +import subprocess +import sys +import numpy as np -from apps import distinct_colors, read_gcp_file, id_generator, convert_tuple_cords_to_list +from apps import distinct_colors, read_gcp_file, id_generator matplotlib.use('TkAgg') from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk) @@ -14,13 +17,14 @@ from matplotlib.text import TextPath from matplotlib.transforms import IdentityTransform from matplotlib.offsetbox import AnnotationBbox, AuxTransformBox -from epipolarCalculation import calc_epipol_line +from matplotlib import pyplot as plt +from geometry import get_all_track_observations, get_tracks_visible_in_image -from opensfm import features from apps import CustomListbox PREVIOUS_UNICODE = u"\u2193" NEXT_UNICODE = u"\u2191" +FONT = "TkFixedFont" class NavigationToolbar(NavigationToolbar2Tk): @@ -30,7 +34,7 @@ def set_message(self, m): class Gui: - def __init__(self, master, database, n_views, sequence_groups=[]): + def __init__(self, master, database, n_views, sequence_groups=()): self.database = database self.point_idx = 0 self.match_idx = 0 @@ -41,16 +45,22 @@ def __init__(self, master, database, n_views, sequence_groups=[]): master.bind_all('q', lambda event: self.go_to_worst_gcp()) master.bind_all('z', lambda event: self.toggle_zoom_on_key()) + master.bind_all('x', lambda event: self.toggle_sticky_zoom()) + master.bind_all('w', lambda event: self.go_to_next_image_all()) + master.bind_all('s', lambda event: self.go_to_prev_image_all()) self.create_ui(master, n_views=n_views) master.lift() - p_default_gcp = self.database.get_path() + '/ground_control_points.json' + p_default_gcp = self.database.path + '/ground_control_points.json' if os.path.exists(p_default_gcp): self.load_gcps(p_default_gcp) - p_shot_std = self.database.get_path() + '/shots_std.csv' + p_shot_std = self.database.path + '/shots_std.csv' if os.path.exists(p_shot_std): self.load_shot_std(p_shot_std) + for view in self.views: + self.populate_sequence_list(view) + def create_ui(self, master, n_views): self.master = master @@ -60,39 +70,65 @@ def create_ui(self, master, n_views): self.views = [] for view_ix in range(n_views): - new_window = tk.Toplevel(self.master) - new_window.title(f"View {view_ix+1}") - self.views.append(new_window) - - self.init_image_windows() + new_view = tk.Toplevel(self.master, name=f"view_ix_{view_ix}") + new_view.title(f"View {view_ix+1}") + self.views.append(new_view) + + self.init_image_views() + self.arrange_ui_onerow() + + def arrange_ui_onerow(self): + master = self.master + # Arrange views on the screen. All views on one single row + w = master.winfo_width() + h = master.winfo_height() + master.geometry("%dx%d+%d+%d" % (w, h, 0, 0)) + x = master.winfo_rootx() + y = master.winfo_rooty() + x, y = 0, y + h + screen_width = master.winfo_screenwidth() + screen_height = master.winfo_screenheight() + w = screen_width / len(self.views) + h = screen_height - y + for view in self.views: + view.geometry("%dx%d+%d+%d" % (w, h, x, y)) + x += w def create_tools(self, master): gcp_list_frame = tk.Frame(master) gcp_list_frame.pack(side='top', fill=tk.BOTH, expand=1) - self.gcp_list_box = CustomListbox(gcp_list_frame, font=("monospace", 10), width=10) - self.gcp_list_box.pack(side='left', expand=tk.YES, fill=tk.Y) - self.gcp_list_box.bind('<>', self.modify_gcp) + self.gcp_list_box = CustomListbox(gcp_list_frame, font=FONT) + self.gcp_list_box.pack(side='left', expand=True, fill=tk.Y) + self.gcp_list_box.bind('<>', self.onclick_gcp_list) plus_minus_frame = tk.Frame(master) plus_minus_frame.pack(side='top') - self.add_button = tk.Button(plus_minus_frame, text="+", command=self.add_gcp) - self.add_button.pack(side='left') - self.remove_button = tk.Button(plus_minus_frame, text="-", command=self.remove_gcp) - self.remove_button.pack(side='left') + add_button = tk.Button(plus_minus_frame, text="Add GCP", command=self.add_gcp) + add_button.pack(side='left') + remove_button = tk.Button(plus_minus_frame, text="Remove GCP", command=self.remove_gcp) + remove_button.pack(side='left') + + # self.if_show_epipolar = tk.IntVar(value=0) + # self.check_button = tk.Checkbutton(master, text="Epipolar lines", + # var=self.if_show_epipolar) + # self.check_button.pack(side='top') + self.sticky_zoom = tk.BooleanVar(value=True) + sticky_zoom_button = tk.Checkbutton(master, text="Sticky zoom (x)", var=self.sticky_zoom) + sticky_zoom_button.pack(side='top') - self.if_show_epipolar = tk.IntVar(value=0) - self.check_button = tk.Checkbutton(master, text="Epipolar\nlines", - var=self.if_show_epipolar) - self.check_button.pack(side='top') + # self.check_button.pack(side='top') + + load_button = tk.Button(master, text="Analyze", command=self.analyze) + load_button.pack(side="top") io_frame = tk.Frame(master) - io_frame.pack(side='top') - self.load_button = tk.Button(io_frame, text="Load", command=self.load_gcps) - self.load_button.pack(side='top') - self.save_button = tk.Button(io_frame, text="Save", command=self.save_gcps) - self.save_button.pack(side='top') - self.save_button = tk.Button(io_frame, text="Save As", command=self.save_gcps_as) - self.save_button.pack(side='top') + io_frame.pack(side="top") + load_button = tk.Button(io_frame, text="Load", command=self.load_gcps) + load_button.pack(side="left") + save_button = tk.Button(io_frame, text="Save", command=self.save_gcps) + save_button.pack(side="left") + save_button = tk.Button(io_frame, text="Save As", command=self.save_gcps_as) + save_button.pack(side="left") def clear_artists(self, view): for artist in view.plt_artists: @@ -101,26 +137,40 @@ def clear_artists(self, view): def set_title(self, view): shot = view.current_image - seq_key = view.current_sequence + seq_key = view.sequence_key seq = self.database.seqs[seq_key] seq_ix = seq.index(shot) - - if shot in self.shot_std: - shot_std_rank, shot_std = self.shot_std[shot] - title = "{} [{}/{}]: {} - #{} (std = {:.2f})".format( - seq_key, seq_ix+1, len(seq), shot, shot_std_rank, shot_std) - else: - title = "{} [{}/{}]: {}".format(seq_key, seq_ix+1, len(seq), shot) + title = "{} [{}/{}]: {}".format(seq_key, seq_ix+1, len(seq), shot) view.title(title) + def populate_sequence_list(self, view): + image_keys = view.image_keys + view.sequence_list_box.delete(0, tk.END) + n_digits = len(str(len(image_keys))) + for ix, shot in enumerate(image_keys): + points = self.database.get_visible_points_coords(shot) + txt = "{:0{n_digits}} {}".format(ix+1, len(points), n_digits=n_digits) + shot_std = self.shot_std.get(shot, None) + if shot_std: + txt += " {:.2f}".format(shot_std) + view.sequence_list_box.insert(tk.END, txt) + + def onclick_sequence_list(self, event): + widget = event.widget + sel = widget.curselection() + if not sel: + return + view = widget.master.master.master + self.go_to_image_index(view, int(sel[0])) - def init_image_windows(self, nav_buttons=False): - for idx, window in enumerate(self.views): - nth_viewer_frame = tk.Frame(window) - nth_viewer_frame.pack(side='left', fill=tk.BOTH, expand=1) + def init_image_views(self, nav_buttons=False): + for idx, view in enumerate(self.views): + canvas_frame = tk.Frame(view) + canvas_frame.pack(side='left', fill=tk.BOTH, expand=1) + view.canvas_frame = canvas_frame if nav_buttons: - button_frame = tk.Frame(nth_viewer_frame) + button_frame = tk.Frame(canvas_frame) button_frame.pack(side='top') prv_btn = tk.Button(button_frame, text=PREVIOUS_UNICODE, command=lambda _idx=idx: self.go_to_prev_image(self.views[_idx])) @@ -129,41 +179,124 @@ def init_image_windows(self, nav_buttons=False): command=lambda _idx=idx: self.go_to_next_image(self.views[_idx])) nxt_btn.pack(side='left') - window.current_sequence = list(self.database.seqs.keys())[idx] - window.current_image = self.database.seqs[window.current_sequence][0] - - window.figure = Figure() - window.subplot = window.figure.add_subplot(111) - window.subplot.imshow(self.database.get_image(window.current_image), aspect='auto') - window.subplot.axis('scaled') - window.figure.set_tight_layout(True) - window.subplot.axis('off') - - window.canvas = FigureCanvasTkAgg(window.figure, nth_viewer_frame) - window.canvas.draw() - window.canvas.get_tk_widget().pack(side='top', fill=tk.BOTH, expand=1) - window.canvas.mpl_connect('button_press_event', + view.image_keys = list(self.database.seqs.values())[idx] + view.sequence_key = list(self.database.seqs.keys())[idx] + view.current_image = view.image_keys[0] + + toolbox = tk.Frame(canvas_frame) + toolbox.pack(side='left', expand=False, fill=tk.Y) + show_tracks_button = tk.Button(toolbox, text="Auto GCP", command=lambda view=view: self.show_tracks(view)) + show_tracks_button.pack(side="top") + view.visible_tracks = None + view.sequence_list_box = CustomListbox(toolbox, font=FONT, width=12) + view.sequence_list_box.pack(side='top', expand=True, fill=tk.Y) + view.sequence_list_box.bind('<>', self.onclick_sequence_list) + + + view.figure = Figure() + view.subplot = view.figure.add_subplot(111) + view.subplot.imshow(self.database.get_image(view.current_image), aspect='auto') + view.subplot.axis('scaled') + view.figure.set_tight_layout(True) + view.subplot.axis('off') + + view.canvas = FigureCanvasTkAgg(view.figure, canvas_frame) + view.canvas.draw() + view.canvas.get_tk_widget().pack(side='top', fill=tk.BOTH, expand=1) + view.canvas.mpl_connect('button_press_event', lambda event: self.on_press(event)) - window.canvas.mpl_connect('scroll_event', + view.canvas.mpl_connect('scroll_event', lambda event: self.on_scroll(event)) - window.zoomed_in = False - window.plt_artists = [] - self.set_title(window) + view.zoomed_in = False + view.last_clicked = None + view.plt_artists = [] + self.set_title(view) + + + def show_tracks(self, view): + h, w = self.database.get_image_size(view.current_image) + tracks = get_tracks_visible_in_image(self.database, view.current_image) + + # Select some tracks to plot, not all + # nice_tracks = sorted(tracks, key=lambda tid: -tracks[tid]['length']) + nice_tracks = tracks.keys() # all tracks + tracks = {k:tracks[k] for k in nice_tracks} + + if len(tracks) == 0: + print("No valid tracks found") + return + + # Draw track projections + points = [] + track_lengths = [] + for t in tracks.values(): + points.append(t[0]) + track_lengths.append(t[1]) + + points = np.array(points) + norm = matplotlib.colors.Normalize() + colors = plt.cm.viridis(norm(track_lengths)) + view.tracks_scatter = view.subplot.scatter(points[:,0], points[:,1], s=10, c=colors, marker='x') + view.canvas.draw_idle() + + # The next left or right click will be handled differently by setting this: + view.visible_tracks = tracks + + + def create_gcp_from_track(self, view, x, y, add): + # Find track closest to click location and create a GCP from it + if add: + points, track_ids = [], [] + for tid, t in view.visible_tracks.items(): + points.append(t[0]) + track_ids.append(tid) + dists = np.linalg.norm(np.array(points) - np.array([[x,y]]), axis=1) + closest_track = track_ids[np.argmin(dists)] + observations = get_all_track_observations(self.database, closest_track) + + new_gcp = self.add_gcp() + print(f"Created new GCP {new_gcp} from track {closest_track} with {len(observations)} observations") + for shot_id, point in observations.items(): + self.database.add_point_observation(new_gcp, shot_id, point) + + view.visible_tracks = None + view.tracks_scatter.remove() + view.canvas.draw() + + + def analyze(self): + # Call the run_ba script + subprocess.run([ + sys.executable, + os.path.dirname(__file__) + '/run_ba.py', + self.database.path + ] + ) + self.shot_std = {} + self.load_shot_std(self.database.path + '/shots_std.csv') + p_gcp_errors = self.database.path + '/gcp_reprojections.json' + self.database.load_gcp_reprojections(p_gcp_errors) + + for view in self.views: + self.populate_sequence_list(view) + + print("Done analyzing") + def load_shot_std(self, path): with open(path, 'r') as f: - for ix, line in enumerate(f): + for line in f: shot, std = line[:-1].split(',') - self.shot_std[shot] = (ix + 1, float(std)) + self.shot_std[shot] = float(std) def load_gcps(self, filename=None): if filename is None: filename = filedialog.askopenfilename( title="Open GCP file", - initialdir=self.database.get_path(), + initialdir=self.database.path, filetypes=(("JSON files", "*.json"), ("all files", "*.*")), ) if filename is None: @@ -171,11 +304,11 @@ def load_gcps(self, filename=None): points = read_gcp_file(filename) self.last_saved_filename = filename self.database.init_points(points) - for view_ix, view in enumerate(self.views): + for view in self.views: self.display_points(view) - if self.if_show_epipolar.get(): - self.show_epipolar_lines(view_ix) - self.repopulate_modify_list() + # if self.if_show_epipolar.get(): + # self.show_epipolar_lines(view_ix) + self.populate_gcp_list() def display_points(self, view): visible_points_coords = self.database.get_visible_points_coords(view.current_image) @@ -187,10 +320,18 @@ def display_points(self, view): p1 = PathPatch(text_path, transform=IdentityTransform(), alpha=1, color=color) offsetbox2 = AuxTransformBox(IdentityTransform()) offsetbox2.add_artist(p1) - ab = AnnotationBbox(offsetbox2, ((coords[0] + 30), (coords[1] + 30)), bboxprops=dict(alpha=0.05)) - circle = mpatches.Circle((coords[0], coords[1]), 10, color=color, fill=False) - dot = mpatches.Circle((coords[0], coords[1]), 2, color=color, fill=False) - for art in (ab, circle, dot): + artists = [ + AnnotationBbox(offsetbox2, ((coords[0] + 30), (coords[1] + 30)), bboxprops={"alpha":0.05}), + mpatches.Circle((coords[0], coords[1]), 10, color=color, fill=False), + mpatches.Circle((coords[0], coords[1]), 2, color=color, fill=False), + ] + + if point_id == self.curr_point: + artists.extend([ + mpatches.Circle((coords[0], coords[1]), 12, color=color, fill=False), + mpatches.Circle((coords[0], coords[1]), 14, color=color, fill=False), + ]) + for art in artists: view.plt_artists.append(art) view.subplot.add_artist(art) @@ -203,23 +344,26 @@ def add_gcp(self): self.gcp_list_box.insert(tk.END, new_id) self.gcp_list_box.selection_clear(0, tk.END) self.gcp_list_box.selection_set(tk.END) + return new_id - def which_canvas(self, event): - for ix, view in enumerate(self.views): + def which_view(self, event): + for view in self.views: if event.canvas == view.canvas: - return ix + return view return None def on_scroll(self, event): - idx = self.which_canvas(event) + view = self.which_view(event) if event.xdata is None or event.ydata is None: return if event.button == 'up': - self.go_to_next_image(self.views[idx]) + self.go_to_next_image(view) elif event.button == 'down': - self.go_to_prev_image(self.views[idx]) + self.go_to_prev_image(view) def zoom_in(self, view, x, y): + if view.zoomed_in: + return xlim = view.subplot.get_xlim() width = max(xlim) - min(xlim) view.subplot.set_xlim(x - width / 20, x + width / 20) @@ -230,11 +374,17 @@ def zoom_out(self, view): view.subplot.autoscale() view.zoomed_in = False + def toggle_sticky_zoom(self): + if self.sticky_zoom.get(): + self.sticky_zoom.set(False) + else: + self.sticky_zoom.set(True) + def toggle_zoom_on_key(self): # Zoom in/out on every view, centered on the location of the current GCP if self.curr_point is None: return - any_zoomed_in = any([view.zoomed_in for view in self.views]) + any_zoomed_in = any(view.zoomed_in for view in self.views) for view in self.views: if any_zoomed_in: self.zoom_out(view) @@ -243,13 +393,30 @@ def toggle_zoom_on_key(self): if projection["shot_id"] == view.current_image: x, y = projection["projection"] self.zoom_in(view, x, y) + break view.canvas.draw_idle() + + def add_move_or_remove_gcp(self, view, x, y, leftclick): + current_image = view.current_image + if self.curr_point is None: + return + self.database.remove_point_observation(self.curr_point, current_image) + + for line in view.subplot.lines: + line.remove() + + if leftclick: # Left click + self.database.add_point_observation(self.curr_point, current_image, (x, y)) + self.zoom_in(view, x, y) + + # if self.if_show_epipolar.get(): + # self.show_epipolar_lines(idx) + + def on_press(self, event): - idx = self.which_canvas(event) - view = self.views[idx] + view = self.which_view(event) x, y = event.xdata, event.ydata - current_image = view.current_image if None in (x, y): return if event.button == 2: # Middle / wheel click: @@ -258,29 +425,28 @@ def on_press(self, event): else: self.zoom_in(view, x, y) view.figure.canvas.draw_idle() - elif self.curr_point is not None and event.button in (1, 3): + elif event.button in (1, 3): # Left click or right click - if not self.curr_point: - return - self.database.remove_point_observation(self.curr_point, current_image) - - for line in self.views[idx].subplot.lines: - line.remove() - if event.button == 1: # Left click - self.database.add_point_observation(self.curr_point, current_image, (x, y)) - + view.last_clicked = x, y + if view.visible_tracks: + self.create_gcp_from_track(view, x, y, event.button == 1) + elif self.curr_point is not None: + self.add_move_or_remove_gcp(view, x, y, event.button == 1) + self.populate_sequence_list(view) self.display_points(view) - if self.if_show_epipolar.get(): - self.show_epipolar_lines(idx) else: return - def repopulate_modify_list(self): + def populate_gcp_list(self): self.gcp_list_box.delete(0, tk.END) errors = self.database.compute_gcp_errors()[-1] sorted_gcp_ids = sorted(errors, key=lambda k: -errors[k]) - for point_id in sorted_gcp_ids: + self.gcps = sorted_gcp_ids + for point_id in self.gcps: self.gcp_list_box.insert(tk.END, "{}".format(point_id)) + self.gcp_list_box.insert(tk.END, "none") + self.gcp_list_box.selection_clear(0, tk.END) + self.master.update_idletasks() def remove_gcp(self): @@ -290,17 +456,23 @@ def remove_gcp(self): self.curr_point = None self.database.remove_gcp(to_be_removed_point) - for image_idx, view in enumerate(self.views): + for view in self.views: self.display_points(view) - if self.if_show_epipolar.get(): - self.show_epipolar_lines(image_idx) + # if self.if_show_epipolar.get(): + # self.show_epipolar_lines(image_idx) - self.repopulate_modify_list() + self.populate_gcp_list() - def modify_gcp(self, event): + def onclick_gcp_list(self, event): widget = event.widget value = widget.get(int(widget.curselection()[0])) - self.curr_point = value + if value == 'none': + self.curr_point = None + else: + self.curr_point = value + + for view in self.views: + self.display_points(view) def save_gcps(self): if self.last_saved_filename is None: @@ -312,7 +484,7 @@ def save_gcps_as(self): filename = filedialog.asksaveasfilename( initialfile="ground_control_points.json", title="Save GCP file", - initialdir=self.database.get_path(), + initialdir=self.database.path, defaultextension=".json", ) if filename is None: @@ -321,67 +493,75 @@ def save_gcps_as(self): self.last_saved_filename = filename return self.save_gcps() - def show_epipolar_lines(self, main_image_idx): - if len(self.views) > 2: - raise NotImplementedError("Not implemented yet for >2 views") - img1 = self.views[0].current_image - img2 = self.views[1].current_image - img1_size = self.database.get_image_size(img1) - img2_size = self.database.get_image_size(img2) - matched_points = self.database.get_visible_points_coords(self.views[main_image_idx].current_image) - matched_points_coords = convert_tuple_cords_to_list(matched_points) - matched_points_coords = features.normalized_image_coordinates(matched_points_coords, img1_size[1], img1_size[0]) - color_idx = 0 - for point_idx, point in enumerate(matched_points_coords): - image_pair = [img1, img2] - line = calc_epipol_line(point, image_pair, self.database.get_path(), main_image_idx) - denormalized_lines = features.denormalized_image_coordinates(line, img2_size[1], img2_size[0]) - for line_segment in denormalized_lines: - circle = mpatches.Circle((line_segment[0], line_segment[1]), 3, - color=distinct_colors[divmod(hash(list(matched_points.keys())[point_idx]), 19)[1]]) - self.views[main_image_idx].plt_artists.append(circle) - self.views[not main_image_idx].subplot.add_artist(circle) - color_idx = color_idx + 1 - self.views[not main_image_idx].figure.canvas.draw_idle() - - def go_to_adjacent_image(self, view, offset): - views_to_update = set([view]) - target_ix = self.database.get_image_index(view.current_image, view.current_sequence) + offset - - for v in self.views: - for group in self.sequence_groups: - if view.current_sequence in group: - views_to_update.add(v) - + # def show_epipolar_lines(self, main_image_idx): + # if len(self.views) > 2: + # raise NotImplementedError("Not implemented yet for >2 views") + # img1 = self.views[0].current_image + # img2 = self.views[1].current_image + # img1_size = self.database.get_image_size(img1) + # img2_size = self.database.get_image_size(img2) + # matched_points = self.database.get_visible_points_coords(self.views[main_image_idx].current_image) + # matched_points_coords = convert_tuple_cords_to_list(matched_points) + # matched_points_coords = features.normalized_image_coordinates(matched_points_coords, img1_size[1], img1_size[0]) + # color_idx = 0 + # for point_idx, point in enumerate(matched_points_coords): + # image_pair = [img1, img2] + # line = calc_epipol_line(point, image_pair, self.database.path, main_image_idx) + # denormalized_lines = features.denormalized_image_coordinates(line, img2_size[1], img2_size[0]) + # for line_segment in denormalized_lines: + # circle = mpatches.Circle((line_segment[0], line_segment[1]), 3, + # color=distinct_colors[divmod(hash(list(matched_points.keys())[point_idx]), 19)[1]]) + # self.views[main_image_idx].plt_artists.append(circle) + # self.views[not main_image_idx].subplot.add_artist(circle) + # color_idx = color_idx + 1 + # self.views[not main_image_idx].figure.canvas.draw_idle() + + def go_to_image_index(self, view, idx, linked=True): + views_to_update = {view} + if linked: + groups_this_view = [g for g in self.sequence_groups if view.sequence_key in g] + for g in groups_this_view: + for v in self.views: + if v.sequence_key in g: + views_to_update.add(v) for v in views_to_update: - new_image = self.database.bring_image(v.current_sequence, target_ix) + new_image = self.database.bring_image(v.sequence_key, idx) self.bring_new_image(new_image, v) + def go_to_adjacent_image(self, view, offset, linked=True): + target_ix = self.database.get_image_index(view.current_image, view.sequence_key) + offset + self.go_to_image_index(view, target_ix, linked) + + def go_to_next_image_all(self): + for view in self.views: + self.go_to_adjacent_image(view, +1, linked=False) + + def go_to_prev_image_all(self): + for view in self.views: + self.go_to_adjacent_image(view, -1, linked=False) + def go_to_next_image(self, view): self.go_to_adjacent_image(view, +1) def go_to_prev_image(self, view): self.go_to_adjacent_image(view, -1) - def highlight_gcp_reprojection(self, image_idx, shot, point_id): + def highlight_gcp_reprojection(self, view, shot, point_id): x, y = 0,0 for obs in self.database.points[point_id]: if obs['shot_id'] == shot: x, y = obs['projection'] x2, y2 = self.database.gcp_reprojections[point_id][shot]['reprojection'] - self.views[image_idx].subplot.plot([x, x2], [y, y2], 'r-') - self.canvases[image_idx].draw_idle() + view.subplot.plot([x, x2], [y, y2], 'r-') + view.canvas.draw_idle() + self.zoom_in(view, x, y) def go_to_worst_gcp(self): if len(self.database.gcp_reprojections) == 0: print("No GCP reprojections available. Can't jump to worst GCP") return - worst_gcp, shot_worst_gcp, worst_gcp_error = self.database.get_worst_gcp() - for i, sequence_images in enumerate(self.database.seqs.values()): - if shot_worst_gcp in sequence_images: - idx_worst_gcp = i - print("Worst GCP observation: {} in shot {}".format(worst_gcp, shot_worst_gcp)) + worst_gcp = self.database.compute_gcp_errors()[0] self.curr_point = worst_gcp self.gcp_list_box.selection_clear(0, "end") @@ -390,8 +570,37 @@ def go_to_worst_gcp(self): self.gcp_list_box.selection_set(ix) break - self.bring_new_image(shot_worst_gcp, self.views[idx_worst_gcp]) - self.highlight_gcp_reprojection(idx_worst_gcp, shot_worst_gcp, worst_gcp) + # For each view + for view in self.views: + # Get the shot with worst reprojection error that in this view + shot_worst_gcp = self.database.shot_with_max_gcp_error(view.image_keys, worst_gcp) + self.bring_new_image(shot_worst_gcp, view) + self.highlight_gcp_reprojection(view, shot_worst_gcp, worst_gcp) + + def point_in_view(self, view, point): + if point is None: + return None + for projection in self.database.points[point]: + if projection["shot_id"] == view.current_image: + return projection["projection"] + return None + + def zoom_logic(self, view): + gcp_visible = self.point_in_view(view, self.curr_point) + if self.sticky_zoom.get() and (self.curr_point and view.last_clicked or gcp_visible): + if gcp_visible is not None: + # Zoom in to the currently selected GCP if visible + x, y = gcp_visible + self.zoom_in(view, x, y) + else: + # Zoom in to the last clicked location if there is a selected GCP + x, y = view.last_clicked + self.zoom_in(view, x, y) + else: + # Show the whole image + view.subplot.axis("scaled") + view.figure.set_tight_layout(True) + view.subplot.axis("off") def bring_new_image(self, new_image, view): if new_image == view.current_image: @@ -401,13 +610,13 @@ def bring_new_image(self, new_image, view): view.subplot.clear() view.subplot.imshow(self.database.get_image(new_image)) view.subplot.axis('off') - self.set_title(view) view.zoomed_in = False - view.subplot.axis('scaled') - view.figure.set_tight_layout(True) - view.subplot.axis('off') + self.set_title(view) + # if self.if_show_epipolar.get(): + # for idx, view in enumerate(self.views): + # self.show_epipolar_lines(idx) + + self.zoom_logic(view) + self.display_points(view) - if self.if_show_epipolar.get(): - for idx, view in enumerate(self.views): - self.show_epipolar_lines(idx) print("Took {:.2f}s to bring_new_image {}".format(time.time()-t0, new_image)) diff --git a/annotation_gui_gcp/apps.py b/annotation_gui_gcp/apps.py index 46e5fce7f..d05f557a8 100644 --- a/annotation_gui_gcp/apps.py +++ b/annotation_gui_gcp/apps.py @@ -123,6 +123,7 @@ def save_gcp(gcp_test, gcp_train, path): def read_gcp_file(file_path): - file = open(file_path) - data = json.load(file) - return data['points'] + if file_path: + with open(file_path, 'r') as f: + data = json.load(f) + return data['points'] diff --git a/annotation_gui_gcp/epipolarCalculation.py b/annotation_gui_gcp/epipolarCalculation.py deleted file mode 100644 index 4d1d3532e..000000000 --- a/annotation_gui_gcp/epipolarCalculation.py +++ /dev/null @@ -1,23 +0,0 @@ -from opensfm import dataset -import numpy as np - - -def calc_epipol_line(point_coord, img_pair, path, main_image_idx): - data = dataset.DataSet(path) - reconstruction = data.load_reconstruction() - if not reconstruction[0].get_shot(img_pair[main_image_idx]): - main_shot = reconstruction[1].get_shot(img_pair[main_image_idx]) - pair_shot = reconstruction[0].get_shot(img_pair[not main_image_idx]) - else : - main_shot = reconstruction[0].get_shot(img_pair[main_image_idx]) - pair_shot = reconstruction[0].get_shot(img_pair[not main_image_idx]) - -# CHANGE COMING FROM RECONS 1 - line_pts = [[] for i in range(2)] - for depth in np.arange(-100, 100, 5): - point3d = main_shot.back_project(point_coord, depth) - reprojection = pair_shot.project(point3d) - line_pts[0].append(reprojection[0]) - line_pts[1].append(reprojection[1]) - - return np.transpose(line_pts) diff --git a/annotation_gui_gcp/geometry.py b/annotation_gui_gcp/geometry.py new file mode 100644 index 000000000..f3cd934b5 --- /dev/null +++ b/annotation_gui_gcp/geometry.py @@ -0,0 +1,53 @@ +import numpy as np +from opensfm import dataset, features + + +def calc_epipol_line(point_coord, img_pair, path, main_image_idx): + data = dataset.DataSet(path) + reconstruction = data.load_reconstruction() + if not reconstruction[0].get_shot(img_pair[main_image_idx]): + main_shot = reconstruction[1].get_shot(img_pair[main_image_idx]) + pair_shot = reconstruction[0].get_shot(img_pair[not main_image_idx]) + else : + main_shot = reconstruction[0].get_shot(img_pair[main_image_idx]) + pair_shot = reconstruction[0].get_shot(img_pair[not main_image_idx]) + +# CHANGE COMING FROM RECONS 1 + line_pts = [[] for i in range(2)] + for depth in np.arange(-100, 100, 5): + point3d = main_shot.back_project(point_coord, depth) + reprojection = pair_shot.project(point3d) + line_pts[0].append(reprojection[0]) + line_pts[1].append(reprojection[1]) + + return np.transpose(line_pts) + +def get_all_track_observations(gcp_database, track_id): + print(f"Getting all observations of track {track_id}") + data = dataset.DataSet(gcp_database.path) + tracks_manager = data.load_tracks_manager() + track_obs = tracks_manager.get_track_observations(track_id) + out = {} + for shot_id, obs in track_obs.items(): + h, w = gcp_database.get_image_size(shot_id) + point_px = features.denormalized_image_coordinates(np.array([obs.point]), w, h)[0] + out[shot_id] = point_px + return out + +def get_tracks_visible_in_image(gcp_database, image_key, min_len=5): + print(f"Getting track observations visible in {image_key}") + data = dataset.DataSet(gcp_database.path) + tracks_manager = data.load_tracks_manager() + reconstruction = data.load_reconstruction()[0] + + out = {} + for track_id in reconstruction.points: + track_obs = tracks_manager.get_track_observations(track_id) + if len(track_obs) < min_len: + continue + for shot_id, obs in track_obs.items(): + if shot_id == image_key: + h, w = gcp_database.get_image_size(shot_id) + point_px = features.denormalized_image_coordinates(np.array([obs.point]), w, h)[0] + out[track_id] = point_px, len(track_obs) + return out diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index debff863c..6ec35fed7 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -5,7 +5,7 @@ from Database import Database from GUI import Gui -from opensfm import dataset, io +from opensfm import io def parse_args(): @@ -31,6 +31,7 @@ def parse_args(): "from the group will always show the same frame index. " "Useful for camera rigs. usage: -g sequence_key_1 sequence_key_2. " "Can be used multiple times to define several groups", + default=[], ) return parser.parse_args() diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py new file mode 100644 index 000000000..020ed191c --- /dev/null +++ b/annotation_gui_gcp/run_ba.py @@ -0,0 +1,306 @@ +import argparse +import json + +import numpy as np +import logging + +import opensfm.reconstruction as orec +from opensfm import align +from opensfm import dataset +from opensfm import log +from opensfm import transformations as tf +from opensfm import types + +logger = logging.getLogger(__name__) + + +def merge_reconstructions(reconstructions, tracks_manager): + """Put all reconstruction points and shots in a single one. + + No alignment is performed. Just merging the lists of points and + shots as they are. + + Point(track) names have to be unique in the merged reconstruction. + We add a prefix according to the source reconstruction index. + """ + merged = types.Reconstruction() + + for ix_r, reconstruction in enumerate(reconstructions): + for camera in reconstruction.cameras.values(): + merged.add_camera(camera) + + for point in reconstruction.points.values(): + new_point = merged.create_point(f"R{ix_r}_{point.id}", point.coordinates) + new_point.color = point.color + + for shot in reconstruction.shots.values(): + merged.create_shot(shot.id, shot.camera.id, shot.pose) + obsdict = tracks_manager.get_shot_observations(shot.id) + for track_id, obs in obsdict.items(): + merged_track_id = f"R{ix_r}_{track_id}" + if merged_track_id in merged.points: + merged.add_observation(shot.id, merged_track_id, obs) + + return merged + + +def resplit_reconstruction(merged, original_reconstructions): + """Resplit reconstructions that were previously merged. + + The original_reconstructions are used to decide which point and shot + goes on which of the split reconstructions. + """ + split = [] + + for ix_r, original in enumerate(original_reconstructions): + r = types.Reconstruction() + for shot_id in original.shots: + r.add_shot(merged.shots[shot_id]) + for point_id in original.points: + merged_point = merged.points[f"R{ix_r}_{point_id}"] + new_point = r.create_point(point_id, merged_point.coordinates) + new_point.color = merged_point.color + for camera_id in original.cameras: + r.add_camera(merged.cameras[camera_id]) + split.append(r) + + return split + + +def triangulate_gcps(gcps, reconstruction): + coords = [] + for gcp in gcps: + res = orec.triangulate_gcp(gcp, reconstruction.shots) + coords.append(res) + return coords + + +def reproject_gcps(gcps, reconstruction): + output = {} + for gcp in gcps: + point = orec.triangulate_gcp(gcp, reconstruction.shots) + if point is None: + point = np.nan + output[gcp.id] = {} + for observation in gcp.observations: + shot = reconstruction.shots[observation.shot_id] + reproj = shot.project(point) + error = np.linalg.norm(reproj - observation.projection) + output[gcp.id][observation.shot_id] = {'error': error, + 'reprojection': [reproj[0], reproj[1]]} + return output + + +def compute_gcp_std(gcp_errors): + """Compute the standard deviation of reprojection errors of all gcps.""" + all_errors = [] + for gcp_id in gcp_errors: + errors = [e['error'] for e in gcp_errors[gcp_id].values()] + print(f"gcp {gcp_id} mean reprojection error = {np.mean(errors)}") + all_errors.extend(errors) + return np.sqrt(np.mean(np.array(all_errors)**2)) + + +def find_alignment(points0, points1): + """Compute similarity transform between point sets. + + Returns (s, A, b) such that ``points0 = s * A * points1 + b`` + """ + v0, v1 = [], [] + for p0, p1 in zip(points0, points1): + if p0 is not None and p1 is not None: + v0.append(p0) + v1.append(p1) + v0 = np.array(v0).T + v1 = np.array(v1).T + M = tf.affine_matrix_from_points(v0, v1, shear=False) + s = np.linalg.det(M[:3, :3])**(1. / 3.) + A = M[:3, :3] / s + b = M[:3, 3] + return s, A, b + + +def add_gcp_to_bundle(ba, gcp, gcp_std, shots): + """Add Ground Control Points constraints to the bundle problem.""" + for point in gcp: + point_id = 'gcp-' + point.id + + coordinates = orec.triangulate_gcp(point, shots) + if coordinates is None: + if point.coordinates is not None: + coordinates = point.coordinates + else: + logger.warning("Cannot initialize GCP '{}'." + " Ignoring it".format(point.id)) + continue + + ba.add_point(point_id, coordinates, False) + + for observation in point.observations: + if observation.shot_id in shots: + ba.add_point_projection_observation( + observation.shot_id, + point_id, + observation.projection[0], + observation.projection[1], + gcp_std) + + +def bundle_with_fixed_images(reconstruction, camera_priors, gcp, gcp_std, + fixed_images, config): + """Bundle adjust a reconstruction while keeping some images fixed.""" + fix_cameras = False + + chrono = orec.Chronometer() + ba = orec.pybundle.BundleAdjuster() + + for camera in reconstruction.cameras.values(): + camera_prior = camera_priors[camera.id] + ba.add_camera(camera.id, camera, camera_prior, fix_cameras) + + for shot in reconstruction.shots.values(): + r = shot.pose.rotation + t = shot.pose.translation + ba.add_shot(shot.id, shot.camera.id, r, t, shot.id in fixed_images) + + for point in reconstruction.points.values(): + ba.add_point(point.id, point.coordinates, False) + x, y, z = point.coordinates + ba.add_point_position_prior(point.id, x, y, z, 100.0) + + for shot_id in reconstruction.shots: + shot = reconstruction.get_shot(shot_id) + for point in shot.get_valid_landmarks(): + obs = shot.get_landmark_observation(point) + ba.add_point_projection_observation( + shot.id, point.id, obs.point[0], obs.point[1], obs.scale) + + add_gcp_to_bundle(ba, gcp, gcp_std, reconstruction.shots) + + ba.set_point_projection_loss_function(config['loss_function'], + config['loss_function_threshold']) + ba.set_internal_parameters_prior_sd( + config['exif_focal_sd'], + config['principal_point_sd'], + config['radial_distortion_k1_sd'], + config['radial_distortion_k2_sd'], + config['tangential_distortion_p1_sd'], + config['tangential_distortion_p2_sd'], + config['radial_distortion_k3_sd'], + config['radial_distortion_k4_sd']) + ba.set_num_threads(config['processes']) + ba.set_max_num_iterations(config['bundle_max_iterations']) + ba.set_linear_solver_type("SPARSE_SCHUR") + ba.set_compute_covariances(True) + + chrono.lap('setup') + ba.run() + chrono.lap('run') + + if not ba.get_covariance_estimation_valid(): + logger.warning('Could not compute covariance') + + for camera in reconstruction.cameras.values(): + orec._get_camera_from_bundle(ba, camera) + + for shot in reconstruction.shots.values(): + s = ba.get_shot(shot.id) + shot.pose.rotation = [s.r[0], s.r[1], s.r[2]] + shot.pose.translation = [s.t[0], s.t[1], s.t[2]] + shot.covariance = np.array([[s.get_covariance_inv_param(i, j) + for j in range(6)] for i in range(6)]) + + for point in reconstruction.points.values(): + p = ba.get_point(point.id) + point.coordinates = [p.p[0], p.p[1], p.p[2]] + point.reprojection_errors = p.reprojection_errors + + chrono.lap('teardown') + + print(ba.full_report()) + print(ba.brief_report()) + report = { + 'wall_times': dict(chrono.lap_times()), + 'brief_report': ba.brief_report(), + } + return report + + +def decompose_covariance(covariance): + """Decompose covariance into a rotation and diagonal scaling.""" + u, s, vh = np.linalg.svd(covariance) + return u, np.sqrt(s) + + +def parse_args(): + parser = argparse.ArgumentParser(description='Merge reconstructions and run BA with GCPs') + parser.add_argument( + 'dataset', + help='dataset to process', + ) + return parser.parse_args() + + +def main(): + args = parse_args() + path = args.dataset + data = dataset.DataSet(path) + + camera_models = data.load_camera_models() + tracks_manager = data.load_tracks_manager() + + reconstructions = [data.load_reconstruction()[index] for index in (0, 1)] + gcps = data.load_ground_control_points() + + coords0 = triangulate_gcps(gcps, reconstructions[0]) + coords1 = triangulate_gcps(gcps, reconstructions[1]) + + s, A, b = find_alignment(coords1, coords0) + align.apply_similarity(reconstructions[1], s, A, b) + + data.save_reconstruction(reconstructions, 'reconstruction_gcp_rigid.json') + + merged = merge_reconstructions(reconstructions, tracks_manager) + data.save_reconstruction([merged], 'reconstruction_merged.json') + + data.config['bundle_max_iterations'] = 200 + orec.bundle(merged, camera_models, gcp=gcps, config=data.config) + # rigid rotation to put images on the ground + orec.align_reconstruction(merged, None, data.config) + data.save_reconstruction([merged], 'reconstruction_gcp_ba.json') + + gcp_reprojections = reproject_gcps(gcps, merged) + with open(data.data_path + '/gcp_reprojections.json', 'w') as f: + json.dump(gcp_reprojections, f, indent=4, sort_keys=True) + + gcp_std = compute_gcp_std(gcp_reprojections) + print("GCP standard deviation:", gcp_std) + + resplit = resplit_reconstruction(merged, reconstructions) + data.save_reconstruction(resplit, 'reconstruction_gcp_ba_resplit.json') + + all_shots_std = [] + for rec_ixs in ((0,1), (1,0)): + fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) + bundle_with_fixed_images(merged, camera_models, gcp=gcps, gcp_std=gcp_std, + fixed_images=fixed_images, config=data.config) + + print("STD in the position of shots in rec {} w.r.t rec {}".format(rec_ixs[1], rec_ixs[0])) + for shot in merged.shots.values(): + if shot.id in reconstructions[rec_ixs[1]].shots: + u, std = decompose_covariance(shot.covariance[3:, 3:]) + all_shots_std.append((shot.id, np.linalg.norm(std))) + print(shot.id, 'position std:', std) + + # Save the shot STD to a file + with open(data.data_path + '/shots_std.csv', 'w') as f: + s = sorted(all_shots_std, key=lambda t:-t[-1]) + for t in s: + line = "{}, {}".format(*t) + f.write(line + '\n') + print("Worst shot: {} with std norm: {}".format(s[0][0], s[0][1])) + + +if __name__ == "__main__": + log.setup() + exit(main()) From 72976704680256327da69d903f2ac6b017f1752e Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 21 Sep 2020 14:43:06 +0200 Subject: [PATCH 02/64] Summary of analysis, better 'sticky' zoom behavior. --- annotation_gui_gcp/GUI.py | 40 +++++++++++++++++++++--------------- annotation_gui_gcp/run_ba.py | 35 ++++++++++++++++++++++++++++--- 2 files changed, 56 insertions(+), 19 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 6b017bbab..3a5b66179 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -185,8 +185,8 @@ def init_image_views(self, nav_buttons=False): toolbox = tk.Frame(canvas_frame) toolbox.pack(side='left', expand=False, fill=tk.Y) - show_tracks_button = tk.Button(toolbox, text="Auto GCP", command=lambda view=view: self.show_tracks(view)) - show_tracks_button.pack(side="top") + auto_gcp_button = tk.Button(toolbox, text="Auto GCP", command=lambda view=view: self.auto_gcp_show_tracks(view)) + auto_gcp_button.pack(side="top") view.visible_tracks = None view.sequence_list_box = CustomListbox(toolbox, font=FONT, width=12) view.sequence_list_box.pack(side='top', expand=True, fill=tk.Y) @@ -211,12 +211,12 @@ def init_image_views(self, nav_buttons=False): self.on_scroll(event)) view.zoomed_in = False - view.last_clicked = None + view.last_seen_px = {} view.plt_artists = [] self.set_title(view) - def show_tracks(self, view): + def auto_gcp_show_tracks(self, view): h, w = self.database.get_image_size(view.current_image) tracks = get_tracks_visible_in_image(self.database, view.current_image) @@ -246,7 +246,7 @@ def show_tracks(self, view): view.visible_tracks = tracks - def create_gcp_from_track(self, view, x, y, add): + def auto_gcp_create(self, view, x, y, add): # Find track closest to click location and create a GCP from it if add: points, track_ids = [], [] @@ -265,9 +265,16 @@ def create_gcp_from_track(self, view, x, y, add): view.visible_tracks = None view.tracks_scatter.remove() view.canvas.draw() + self.populate_sequence_list(view) def analyze(self): + # Check that there is a recent ground_control_points.json file + t = time.time() - os.path.getmtime(self.database.path + '/ground_control_points.json') + if t > 30: + print("Please save before running the analysis") + return + # Call the run_ba script subprocess.run([ sys.executable, @@ -427,9 +434,9 @@ def on_press(self, event): view.figure.canvas.draw_idle() elif event.button in (1, 3): # Left click or right click - view.last_clicked = x, y + view.last_seen_px[self.curr_point] = x, y if view.visible_tracks: - self.create_gcp_from_track(view, x, y, event.button == 1) + self.auto_gcp_create(view, x, y, event.button == 1) elif self.curr_point is not None: self.add_move_or_remove_gcp(view, x, y, event.button == 1) self.populate_sequence_list(view) @@ -587,15 +594,10 @@ def point_in_view(self, view, point): def zoom_logic(self, view): gcp_visible = self.point_in_view(view, self.curr_point) - if self.sticky_zoom.get() and (self.curr_point and view.last_clicked or gcp_visible): - if gcp_visible is not None: - # Zoom in to the currently selected GCP if visible - x, y = gcp_visible - self.zoom_in(view, x, y) - else: - # Zoom in to the last clicked location if there is a selected GCP - x, y = view.last_clicked - self.zoom_in(view, x, y) + if self.sticky_zoom.get() and self.curr_point in view.last_seen_px: + # Zoom in to the last seen location of this GCP + x, y = view.last_seen_px[self.curr_point] + self.zoom_in(view, x, y) else: # Show the whole image view.subplot.axis("scaled") @@ -616,6 +618,12 @@ def bring_new_image(self, new_image, view): # for idx, view in enumerate(self.views): # self.show_epipolar_lines(idx) + # Update 'last seen' coordinates for all gcps in this view + for gcp_id in self.database.points: + gcp_visible = self.point_in_view(view, gcp_id) + if gcp_visible is not None: + view.last_seen_px[gcp_id] = gcp_visible + self.zoom_logic(view) self.display_points(view) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 020ed191c..259db766b 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -91,6 +91,20 @@ def reproject_gcps(gcps, reconstruction): return output +def get_mean_max_reprojection_errors(gcp_reprojections): + max_gcp_reprojection_error = 0 + mean_gcp_reprojection_error = [] + for gcp_id in gcp_reprojections: + for shot_id in gcp_reprojections[gcp_id]: + e = gcp_reprojections[gcp_id][shot_id]['error'] + mean_gcp_reprojection_error.append(e) + if e > max_gcp_reprojection_error: + max_gcp_reprojection_error = e + mean_gcp_reprojection_error = np.mean(mean_gcp_reprojection_error) + median_gcp_reprojection_error = np.median(mean_gcp_reprojection_error) + return mean_gcp_reprojection_error, max_gcp_reprojection_error, median_gcp_reprojection_error + + def compute_gcp_std(gcp_errors): """Compute the standard deviation of reprojection errors of all gcps.""" all_errors = [] @@ -270,11 +284,12 @@ def main(): data.save_reconstruction([merged], 'reconstruction_gcp_ba.json') gcp_reprojections = reproject_gcps(gcps, merged) + mean_reprojection_error, max_reprojection_error, median_reprojection_error = get_mean_max_reprojection_errors(gcp_reprojections) with open(data.data_path + '/gcp_reprojections.json', 'w') as f: json.dump(gcp_reprojections, f, indent=4, sort_keys=True) gcp_std = compute_gcp_std(gcp_reprojections) - print("GCP standard deviation:", gcp_std) + print("GCP reprojection error STD:", gcp_std) resplit = resplit_reconstruction(merged, reconstructions) data.save_reconstruction(resplit, 'reconstruction_gcp_ba_resplit.json') @@ -290,7 +305,10 @@ def main(): if shot.id in reconstructions[rec_ixs[1]].shots: u, std = decompose_covariance(shot.covariance[3:, 3:]) all_shots_std.append((shot.id, np.linalg.norm(std))) - print(shot.id, 'position std:', std) + print(shot.id, 'position std:', np.linalg.norm(std)) + # Average positional STD + average_shot_std = np.mean([t[1] for t in all_shots_std]) + median_shot_std = np.median([t[1] for t in all_shots_std]) # Save the shot STD to a file with open(data.data_path + '/shots_std.csv', 'w') as f: @@ -298,7 +316,18 @@ def main(): for t in s: line = "{}, {}".format(*t) f.write(line + '\n') - print("Worst shot: {} with std norm: {}".format(s[0][0], s[0][1])) + + print("=============== Key metrics ================") + + print("Position STD") + print(f" max: {s[0][1]} ({s[0][0]})") + print(f" mean: {average_shot_std}") + print(f"median: {median_shot_std}") + + print("Reprojection errors [in px / max(w,h)]") + print(f" max: {max_reprojection_error}") + print(f" mean: {mean_reprojection_error}") + print(f"median: {median_reprojection_error}") if __name__ == "__main__": From d653a90ef20615ce5d363f13a65209fcba9bc740 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 25 Sep 2020 09:34:56 +0200 Subject: [PATCH 03/64] Add 'group by reconstruction' arg --- annotation_gui_gcp/main.py | 29 ++++++++++++++++++++++++++--- annotation_gui_gcp/run_ba.py | 4 ++-- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 6ec35fed7..3a18e2325 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -1,11 +1,11 @@ import argparse import tkinter as tk -from collections import OrderedDict +from collections import OrderedDict, defaultdict from pathlib import Path from Database import Database from GUI import Gui -from opensfm import io +from opensfm import io, dataset def parse_args(): @@ -14,6 +14,12 @@ def parse_args(): parser.add_argument( "-n", "--no-preload", help="skip preloading", action="store_true" ) + parser.add_argument( + "--group-by-reconstruction", + action="store_true", + help="If set, the UI will show one window per reconstruction, " + "otherwise, it will use sequences as specified by 'sequence-file'", + ) parser.add_argument( "--sequence-file", help="dict-of-image-keys JSON file describing each sequence. " @@ -81,10 +87,27 @@ def load_sequence_database_from_file( return seq_dict +def group_images_by_reconstruction(path): + data = dataset.DataSet(path) + reconstructions = data.load_reconstruction() + seq_dict = defaultdict(list) + for recons in reconstructions: + for shot in recons.shots.values(): + skey = shot.metadata.sequence_key.value + seq_dict[skey].append(shot) + for skey in seq_dict: + sorted_ims = sorted(seq_dict[skey], key=lambda x: x.metadata.capture_time.value) + seq_dict[skey] = [shot.id for shot in sorted_ims] + return seq_dict + + if __name__ == "__main__": args = parse_args() path = args.dataset - seqs = load_sequence_database_from_file(path, args.sequence_file, skip_missing=True) + if args.group_by_reconstruction: + seqs = group_images_by_reconstruction(path) + else: + seqs = load_sequence_database_from_file(path, args.sequence_file, skip_missing=True) database = Database(seqs, path, preload_images=not args.no_preload) root = tk.Tk() root.resizable(True, True) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 259db766b..a258831a4 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -321,12 +321,12 @@ def main(): print("Position STD") print(f" max: {s[0][1]} ({s[0][0]})") - print(f" mean: {average_shot_std}") + #print(f" mean: {average_shot_std}") print(f"median: {median_shot_std}") print("Reprojection errors [in px / max(w,h)]") print(f" max: {max_reprojection_error}") - print(f" mean: {mean_reprojection_error}") + #print(f" mean: {mean_reprojection_error}") print(f"median: {median_reprojection_error}") From be62ac3ad06a65d0bdf417c0467a2975f0b5b85e Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 25 Sep 2020 15:18:09 +0200 Subject: [PATCH 04/64] fix: copy shot metadata --- annotation_gui_gcp/run_ba.py | 1 + 1 file changed, 1 insertion(+) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index a258831a4..f1b3ac502 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -35,6 +35,7 @@ def merge_reconstructions(reconstructions, tracks_manager): for shot in reconstruction.shots.values(): merged.create_shot(shot.id, shot.camera.id, shot.pose) + merged.shots[shot.id].metadata.set(shot.metadata) obsdict = tracks_manager.get_shot_observations(shot.id) for track_id, obs in obsdict.items(): merged_track_id = f"R{ix_r}_{track_id}" From e7ec8e4a5c7876163abdbacf791d3e8d52f02ade Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 25 Sep 2020 15:22:40 +0200 Subject: [PATCH 05/64] chore: simplify shot copy --- annotation_gui_gcp/run_ba.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index f1b3ac502..5c020ac3e 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -34,8 +34,7 @@ def merge_reconstructions(reconstructions, tracks_manager): new_point.color = point.color for shot in reconstruction.shots.values(): - merged.create_shot(shot.id, shot.camera.id, shot.pose) - merged.shots[shot.id].metadata.set(shot.metadata) + merged.add_shot(shot) obsdict = tracks_manager.get_shot_observations(shot.id) for track_id, obs in obsdict.items(): merged_track_id = f"R{ix_r}_{track_id}" From 33b2297a1b4786ec69bbb9ac23c88177e5aa529a Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 30 Sep 2020 08:52:40 +0200 Subject: [PATCH 06/64] chore: add sequence_database.json to berlin dataset --- annotation_gui_gcp/run_ba.py | 26 ++++++++++++++++++-------- bin/opensfm_run_all | 6 +++--- data/berlin/sequence_database.json | 7 +++++++ 3 files changed, 28 insertions(+), 11 deletions(-) create mode 100644 data/berlin/sequence_database.json diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 5c020ac3e..70e164c5c 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -1,5 +1,6 @@ import argparse import json +import os import numpy as np import logging @@ -259,6 +260,14 @@ def main(): args = parse_args() path = args.dataset data = dataset.DataSet(path) + for fn in ( + 'reconstruction.json', + 'ground_control_points.json', + 'tracks.csv' + ): + if not (os.path.exists(os.path.join(path, fn))): + print(f"Missing file: {fn}") + return camera_models = data.load_camera_models() tracks_manager = data.load_tracks_manager() @@ -319,15 +328,16 @@ def main(): print("=============== Key metrics ================") - print("Position STD") - print(f" max: {s[0][1]} ({s[0][0]})") - #print(f" mean: {average_shot_std}") - print(f"median: {median_shot_std}") + metrics = { + 'median_shot_std' : median_shot_std, + 'max_shot_std': s[0][1], + 'max_reprojection_error': max_reprojection_error, + 'median_reprojection_error': median_reprojection_error, + } - print("Reprojection errors [in px / max(w,h)]") - print(f" max: {max_reprojection_error}") - #print(f" mean: {mean_reprojection_error}") - print(f"median: {median_reprojection_error}") + print(metrics) + with open(data.data_path + '/run_ba_metrics.json', 'w') as f: + json.dump(metrics, f, indent=4, sort_keys=True) if __name__ == "__main__": diff --git a/bin/opensfm_run_all b/bin/opensfm_run_all index 0b0cbced0..a19a35a48 100755 --- a/bin/opensfm_run_all +++ b/bin/opensfm_run_all @@ -12,6 +12,6 @@ $PYTHON $DIR/opensfm detect_features $1 $PYTHON $DIR/opensfm match_features $1 $PYTHON $DIR/opensfm create_tracks $1 $PYTHON $DIR/opensfm reconstruct $1 -$PYTHON $DIR/opensfm mesh $1 -$PYTHON $DIR/opensfm undistort $1 -$PYTHON $DIR/opensfm compute_depthmaps $1 +#$PYTHON $DIR/opensfm mesh $1 +#$PYTHON $DIR/opensfm undistort $1 +#$PYTHON $DIR/opensfm compute_depthmaps $1 diff --git a/data/berlin/sequence_database.json b/data/berlin/sequence_database.json new file mode 100644 index 000000000..b0a795408 --- /dev/null +++ b/data/berlin/sequence_database.json @@ -0,0 +1,7 @@ +{ + "sequence_1":[ + "01.jpg", + "02.jpg", + "03.jpg" + ] +} From a2915d9db9b242551cb0fef74237044457c98a44 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 5 Oct 2020 11:53:05 +0200 Subject: [PATCH 07/64] feat: better summary of run_ba.py in terminal --- annotation_gui_gcp/run_ba.py | 91 +++++++++++++++++++++++++++--------- bin/opensfm_run_all | 6 +-- 2 files changed, 73 insertions(+), 24 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 70e164c5c..6eb1aaaaa 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -92,18 +92,13 @@ def reproject_gcps(gcps, reconstruction): return output -def get_mean_max_reprojection_errors(gcp_reprojections): - max_gcp_reprojection_error = 0 - mean_gcp_reprojection_error = [] +def get_all_reprojection_errors(gcp_reprojections): + output = [] for gcp_id in gcp_reprojections: for shot_id in gcp_reprojections[gcp_id]: e = gcp_reprojections[gcp_id][shot_id]['error'] - mean_gcp_reprojection_error.append(e) - if e > max_gcp_reprojection_error: - max_gcp_reprojection_error = e - mean_gcp_reprojection_error = np.mean(mean_gcp_reprojection_error) - median_gcp_reprojection_error = np.median(mean_gcp_reprojection_error) - return mean_gcp_reprojection_error, max_gcp_reprojection_error, median_gcp_reprojection_error + output.append((gcp_id, shot_id, e)) + return sorted(output, key=lambda t:-t[2]) def compute_gcp_std(gcp_errors): @@ -111,7 +106,7 @@ def compute_gcp_std(gcp_errors): all_errors = [] for gcp_id in gcp_errors: errors = [e['error'] for e in gcp_errors[gcp_id].values()] - print(f"gcp {gcp_id} mean reprojection error = {np.mean(errors)}") + logger.info(f"gcp {gcp_id} mean reprojection error = {np.mean(errors)}") all_errors.extend(errors) return np.sqrt(np.mean(np.array(all_errors)**2)) @@ -232,8 +227,8 @@ def bundle_with_fixed_images(reconstruction, camera_priors, gcp, gcp_std, chrono.lap('teardown') - print(ba.full_report()) - print(ba.brief_report()) + logger.info(ba.full_report()) + logger.info(ba.brief_report()) report = { 'wall_times': dict(chrono.lap_times()), 'brief_report': ba.brief_report(), @@ -253,6 +248,16 @@ def parse_args(): 'dataset', help='dataset to process', ) + parser.add_argument( + '--std-threshold', + default=0.3, + help='positional threshold (m) above which we consider an image to be badly localized', + ) + parser.add_argument( + '--px-threshold', + default=0.008, # a little bit over 5 pixels at VGA resolution + help='threshold in normalized pixels above which we consider a GCP annotation to be wrong', + ) return parser.parse_args() @@ -266,13 +271,18 @@ def main(): 'tracks.csv' ): if not (os.path.exists(os.path.join(path, fn))): - print(f"Missing file: {fn}") + logger.error(f"Missing file: {fn}") return camera_models = data.load_camera_models() tracks_manager = data.load_tracks_manager() - reconstructions = [data.load_reconstruction()[index] for index in (0, 1)] + all_reconstructions = data.load_reconstruction() + + if len(all_reconstructions) > 2: + logger.warning(f"WARNING: There are more than two reconstructions in {path}") + + reconstructions = all_reconstructions[:2] gcps = data.load_ground_control_points() coords0 = triangulate_gcps(gcps, reconstructions[0]) @@ -293,12 +303,18 @@ def main(): data.save_reconstruction([merged], 'reconstruction_gcp_ba.json') gcp_reprojections = reproject_gcps(gcps, merged) - mean_reprojection_error, max_reprojection_error, median_reprojection_error = get_mean_max_reprojection_errors(gcp_reprojections) + reprojection_errors = get_all_reprojection_errors(gcp_reprojections) + n_bad_gcp_annotations = sum(t[2] > args.px_threshold for t in reprojection_errors) + err_values = [t[2] for t in reprojection_errors] + mean_reprojection_error = np.mean(err_values) + max_reprojection_error = np.max(err_values) + median_reprojection_error = np.median(err_values) + with open(data.data_path + '/gcp_reprojections.json', 'w') as f: json.dump(gcp_reprojections, f, indent=4, sort_keys=True) gcp_std = compute_gcp_std(gcp_reprojections) - print("GCP reprojection error STD:", gcp_std) + logger.info(f"GCP reprojection error STD: {gcp_std}") resplit = resplit_reconstruction(merged, reconstructions) data.save_reconstruction(resplit, 'reconstruction_gcp_ba_resplit.json') @@ -309,12 +325,20 @@ def main(): bundle_with_fixed_images(merged, camera_models, gcp=gcps, gcp_std=gcp_std, fixed_images=fixed_images, config=data.config) - print("STD in the position of shots in rec {} w.r.t rec {}".format(rec_ixs[1], rec_ixs[0])) + logger.info(f"STD in the position of shots in rec {rec_ixs[1]} w.r.t rec {rec_ixs[0]}") for shot in merged.shots.values(): if shot.id in reconstructions[rec_ixs[1]].shots: u, std = decompose_covariance(shot.covariance[3:, 3:]) all_shots_std.append((shot.id, np.linalg.norm(std))) - print(shot.id, 'position std:', np.linalg.norm(std)) + logger.info(f"{shot.id} position std: {np.linalg.norm(std)}") + + # If the STD of all shots is the same, replace by nan + std_values = [x[1] for x in all_shots_std] + n_bad_std = sum(std > args.std_threshold for std in std_values) + if np.allclose(std_values, std_values[0]): + all_shots_std = [(x[0], np.nan) for x in all_shots_std] + n_bad_std = len(std_values) + # Average positional STD average_shot_std = np.mean([t[1] for t in all_shots_std]) median_shot_std = np.median([t[1] for t in all_shots_std]) @@ -326,19 +350,44 @@ def main(): line = "{}, {}".format(*t) f.write(line + '\n') - print("=============== Key metrics ================") metrics = { + 'n_reconstructions': len(all_reconstructions), 'median_shot_std' : median_shot_std, 'max_shot_std': s[0][1], 'max_reprojection_error': max_reprojection_error, 'median_reprojection_error': median_reprojection_error, + 'n_gcp': len(gcps), + 'n_bad_gcp_annotations': int(n_bad_gcp_annotations), + 'n_bad_position_std': int(n_bad_std), } - print(metrics) - with open(data.data_path + '/run_ba_metrics.json', 'w') as f: + logger.info(metrics) + p_metrics = data.data_path + '/run_ba_metrics.json' + logger.info(f"Saved metrics to {p_metrics}") + with open(p_metrics, 'w') as f: json.dump(metrics, f, indent=4, sort_keys=True) + logger.info("========================================") + logger.info("=============== Summary ================") + logger.info("========================================") + if n_bad_std == 0 and n_bad_gcp_annotations == 0: + logger.info( + f"No issues. All gcp reprojections are under {args.px_threshold}" + "and all frames are localized within {args.std_threshold}m" + ) + else: + if np.isnan(all_shots_std[0][1]): + logger.info( + f"There is an issue while analyzing. It could be because: a) there are not enough GCPs." + " b) they are badly distributed in 3D. c) there are some wrong annotations" + ) + else: + logger.info(f"{n_bad_std} badly localized images (error>{args.std_threshold}). Use the frame list on each view to find these") + logger.info(f"{n_bad_gcp_annotations} annotations with large reprojection error. Press Q to jump to the worst. Here are the top 5:") + for ix, t in enumerate(reprojection_errors[:5]): + logger.info(f"#{ix+1}: GCP[{t[0]}] on image {t[1]}") + if __name__ == "__main__": log.setup() diff --git a/bin/opensfm_run_all b/bin/opensfm_run_all index a19a35a48..0b0cbced0 100755 --- a/bin/opensfm_run_all +++ b/bin/opensfm_run_all @@ -12,6 +12,6 @@ $PYTHON $DIR/opensfm detect_features $1 $PYTHON $DIR/opensfm match_features $1 $PYTHON $DIR/opensfm create_tracks $1 $PYTHON $DIR/opensfm reconstruct $1 -#$PYTHON $DIR/opensfm mesh $1 -#$PYTHON $DIR/opensfm undistort $1 -#$PYTHON $DIR/opensfm compute_depthmaps $1 +$PYTHON $DIR/opensfm mesh $1 +$PYTHON $DIR/opensfm undistort $1 +$PYTHON $DIR/opensfm compute_depthmaps $1 From 15f8614b5da777ac3748fef60ece3b071f129cfc Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 5 Oct 2020 14:03:47 +0200 Subject: [PATCH 08/64] feat: minor improvements to annotation UI: - 'Jump to GCP' (press a to jump to the current GCP in all views) - New crosshairs for the annotations: small dot for the center and a circle of radius 5px. - Friendly names for GCPs instead of random characters - Currently selected frame is highlighted in each view --- annotation_gui_gcp/GUI.py | 43 +++++++++++++++++++++++++++++++------- annotation_gui_gcp/apps.py | 27 ++++++++++++++++-------- 2 files changed, 53 insertions(+), 17 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 3a5b66179..594da1b4b 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -7,6 +7,7 @@ import subprocess import sys import numpy as np +import random from apps import distinct_colors, read_gcp_file, id_generator @@ -46,6 +47,7 @@ def __init__(self, master, database, n_views, sequence_groups=()): master.bind_all('q', lambda event: self.go_to_worst_gcp()) master.bind_all('z', lambda event: self.toggle_zoom_on_key()) master.bind_all('x', lambda event: self.toggle_sticky_zoom()) + master.bind_all('a', lambda event: self.go_to_current_gcp()) master.bind_all('w', lambda event: self.go_to_next_image_all()) master.bind_all('s', lambda event: self.go_to_prev_image_all()) self.create_ui(master, n_views=n_views) @@ -323,20 +325,21 @@ def display_points(self, view): for point_id, coords in visible_points_coords.items(): color = distinct_colors[divmod(hash(point_id), 19)[1]] - text_path = TextPath((0, 0), point_id, size=10) + text_path = TextPath((0, 0), point_id, size=12) p1 = PathPatch(text_path, transform=IdentityTransform(), alpha=1, color=color) offsetbox2 = AuxTransformBox(IdentityTransform()) offsetbox2.add_artist(p1) artists = [ AnnotationBbox(offsetbox2, ((coords[0] + 30), (coords[1] + 30)), bboxprops={"alpha":0.05}), - mpatches.Circle((coords[0], coords[1]), 10, color=color, fill=False), - mpatches.Circle((coords[0], coords[1]), 2, color=color, fill=False), + mpatches.Circle((coords[0], coords[1]), 5, color=color, fill=False), + mpatches.Circle((coords[0], coords[1]), 0.5, color=color, fill=True), ] if point_id == self.curr_point: artists.extend([ + mpatches.Circle((coords[0], coords[1]), 10, color=color, fill=False), + mpatches.Circle((coords[0], coords[1]), 11, color=color, fill=False), mpatches.Circle((coords[0], coords[1]), 12, color=color, fill=False), - mpatches.Circle((coords[0], coords[1]), 14, color=color, fill=False), ]) for art in artists: view.plt_artists.append(art) @@ -346,6 +349,8 @@ def display_points(self, view): def add_gcp(self): new_id = id_generator() + while self.database.point_exists(new_id): + new_id = id_generator() self.database.add_point(new_id) self.curr_point = new_id self.gcp_list_box.insert(tk.END, new_id) @@ -373,8 +378,10 @@ def zoom_in(self, view, x, y): return xlim = view.subplot.get_xlim() width = max(xlim) - min(xlim) - view.subplot.set_xlim(x - width / 20, x + width / 20) - view.subplot.set_ylim(y + width / 20, y - width / 20) + window_size = width / 10 + window_size = 100 + view.subplot.set_xlim(x - window_size/2, x + window_size/2) + view.subplot.set_ylim(y + window_size/2, y - window_size/2) view.zoomed_in = True def zoom_out(self, view): @@ -472,7 +479,8 @@ def remove_gcp(self): def onclick_gcp_list(self, event): widget = event.widget - value = widget.get(int(widget.curselection()[0])) + selected_row = int(widget.curselection()[0]) + value = widget.get(selected_row) if value == 'none': self.curr_point = None else: @@ -564,6 +572,17 @@ def highlight_gcp_reprojection(self, view, shot, point_id): view.canvas.draw_idle() self.zoom_in(view, x, y) + def go_to_current_gcp(self): + """ + Jumps to the currently selected GCP in all views where it was not visible + """ + shots_gcp_seen = set(self.database.gcp_reprojections[self.curr_point].keys()) + for view in self.views: + shots_gcp_seen_this_view = list(shots_gcp_seen.intersection(view.image_keys)) + if len(shots_gcp_seen_this_view) > 0 and view.current_image not in shots_gcp_seen: + target_shot = random.choice(shots_gcp_seen_this_view) + self.bring_new_image(target_shot, view) + def go_to_worst_gcp(self): if len(self.database.gcp_reprojections) == 0: print("No GCP reprojections available. Can't jump to worst GCP") @@ -577,7 +596,6 @@ def go_to_worst_gcp(self): self.gcp_list_box.selection_set(ix) break - # For each view for view in self.views: # Get the shot with worst reprojection error that in this view shot_worst_gcp = self.database.shot_with_max_gcp_error(view.image_keys, worst_gcp) @@ -626,5 +644,14 @@ def bring_new_image(self, new_image, view): self.zoom_logic(view) + # Update frame list so that this frame is highlighted + row_new_image = view.image_keys.index(new_image) + defaultbg = view.cget('bg') + for ix, _ in enumerate(view.image_keys): + if ix == row_new_image: + view.sequence_list_box.itemconfig(ix, bg='green') + else: + view.sequence_list_box.itemconfig(ix, bg=defaultbg) + self.display_points(view) print("Took {:.2f}s to bring_new_image {}".format(time.time()-t0, new_image)) diff --git a/annotation_gui_gcp/apps.py b/annotation_gui_gcp/apps.py index d05f557a8..16511f605 100644 --- a/annotation_gui_gcp/apps.py +++ b/annotation_gui_gcp/apps.py @@ -23,17 +23,26 @@ def set(self, obj): self.obj = obj '#3cb44b', '#ffe119', '#4363d8', '#f58231', '#911eb4', '#000075', '#808080', '#ffffff', '#000000'] - -class Imageset: - - def __init__(self): - self.prev = None - self.img = None - self.next = None - +words = None def id_generator(size=6, chars=string.ascii_uppercase + string.digits): - return ''.join(random.choice(chars) for _ in range(size)) + global words + if words is None: + try: + import urllib.request + word_url = "http://www.gutenberg.org/files/3201/files/NAMES.TXT" + response = urllib.request.urlopen(word_url) + long_txt = response.read().decode('unicode_escape') + words = long_txt.splitlines() + except Exception as e: + print(e) + words = [] + if len(words) > 0: + firstname = random.choice(words) + lastname = random.choice(words) + return '_'.join((firstname, lastname)).upper() + else: + return ''.join(random.choice(chars) for _ in range(size)) def normalize_img_coords(pixel_coords, img_size): From 5b5898597c2b315722c324c27f75b2c75e33957c Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 5 Oct 2020 15:38:31 +0200 Subject: [PATCH 09/64] feat: counter for the annotations of each GCP --- annotation_gui_gcp/GUI.py | 34 +++++++++++++++++++++------------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 594da1b4b..957aca5f0 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -149,6 +149,7 @@ def populate_sequence_list(self, view): image_keys = view.image_keys view.sequence_list_box.delete(0, tk.END) n_digits = len(str(len(image_keys))) + defaultbg = view.cget('bg') for ix, shot in enumerate(image_keys): points = self.database.get_visible_points_coords(shot) txt = "{:0{n_digits}} {}".format(ix+1, len(points), n_digits=n_digits) @@ -156,6 +157,9 @@ def populate_sequence_list(self, view): if shot_std: txt += " {:.2f}".format(shot_std) view.sequence_list_box.insert(tk.END, txt) + # Highlight current frame + if shot == view.current_image: + view.sequence_list_box.itemconfig(ix, bg='green') def onclick_sequence_list(self, event): widget = event.widget @@ -353,9 +357,7 @@ def add_gcp(self): new_id = id_generator() self.database.add_point(new_id) self.curr_point = new_id - self.gcp_list_box.insert(tk.END, new_id) - self.gcp_list_box.selection_clear(0, tk.END) - self.gcp_list_box.selection_set(tk.END) + self.populate_gcp_list() return new_id def which_view(self, event): @@ -446,20 +448,33 @@ def on_press(self, event): self.auto_gcp_create(view, x, y, event.button == 1) elif self.curr_point is not None: self.add_move_or_remove_gcp(view, x, y, event.button == 1) + self.populate_gcp_list() self.populate_sequence_list(view) self.display_points(view) else: return + def populate_gcp_list(self): self.gcp_list_box.delete(0, tk.END) errors = self.database.compute_gcp_errors()[-1] sorted_gcp_ids = sorted(errors, key=lambda k: -errors[k]) self.gcps = sorted_gcp_ids for point_id in self.gcps: - self.gcp_list_box.insert(tk.END, "{}".format(point_id)) + if point_id in self.database.points: + n_annotations = len(self.database.points[point_id]) + else: + n_annotations = 0 + self.gcp_list_box.insert(tk.END, f"{point_id} {n_annotations}") self.gcp_list_box.insert(tk.END, "none") self.gcp_list_box.selection_clear(0, tk.END) + + # Re-select the currently selected point + if self.curr_point: + for ix, gcp_id in enumerate(self.gcp_list_box.get(0, "end")): + if self.curr_point in gcp_id: + self.gcp_list_box.selection_set(ix) + break self.master.update_idletasks() @@ -484,7 +499,7 @@ def onclick_gcp_list(self, event): if value == 'none': self.curr_point = None else: - self.curr_point = value + self.curr_point = value.split(' ')[0] for view in self.views: self.display_points(view) @@ -644,14 +659,7 @@ def bring_new_image(self, new_image, view): self.zoom_logic(view) - # Update frame list so that this frame is highlighted - row_new_image = view.image_keys.index(new_image) - defaultbg = view.cget('bg') - for ix, _ in enumerate(view.image_keys): - if ix == row_new_image: - view.sequence_list_box.itemconfig(ix, bg='green') - else: - view.sequence_list_box.itemconfig(ix, bg=defaultbg) + self.populate_sequence_list(view) self.display_points(view) print("Took {:.2f}s to bring_new_image {}".format(time.time()-t0, new_image)) From 09150efeb708bc9e2a79d82e7f5c52270845e3f9 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 5 Oct 2020 17:17:44 +0200 Subject: [PATCH 10/64] fix: jump to gcp now works without 'Analyze' being pressed --- annotation_gui_gcp/GUI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 957aca5f0..cd4c7d183 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -591,7 +591,7 @@ def go_to_current_gcp(self): """ Jumps to the currently selected GCP in all views where it was not visible """ - shots_gcp_seen = set(self.database.gcp_reprojections[self.curr_point].keys()) + shots_gcp_seen = set(p['shot_id'] for p in self.database.points[self.curr_point]) for view in self.views: shots_gcp_seen_this_view = list(shots_gcp_seen.intersection(view.image_keys)) if len(shots_gcp_seen_this_view) > 0 and view.current_image not in shots_gcp_seen: From b12d0ea8aa2955d202b942d6bdc56f5b40339c81 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Tue, 6 Oct 2020 11:19:12 +0200 Subject: [PATCH 11/64] feat: resect images with enough GCP annotations --- annotation_gui_gcp/run_ba.py | 101 +++++++++++++++++++++++++++++++++-- 1 file changed, 96 insertions(+), 5 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 6eb1aaaaa..2555dc0ab 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -9,6 +9,8 @@ from opensfm import align from opensfm import dataset from opensfm import log +from opensfm import multiview +from opensfm import pygeometry from opensfm import transformations as tf from opensfm import types @@ -84,6 +86,8 @@ def reproject_gcps(gcps, reconstruction): point = np.nan output[gcp.id] = {} for observation in gcp.observations: + if observation.shot_id not in reconstruction.shots: + continue shot = reconstruction.shots[observation.shot_id] reproj = shot.project(point) error = np.linalg.norm(reproj - observation.projection) @@ -242,6 +246,89 @@ def decompose_covariance(covariance): return u, np.sqrt(s) +def resect_annotated_single_images(reconstruction, gcps, camera_models, data): + """Resect images that do not belong to reconstruction but have enough GCPs annotated. + + Returns: + A reconstruction with all the resected images. + """ + not_in_rec = set() + for gcp in gcps: + for obs in gcp.observations: + im = obs.shot_id + if im not in reconstruction.shots: + not_in_rec.add(im) + + resected = types.Reconstruction() + for im in not_in_rec: + exif = data.load_exif(im) + camera = camera_models[exif["camera"]] + shot = resect_image(im, camera, gcps, reconstruction, data) + if shot is not None: + resected.add_shot(shot) + + print(f"Resected: {len(resected.shots)} shots and {len(resected.cameras)} cameras") + return resected + + +def _gcp_image_observation(gcp, image): + for obs in gcp.observations: + if image == obs.shot_id: + return obs + return None + + +def resect_image(im, camera, gcps, reconstruction, data): + """Resect an image based only on GCPs annotations. + + Returns: + The resected shot. + """ + threshold = 0.01 + min_inliers = 3 + + bs, Xs = [], [] + for gcp in gcps: + obs = _gcp_image_observation(gcp, im) + if not obs: + continue + gcp_3d_coords = orec.triangulate_gcp(gcp, reconstruction.shots) + if gcp_3d_coords is None: + continue + + b = camera.pixel_bearing(obs.projection) + bs.append(b) + Xs.append(gcp_3d_coords) + bs = np.array(bs) + Xs = np.array(Xs) + + if len(bs) < min_inliers: + print(f"Not enough annotations to resect image {im}") + return None + + T = multiview.absolute_pose_ransac(bs, Xs, threshold, 1000, 0.999) + R = T[:, :3] + t = T[:, 3] + + reprojected_bs = R.T.dot((Xs - t).T).T + reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis] + + inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold + ninliers = int(sum(inliers)) + + logger.info(f"{im} resection inliers: {ninliers} / {len(bs)}") + + if ninliers >= min_inliers: + R = T[:, :3].T + t = -R.dot(T[:, 3]) + shot = reconstruction.create_shot(im, camera.id, pygeometry.Pose(R,t)) + shot.metadata = orec.get_image_metadata(data, im) + return shot + else: + print(f"Not enough inliers to resect image {im}") + return None + + def parse_args(): parser = argparse.ArgumentParser(description='Merge reconstructions and run BA with GCPs') parser.add_argument( @@ -285,12 +372,16 @@ def main(): reconstructions = all_reconstructions[:2] gcps = data.load_ground_control_points() - coords0 = triangulate_gcps(gcps, reconstructions[0]) - coords1 = triangulate_gcps(gcps, reconstructions[1]) - - s, A, b = find_alignment(coords1, coords0) - align.apply_similarity(reconstructions[1], s, A, b) + if False: + coords0 = triangulate_gcps(gcps, reconstructions[0]) + coords1 = triangulate_gcps(gcps, reconstructions[1]) + s, A, b = find_alignment(coords1, coords0) + align.apply_similarity(reconstructions[1], s, A, b) + else: + base = reconstructions[0] + resected = resect_annotated_single_images(base, gcps, camera_models, data) + reconstructions = [base, resected] data.save_reconstruction(reconstructions, 'reconstruction_gcp_rigid.json') merged = merge_reconstructions(reconstructions, tracks_manager) From 681136c1fede17abad9a9c0feda0b7f614d7fd8c Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 6 Oct 2020 14:00:50 +0200 Subject: [PATCH 12/64] chore: only run one-way BA for 3d-to-2d analysis. added buttons for both types of analysis. --- annotation_gui_gcp/Database.py | 5 ++++- annotation_gui_gcp/GUI.py | 37 +++++++++++++++++++++++----------- annotation_gui_gcp/run_ba.py | 21 ++++++++++++++----- 3 files changed, 45 insertions(+), 18 deletions(-) diff --git a/annotation_gui_gcp/Database.py b/annotation_gui_gcp/Database.py index 11cf903a8..2a6a11a23 100644 --- a/annotation_gui_gcp/Database.py +++ b/annotation_gui_gcp/Database.py @@ -149,7 +149,10 @@ def shot_with_max_gcp_error(self, image_keys, gcp): # Return they key with most reprojection error for this GCP annotated_images = set(self.gcp_reprojections[gcp]).intersection(set(image_keys)) errors = {k: self.gcp_reprojections[gcp][k]['error'] for k in annotated_images} - return max(errors, key=lambda k: errors[k]) + if len(errors) > 0: + return max(errors, key=lambda k: errors[k]) + else: + return None def remove_gcp(self, point_id): if self.point_exists(point_id): diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index cd4c7d183..6046c7b98 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -120,17 +120,23 @@ def create_tools(self, master): # self.check_button.pack(side='top') - load_button = tk.Button(master, text="Analyze", command=self.analyze) - load_button.pack(side="top") + txt = tk.Label(master, text="Analysis") + txt.pack(side="top") + analysis_frame = tk.Frame(master) + analysis_frame.pack(side="top") + button = tk.Button(analysis_frame, text="3d-to-3d", command=self.analyze_3d_to_3d) + button.pack(side="left") + button = tk.Button(analysis_frame, text="3d-to-2d", command=self.analyze_3d_to_2d) + button.pack(side="left") io_frame = tk.Frame(master) io_frame.pack(side="top") - load_button = tk.Button(io_frame, text="Load", command=self.load_gcps) - load_button.pack(side="left") - save_button = tk.Button(io_frame, text="Save", command=self.save_gcps) - save_button.pack(side="left") - save_button = tk.Button(io_frame, text="Save As", command=self.save_gcps_as) - save_button.pack(side="left") + button = tk.Button(io_frame, text="Load", command=self.load_gcps) + button.pack(side="left") + button = tk.Button(io_frame, text="Save", command=self.save_gcps) + button.pack(side="left") + button = tk.Button(io_frame, text="Save As", command=self.save_gcps_as) + button.pack(side="left") def clear_artists(self, view): for artist in view.plt_artists: @@ -274,7 +280,12 @@ def auto_gcp_create(self, view, x, y, add): self.populate_sequence_list(view) - def analyze(self): + def analyze_3d_to_3d(self): + self.analyze(mode="3d_to_3d") + def analyze_3d_to_2d(self): + self.analyze(mode="3d_to_2d") + + def analyze(self, mode): # Check that there is a recent ground_control_points.json file t = time.time() - os.path.getmtime(self.database.path + '/ground_control_points.json') if t > 30: @@ -285,7 +296,8 @@ def analyze(self): subprocess.run([ sys.executable, os.path.dirname(__file__) + '/run_ba.py', - self.database.path + self.database.path, + '--mode', mode, ] ) self.shot_std = {} @@ -614,8 +626,9 @@ def go_to_worst_gcp(self): for view in self.views: # Get the shot with worst reprojection error that in this view shot_worst_gcp = self.database.shot_with_max_gcp_error(view.image_keys, worst_gcp) - self.bring_new_image(shot_worst_gcp, view) - self.highlight_gcp_reprojection(view, shot_worst_gcp, worst_gcp) + if shot_worst_gcp: + self.bring_new_image(shot_worst_gcp, view) + self.highlight_gcp_reprojection(view, shot_worst_gcp, worst_gcp) def point_in_view(self, view, point): if point is None: diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 2555dc0ab..730481dee 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -335,6 +335,13 @@ def parse_args(): 'dataset', help='dataset to process', ) + parser.add_argument( + '--mode', + default="3d_to_3d", + help="If '3d_to_2d': the largest reconstruction is used as a fixed reference: " + "Images not belonging to it are resected into it using the ground control points. " + "If '3d_to_3d': The two largest reconstructions are aligned to each other using the ground control points." + ) parser.add_argument( '--std-threshold', default=0.3, @@ -345,7 +352,9 @@ def parse_args(): default=0.008, # a little bit over 5 pixels at VGA resolution help='threshold in normalized pixels above which we consider a GCP annotation to be wrong', ) - return parser.parse_args() + args = parser.parse_args() + assert args.mode in ("3d_to_3d", "3d_to_2d") + return args def main(): @@ -367,15 +376,14 @@ def main(): all_reconstructions = data.load_reconstruction() if len(all_reconstructions) > 2: - logger.warning(f"WARNING: There are more than two reconstructions in {path}") + logger.warning(f"There are more than two reconstructions in {path}") reconstructions = all_reconstructions[:2] gcps = data.load_ground_control_points() - if False: + if args.mode == "3d_to_3d": coords0 = triangulate_gcps(gcps, reconstructions[0]) coords1 = triangulate_gcps(gcps, reconstructions[1]) - s, A, b = find_alignment(coords1, coords0) align.apply_similarity(reconstructions[1], s, A, b) else: @@ -411,7 +419,10 @@ def main(): data.save_reconstruction(resplit, 'reconstruction_gcp_ba_resplit.json') all_shots_std = [] - for rec_ixs in ((0,1), (1,0)): + # We run bundle by fixing one reconstruction. + # If we have two reconstructions, we do this twice, fixing each one. + _rec_ixs = [(0,1), (1,0)] if args.mode == "3d_to_3d" else [(0,1)] + for rec_ixs in _rec_ixs: fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) bundle_with_fixed_images(merged, camera_models, gcp=gcps, gcp_std=gcp_std, fixed_images=fixed_images, config=data.config) From 67136204f9349f17b94a73988b50ee5e324d79a2 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 6 Oct 2020 19:46:51 +0200 Subject: [PATCH 13/64] misc fixes: - don't duplicate the resected shots - use the gcps also in the initial BA - ignore the GPS of the resected shots during BA --- annotation_gui_gcp/run_ba.py | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 730481dee..4aaef7533 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -263,9 +263,7 @@ def resect_annotated_single_images(reconstruction, gcps, camera_models, data): for im in not_in_rec: exif = data.load_exif(im) camera = camera_models[exif["camera"]] - shot = resect_image(im, camera, gcps, reconstruction, data) - if shot is not None: - resected.add_shot(shot) + resect_image(im, camera, gcps, reconstruction, data, resected) print(f"Resected: {len(resected.shots)} shots and {len(resected.cameras)} cameras") return resected @@ -278,8 +276,11 @@ def _gcp_image_observation(gcp, image): return None -def resect_image(im, camera, gcps, reconstruction, data): - """Resect an image based only on GCPs annotations. +def resect_image(im, camera, gcps, reconstruction, data, dst_reconstruction=None): + """ + Resect an image into a reconstruction based only on GCPs annotations. + Pass another reconstruction to dst_reconstruction + if you want the resected points to be added there instead Returns: The resected shot. @@ -318,10 +319,14 @@ def resect_image(im, camera, gcps, reconstruction, data): logger.info(f"{im} resection inliers: {ninliers} / {len(bs)}") + if dst_reconstruction is None: + dst_reconstruction = reconstruction + if ninliers >= min_inliers: R = T[:, :3].T t = -R.dot(T[:, 3]) - shot = reconstruction.create_shot(im, camera.id, pygeometry.Pose(R,t)) + dst_reconstruction.add_camera(camera) + shot = dst_reconstruction.create_shot(im, camera.id, pygeometry.Pose(R,t)) shot.metadata = orec.get_image_metadata(data, im) return shot else: @@ -349,7 +354,7 @@ def parse_args(): ) parser.add_argument( '--px-threshold', - default=0.008, # a little bit over 5 pixels at VGA resolution + default=0.016, # a little bit over 10 pixels at VGA resolution help='threshold in normalized pixels above which we consider a GCP annotation to be wrong', ) args = parser.parse_args() @@ -389,6 +394,9 @@ def main(): else: base = reconstructions[0] resected = resect_annotated_single_images(base, gcps, camera_models, data) + for shot in resected.shots.values(): + shot.metadata.gps_accuracy.value = 1E12 + shot.metadata.gps_position.value = shot.pose.get_origin() reconstructions = [base, resected] data.save_reconstruction(reconstructions, 'reconstruction_gcp_rigid.json') @@ -396,6 +404,7 @@ def main(): data.save_reconstruction([merged], 'reconstruction_merged.json') data.config['bundle_max_iterations'] = 200 + data.config['bundle_use_gcp'] = True orec.bundle(merged, camera_models, gcp=gcps, config=data.config) # rigid rotation to put images on the ground orec.align_reconstruction(merged, None, data.config) From 5323b74d744b01d4a062d110b2eec39bea758d4a Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 9 Oct 2020 11:04:32 +0200 Subject: [PATCH 14/64] fix: 'Auto GCP' works on all reconstructions Before, it only worked on the largest one. --- annotation_gui_gcp/geometry.py | 5 ++++- annotation_gui_gcp/run_ba.py | 6 +++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/annotation_gui_gcp/geometry.py b/annotation_gui_gcp/geometry.py index f3cd934b5..c5d2bfae9 100644 --- a/annotation_gui_gcp/geometry.py +++ b/annotation_gui_gcp/geometry.py @@ -38,7 +38,10 @@ def get_tracks_visible_in_image(gcp_database, image_key, min_len=5): print(f"Getting track observations visible in {image_key}") data = dataset.DataSet(gcp_database.path) tracks_manager = data.load_tracks_manager() - reconstruction = data.load_reconstruction()[0] + reconstructions = data.load_reconstruction() + for reconstruction in reconstructions: + if image_key in reconstruction.shots: + break out = {} for track_id in reconstruction.points: diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 4aaef7533..2d23d483c 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -485,17 +485,17 @@ def main(): if n_bad_std == 0 and n_bad_gcp_annotations == 0: logger.info( f"No issues. All gcp reprojections are under {args.px_threshold}" - "and all frames are localized within {args.std_threshold}m" + f" and all frames are localized within {args.std_threshold}m" ) else: if np.isnan(all_shots_std[0][1]): logger.info( - f"There is an issue while analyzing. It could be because: a) there are not enough GCPs." + f"Could not get positional uncertainty. It could be because: a) there are not enough GCPs." " b) they are badly distributed in 3D. c) there are some wrong annotations" ) else: logger.info(f"{n_bad_std} badly localized images (error>{args.std_threshold}). Use the frame list on each view to find these") - logger.info(f"{n_bad_gcp_annotations} annotations with large reprojection error. Press Q to jump to the worst. Here are the top 5:") + logger.info(f"{n_bad_gcp_annotations} annotations with large reprojection error. Press Q to jump to the worst, correct it and repeat. Here are the top 5:") for ix, t in enumerate(reprojection_errors[:5]): logger.info(f"#{ix+1}: GCP[{t[0]}] on image {t[1]}") From 5edd49b05cb909e38eae747ba0c39bd071f9ffc7 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 9 Oct 2020 15:13:52 +0200 Subject: [PATCH 15/64] fix: fix cameras during 3d-to-2d bundle adjustment --- annotation_gui_gcp/run_ba.py | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 2d23d483c..578dc9660 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -105,6 +105,14 @@ def get_all_reprojection_errors(gcp_reprojections): return sorted(output, key=lambda t:-t[2]) +def get_number_of_wrong_annotations_per_gcp(gcp_reprojections, wrong_threshold): + output = {} + for gcp_id, reprojections in gcp_reprojections.items(): + errors = [reprojections[shot_id]['error'] for shot_id in reprojections] + output[gcp_id] = sum(e>wrong_threshold for e in errors) + return output + + def compute_gcp_std(gcp_errors): """Compute the standard deviation of reprojection errors of all gcps.""" all_errors = [] @@ -163,7 +171,7 @@ def add_gcp_to_bundle(ba, gcp, gcp_std, shots): def bundle_with_fixed_images(reconstruction, camera_priors, gcp, gcp_std, fixed_images, config): """Bundle adjust a reconstruction while keeping some images fixed.""" - fix_cameras = False + fix_cameras = True chrono = orec.Chronometer() ba = orec.pybundle.BundleAdjuster() @@ -412,7 +420,6 @@ def main(): gcp_reprojections = reproject_gcps(gcps, merged) reprojection_errors = get_all_reprojection_errors(gcp_reprojections) - n_bad_gcp_annotations = sum(t[2] > args.px_threshold for t in reprojection_errors) err_values = [t[2] for t in reprojection_errors] mean_reprojection_error = np.mean(err_values) max_reprojection_error = np.max(err_values) @@ -462,6 +469,7 @@ def main(): f.write(line + '\n') + n_bad_gcp_annotations = int(sum(t[2] > args.px_threshold for t in reprojection_errors)) metrics = { 'n_reconstructions': len(all_reconstructions), 'median_shot_std' : median_shot_std, @@ -469,7 +477,7 @@ def main(): 'max_reprojection_error': max_reprojection_error, 'median_reprojection_error': median_reprojection_error, 'n_gcp': len(gcps), - 'n_bad_gcp_annotations': int(n_bad_gcp_annotations), + 'n_bad_gcp_annotations': n_bad_gcp_annotations, 'n_bad_position_std': int(n_bad_std), } @@ -496,8 +504,11 @@ def main(): else: logger.info(f"{n_bad_std} badly localized images (error>{args.std_threshold}). Use the frame list on each view to find these") logger.info(f"{n_bad_gcp_annotations} annotations with large reprojection error. Press Q to jump to the worst, correct it and repeat. Here are the top 5:") - for ix, t in enumerate(reprojection_errors[:5]): - logger.info(f"#{ix+1}: GCP[{t[0]}] on image {t[1]}") + + stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp(gcp_reprojections, args.px_threshold) + gcps_sorted = sorted(stats_bad_reprojections, key=lambda k:-stats_bad_reprojections[k])[:5] + for ix, gcp_id in enumerate(gcps_sorted): + logger.info(f"#{ix+1}: GCP: {gcp_id} with {stats_bad_reprojections[gcp_id]} bad annotations") if __name__ == "__main__": From ff66b1da4c204fa06f0d6f9f6db6a74f5d481acb Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 13 Oct 2020 13:10:28 +0200 Subject: [PATCH 16/64] feat: preload images with multiple processes --- annotation_gui_gcp/Database.py | 49 +++++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 16 deletions(-) diff --git a/annotation_gui_gcp/Database.py b/annotation_gui_gcp/Database.py index 2a6a11a23..7d4d6e11e 100644 --- a/annotation_gui_gcp/Database.py +++ b/annotation_gui_gcp/Database.py @@ -8,7 +8,22 @@ from collections import OrderedDict import json from matplotlib.image import _rgb_to_rgba +import multiprocessing +IMAGE_MAX_SIZE = 1000 + +def load_image(path): + print(f"Loading {path}") + rgb = Image.open(path) + + # Reduce to some reasonable maximum size + scale = max(rgb.size) / IMAGE_MAX_SIZE + if scale > 1: + new_w = int(round(rgb.size[0] / scale)) + new_h = int(round(rgb.size[1] / scale)) + rgb = rgb.resize((new_w, new_h), resample=Image.BILINEAR) + # Matplotlib will transform to rgba when plotting + return _rgb_to_rgba(np.asarray(rgb)) class Database: def __init__(self, seqs, path, preload_images=True): @@ -18,10 +33,7 @@ def __init__(self, seqs, path, preload_images=True): self.image_cache = {} if preload_images: - print("Preloading images") - for keys in self.seqs.values(): - for k in keys: - self.get_image(k) + self.preload_images() p_gcp_errors = self.path + '/gcp_reprojections.json' if os.path.exists(p_gcp_errors): @@ -55,21 +67,26 @@ def init_points(self, points): np.array([observation["projection"]]), w, h)[0] self.points[point_id] = observations - def get_image(self, img_name, max_size=1000): + def get_image(self, img_name): if img_name not in self.image_cache: - rgb = Image.open(self.go_to_image_path() + img_name) - - # Reduce to some reasonable maximum size - scale = max(rgb.size) / max_size - if scale > 1: - new_w = int(round(rgb.size[0] / scale)) - new_h = int(round(rgb.size[1] / scale)) - rgb = rgb.resize((new_w, new_h), resample=Image.BILINEAR) - - # Matplotlib will transform to rgba when plotting - self.image_cache[img_name] = _rgb_to_rgba(np.asarray(rgb)) + path = self.go_to_image_path() + img_name + self.image_cache[img_name] = self.load_image(path) return self.image_cache[img_name] + def preload_images(self): + n_cpu = multiprocessing.cpu_count() + print(f"Preloading images with {n_cpu} processes") + paths = [] + img_names = [] + for keys in self.seqs.values(): + for k in keys: + img_names.append(k) + paths.append(self.go_to_image_path() + k) + pool = multiprocessing.Pool(processes=n_cpu) + images = pool.map(load_image, paths) + for img_name, im in zip(img_names, images): + self.image_cache[img_name] = im + def get_image_size(self, img_name): img = self.get_image(img_name) return img.shape[:2] From b13db83a10b6d06a4a5022736f303a2ce87e5120 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 13 Oct 2020 14:25:39 +0200 Subject: [PATCH 17/64] feat: Improved text labels in annotation UI - faster draw times - better contrast - optionally disabled --- annotation_gui_gcp/GUI.py | 49 ++++++++++++++++++++++++++------------- 1 file changed, 33 insertions(+), 16 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 6046c7b98..af38d92cc 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -8,16 +8,19 @@ import sys import numpy as np import random +import colorsys from apps import distinct_colors, read_gcp_file, id_generator matplotlib.use('TkAgg') -from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk) +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg from matplotlib.figure import Figure from matplotlib.patches import PathPatch from matplotlib.text import TextPath from matplotlib.transforms import IdentityTransform from matplotlib.offsetbox import AnnotationBbox, AuxTransformBox +from matplotlib import patheffects + from matplotlib import pyplot as plt from geometry import get_all_track_observations, get_tracks_visible_in_image @@ -27,11 +30,12 @@ NEXT_UNICODE = u"\u2191" FONT = "TkFixedFont" - -class NavigationToolbar(NavigationToolbar2Tk): - def set_message(self, m): - pass - +def comp_color(color): + r,g,b = color[1:3], color[3:5], color[5:7] + r,g,b = [int(c, 16) for c in (r,g,b)] + h, s, v = colorsys.rgb_to_hsv(r,g,b) + comp_color = 'black' if v > 128 else 'white' + return comp_color class Gui: @@ -115,10 +119,12 @@ def create_tools(self, master): # var=self.if_show_epipolar) # self.check_button.pack(side='top') self.sticky_zoom = tk.BooleanVar(value=True) - sticky_zoom_button = tk.Checkbutton(master, text="Sticky zoom (x)", var=self.sticky_zoom) - sticky_zoom_button.pack(side='top') + button = tk.Checkbutton(master, text="Sticky zoom (x)", var=self.sticky_zoom) + button.pack(side='top') - # self.check_button.pack(side='top') + self.show_gcp_names = tk.BooleanVar(value=False) + button = tk.Checkbutton(master, text="Show GCP names", var=self.show_gcp_names) + button.pack(side='top') txt = tk.Label(master, text="Analysis") txt.pack(side="top") @@ -341,18 +347,30 @@ def display_points(self, view): for point_id, coords in visible_points_coords.items(): color = distinct_colors[divmod(hash(point_id), 19)[1]] - text_path = TextPath((0, 0), point_id, size=12) - p1 = PathPatch(text_path, transform=IdentityTransform(), alpha=1, color=color) - offsetbox2 = AuxTransformBox(IdentityTransform()) - offsetbox2.add_artist(p1) artists = [ - AnnotationBbox(offsetbox2, ((coords[0] + 30), (coords[1] + 30)), bboxprops={"alpha":0.05}), mpatches.Circle((coords[0], coords[1]), 5, color=color, fill=False), - mpatches.Circle((coords[0], coords[1]), 0.5, color=color, fill=True), ] + if self.show_gcp_names.get() or point_id == self.curr_point: + text = matplotlib.text.Annotation( + point_id, + coords, + xytext=[0, 7], + #fontsize=9, + textcoords='offset points', + ha='center', + va='bottom', + color=color, + ) + text.set_path_effects([ + patheffects.Stroke(linewidth=3, foreground=comp_color(color)), + patheffects.Normal(), + ]) + artists.append(text) + if point_id == self.curr_point: artists.extend([ + mpatches.Circle((coords[0], coords[1]), 0.5, color=color, fill=True), mpatches.Circle((coords[0], coords[1]), 10, color=color, fill=False), mpatches.Circle((coords[0], coords[1]), 11, color=color, fill=False), mpatches.Circle((coords[0], coords[1]), 12, color=color, fill=False), @@ -675,4 +693,3 @@ def bring_new_image(self, new_image, view): self.populate_sequence_list(view) self.display_points(view) - print("Took {:.2f}s to bring_new_image {}".format(time.time()-t0, new_image)) From 3c6841ca531d7ad084490cf013a53b24a115310a Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 13 Oct 2020 14:48:10 +0200 Subject: [PATCH 18/64] fix: no more 'jumpy' frame lists. The selection on the frame list always follows the currently selected frame --- annotation_gui_gcp/GUI.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index af38d92cc..0223e4824 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -1,6 +1,5 @@ import matplotlib.patches as mpatches import tkinter as tk -from tkinter import filedialog import time import matplotlib import os @@ -162,6 +161,7 @@ def populate_sequence_list(self, view): view.sequence_list_box.delete(0, tk.END) n_digits = len(str(len(image_keys))) defaultbg = view.cget('bg') + ix_selected = None for ix, shot in enumerate(image_keys): points = self.database.get_visible_points_coords(shot) txt = "{:0{n_digits}} {}".format(ix+1, len(points), n_digits=n_digits) @@ -172,6 +172,13 @@ def populate_sequence_list(self, view): # Highlight current frame if shot == view.current_image: view.sequence_list_box.itemconfig(ix, bg='green') + ix_selected = ix + + # Focus on the current frame in the list + if ix_selected: + #view.sequence_list_box.selection_set(sel) + view.sequence_list_box.see(ix_selected) + def onclick_sequence_list(self, event): widget = event.widget @@ -325,7 +332,7 @@ def load_shot_std(self, path): def load_gcps(self, filename=None): if filename is None: - filename = filedialog.askopenfilename( + filename = tk.filedialog.askopenfilename( title="Open GCP file", initialdir=self.database.path, filetypes=(("JSON files", "*.json"), ("all files", "*.*")), @@ -541,7 +548,7 @@ def save_gcps(self): return self.database.write_to_file(self.last_saved_filename) def save_gcps_as(self): - filename = filedialog.asksaveasfilename( + filename = tk.filedialog.asksaveasfilename( initialfile="ground_control_points.json", title="Save GCP file", initialdir=self.database.path, From 573c48c38aa043e13f81dbb783588e3110fde5d2 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 13 Oct 2020 17:34:18 +0200 Subject: [PATCH 19/64] feat: Always display reprojection for selected GCP --- annotation_gui_gcp/GUI.py | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 0223e4824..96f5e8471 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -176,7 +176,6 @@ def populate_sequence_list(self, view): # Focus on the current frame in the list if ix_selected: - #view.sequence_list_box.selection_set(sel) view.sequence_list_box.see(ix_selected) @@ -540,6 +539,8 @@ def onclick_gcp_list(self, event): for view in self.views: self.display_points(view) + if self.curr_point: + self.highlight_gcp_reprojection(view, self.curr_point, zoom=False) def save_gcps(self): if self.last_saved_filename is None: @@ -613,16 +614,23 @@ def go_to_next_image(self, view): def go_to_prev_image(self, view): self.go_to_adjacent_image(view, -1) - def highlight_gcp_reprojection(self, view, shot, point_id): - x, y = 0,0 + def highlight_gcp_reprojection(self, view, point_id, zoom=True): + if point_id not in self.database.gcp_reprojections: + return + shot = view.current_image + x, y = None, None for obs in self.database.points[point_id]: if obs['shot_id'] == shot: x, y = obs['projection'] + if x is None: + return x2, y2 = self.database.gcp_reprojections[point_id][shot]['reprojection'] - view.subplot.plot([x, x2], [y, y2], 'r-') + artists = view.subplot.plot([x, x2], [y, y2], 'r-') + view.plt_artists.extend(artists) + if zoom: + self.zoom_in(view, x, y) view.canvas.draw_idle() - self.zoom_in(view, x, y) def go_to_current_gcp(self): """ @@ -653,7 +661,6 @@ def go_to_worst_gcp(self): shot_worst_gcp = self.database.shot_with_max_gcp_error(view.image_keys, worst_gcp) if shot_worst_gcp: self.bring_new_image(shot_worst_gcp, view) - self.highlight_gcp_reprojection(view, shot_worst_gcp, worst_gcp) def point_in_view(self, view, point): if point is None: @@ -700,3 +707,6 @@ def bring_new_image(self, new_image, view): self.populate_sequence_list(view) self.display_points(view) + + if self.curr_point: + self.highlight_gcp_reprojection(view, self.curr_point, zoom=False) From 2b3a25de68fa17788076da6b8ccf747ae777f178 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 14 Oct 2020 11:01:51 +0200 Subject: [PATCH 20/64] feat: run_ba saves the number of localized images --- annotation_gui_gcp/run_ba.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 578dc9660..bd1300bc2 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -453,6 +453,7 @@ def main(): # If the STD of all shots is the same, replace by nan std_values = [x[1] for x in all_shots_std] n_bad_std = sum(std > args.std_threshold for std in std_values) + n_good_std = sum(std <= args.std_threshold for std in std_values) if np.allclose(std_values, std_values[0]): all_shots_std = [(x[0], np.nan) for x in all_shots_std] n_bad_std = len(std_values) @@ -479,6 +480,7 @@ def main(): 'n_gcp': len(gcps), 'n_bad_gcp_annotations': n_bad_gcp_annotations, 'n_bad_position_std': int(n_bad_std), + 'n_good_position_std': int(n_good_std), } logger.info(metrics) From f713d120a1a0c988762b4dfc1be174ada1a9091e Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 15 Oct 2020 13:19:23 +0200 Subject: [PATCH 21/64] chore: Refactor the annotation UI This refactor is to accommodate other types of views (aerial, orthophotos). The gcp database, image loading logic and view ui logic have been disentangled into different classes. The old view is now an ImageSequenceView subclassing from a more general View class. Auto linting and formatting applied to all involved files as well. --- annotation_gui_gcp/Database.py | 198 ------- annotation_gui_gcp/GUI.py | 670 +++++----------------- annotation_gui_gcp/apps.py | 138 ----- annotation_gui_gcp/gcp_manager.py | 159 +++++ annotation_gui_gcp/geometry.py | 35 +- annotation_gui_gcp/image_manager.py | 56 ++ annotation_gui_gcp/image_sequence_view.py | 116 ++++ annotation_gui_gcp/main.py | 33 +- annotation_gui_gcp/view.py | 325 +++++++++++ 9 files changed, 825 insertions(+), 905 deletions(-) delete mode 100644 annotation_gui_gcp/Database.py delete mode 100644 annotation_gui_gcp/apps.py create mode 100644 annotation_gui_gcp/gcp_manager.py create mode 100644 annotation_gui_gcp/image_manager.py create mode 100644 annotation_gui_gcp/image_sequence_view.py create mode 100644 annotation_gui_gcp/view.py diff --git a/annotation_gui_gcp/Database.py b/annotation_gui_gcp/Database.py deleted file mode 100644 index 7d4d6e11e..000000000 --- a/annotation_gui_gcp/Database.py +++ /dev/null @@ -1,198 +0,0 @@ - -from PIL import Image -import time - -from opensfm import features -import numpy as np -import os -from collections import OrderedDict -import json -from matplotlib.image import _rgb_to_rgba -import multiprocessing - -IMAGE_MAX_SIZE = 1000 - -def load_image(path): - print(f"Loading {path}") - rgb = Image.open(path) - - # Reduce to some reasonable maximum size - scale = max(rgb.size) / IMAGE_MAX_SIZE - if scale > 1: - new_w = int(round(rgb.size[0] / scale)) - new_h = int(round(rgb.size[1] / scale)) - rgb = rgb.resize((new_w, new_h), resample=Image.BILINEAR) - # Matplotlib will transform to rgba when plotting - return _rgb_to_rgba(np.asarray(rgb)) - -class Database: - def __init__(self, seqs, path, preload_images=True): - self.points = OrderedDict() - self.seqs = seqs - self.path = path - self.image_cache = {} - - if preload_images: - self.preload_images() - - p_gcp_errors = self.path + '/gcp_reprojections.json' - if os.path.exists(p_gcp_errors): - self.load_gcp_reprojections(p_gcp_errors) - else: - self.gcp_reprojections = {} - - def load_gcp_reprojections(self, filename): - with open(filename, 'r') as f: - d = json.load(f) - - for gcp_id in d: - for shot_id in d[gcp_id]: - h, w = self.get_image_size(shot_id) - reproj = d[gcp_id][shot_id]['reprojection'] - reproj = features.denormalized_image_coordinates(np.array([reproj]), w, h)[0] - d[gcp_id][shot_id]['reprojection'] = reproj - - self.gcp_reprojections = d - - def go_to_image_path(self): - img_folder = self.path + "/images/" - return img_folder - - def init_points(self, points): - for point in points: - point_id, observations = point['id'], point['observations'] - for observation in observations: - h, w = self.get_image_size(observation["shot_id"]) - observation["projection"] = features.denormalized_image_coordinates( - np.array([observation["projection"]]), w, h)[0] - self.points[point_id] = observations - - def get_image(self, img_name): - if img_name not in self.image_cache: - path = self.go_to_image_path() + img_name - self.image_cache[img_name] = self.load_image(path) - return self.image_cache[img_name] - - def preload_images(self): - n_cpu = multiprocessing.cpu_count() - print(f"Preloading images with {n_cpu} processes") - paths = [] - img_names = [] - for keys in self.seqs.values(): - for k in keys: - img_names.append(k) - paths.append(self.go_to_image_path() + k) - pool = multiprocessing.Pool(processes=n_cpu) - images = pool.map(load_image, paths) - for img_name, im in zip(img_names, images): - self.image_cache[img_name] = im - - def get_image_size(self, img_name): - img = self.get_image(img_name) - return img.shape[:2] - - def get_visible_points_coords(self, main_image): - visible_points_coords = OrderedDict() - for point_id, observations in self.points.items(): - for observation in observations: - if observation["shot_id"] == main_image: - visible_points_coords[point_id] = observation["projection"] - - return visible_points_coords - - def point_exists(self, point_id): - return point_id in self.points - - def point_sees(self, point, image): - for obs in self.points[point]: - if image == obs['shot_id']: - return True - return False - - def add_point(self, point_id): - if self.point_exists(point_id): - print('ERROR: trying to add an existing point') - return - self.points[point_id] = [] - - def add_point_observation(self, point_id, shot_id, projection): - if not self.point_exists(point_id): - print('ERROR: trying to modify a non-existing point') - return - self.points[point_id].append({ - "shot_id": shot_id, - "projection": projection, - }) - - def write_to_file(self, filename): - data = {"points": []} - for point_id, observations in self.points.items(): - point = {"id": point_id, "observations": []} - for observation in observations: - h, w = self.get_image_size(observation["shot_id"]) - scaled_projection = features.normalized_image_coordinates( - np.array([observation["projection"]]), w, h)[0].tolist() - point["observations"].append({ - "shot_id": observation["shot_id"], - "projection": scaled_projection, - }) - data["points"].append(point) - - with open(filename, 'wt') as fp: - json.dump(data, fp, indent=4, sort_keys=True) - - def compute_gcp_errors(self): - error_avg = {} - worst_gcp_error = 0 - worst_gcp = None - shot_worst_gcp = None - for gcp_id in self.points: - error_avg[gcp_id] = 0 - for gcp_id in self.gcp_reprojections: - if gcp_id not in self.points: - continue - for shot_id in self.gcp_reprojections[gcp_id]: - err = self.gcp_reprojections[gcp_id][shot_id]['error'] - error_avg[gcp_id] += err - if err > worst_gcp_error: - worst_gcp_error = err - shot_worst_gcp = shot_id - worst_gcp = gcp_id - error_avg[gcp_id] /= len(self.gcp_reprojections[gcp_id]) - - return worst_gcp, shot_worst_gcp, worst_gcp_error, error_avg - - def shot_with_max_gcp_error(self, image_keys, gcp): - # Return they key with most reprojection error for this GCP - annotated_images = set(self.gcp_reprojections[gcp]).intersection(set(image_keys)) - errors = {k: self.gcp_reprojections[gcp][k]['error'] for k in annotated_images} - if len(errors) > 0: - return max(errors, key=lambda k: errors[k]) - else: - return None - - def remove_gcp(self, point_id): - if self.point_exists(point_id): - del self.points[point_id] - - def remove_point_observation(self, point_id, shot_id): - if not self.point_exists(point_id): - print('ERROR: trying to modify a non-existing point') - return - self.points[point_id] = [obs for obs in self.points[point_id] if obs["shot_id"] != shot_id] - if point_id in self.gcp_reprojections: - if shot_id in self.gcp_reprojections[point_id]: - self.gcp_reprojections[point_id][shot_id]['error'] = 0 - - def bring_adjacent_image(self, image, skey, offset): - self.get_image_index(image, skey) - next_idx = self.get_image_index + offset - return self.bring_image(skey, next_idx) - - def get_image_index(self, image, skey): - seq = self.seqs[skey] - return seq.index(image) - - def bring_image(self, skey, idx): - seq = self.seqs[skey] - return seq[max(0, min(idx, len(seq) - 1))] diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 96f5e8471..dacf16cec 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -1,85 +1,56 @@ -import matplotlib.patches as mpatches -import tkinter as tk -import time -import matplotlib import os +import random import subprocess import sys -import numpy as np -import random -import colorsys - -from apps import distinct_colors, read_gcp_file, id_generator +import time +import tkinter as tk -matplotlib.use('TkAgg') -from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg -from matplotlib.figure import Figure -from matplotlib.patches import PathPatch -from matplotlib.text import TextPath -from matplotlib.transforms import IdentityTransform -from matplotlib.offsetbox import AnnotationBbox, AuxTransformBox -from matplotlib import patheffects +import matplotlib -from matplotlib import pyplot as plt -from geometry import get_all_track_observations, get_tracks_visible_in_image +matplotlib.use("TkAgg") -from apps import CustomListbox +from image_sequence_view import ImageSequenceView -PREVIOUS_UNICODE = u"\u2193" -NEXT_UNICODE = u"\u2191" FONT = "TkFixedFont" -def comp_color(color): - r,g,b = color[1:3], color[3:5], color[5:7] - r,g,b = [int(c, 16) for c in (r,g,b)] - h, s, v = colorsys.rgb_to_hsv(r,g,b) - comp_color = 'black' if v > 128 else 'white' - return comp_color class Gui: - - def __init__(self, master, database, n_views, sequence_groups=()): - self.database = database - self.point_idx = 0 - self.match_idx = 0 + def __init__(self, master, gcp_manager, image_manager, sequence_groups=()): + self.master = master + self.gcp_manager = gcp_manager + self.image_manager = image_manager self.curr_point = None - self.last_saved_filename = None + self.quick_save_filename = None self.shot_std = {} self.sequence_groups = sequence_groups - - master.bind_all('q', lambda event: self.go_to_worst_gcp()) - master.bind_all('z', lambda event: self.toggle_zoom_on_key()) - master.bind_all('x', lambda event: self.toggle_sticky_zoom()) - master.bind_all('a', lambda event: self.go_to_current_gcp()) - master.bind_all('w', lambda event: self.go_to_next_image_all()) - master.bind_all('s', lambda event: self.go_to_prev_image_all()) - self.create_ui(master, n_views=n_views) + self.path = self.gcp_manager.path + + master.bind_all("q", lambda event: self.go_to_worst_gcp()) + master.bind_all("z", lambda event: self.toggle_zoom_all_views()) + master.bind_all("x", lambda event: self.toggle_sticky_zoom()) + master.bind_all("a", lambda event: self.go_to_current_gcp()) + master.bind_all("w", lambda event: self.go_to_next_image_all()) + master.bind_all("s", lambda event: self.go_to_prev_image_all()) + self.create_ui() master.lift() - p_default_gcp = self.database.path + '/ground_control_points.json' + p_default_gcp = self.path + "/ground_control_points.json" if os.path.exists(p_default_gcp): self.load_gcps(p_default_gcp) - p_shot_std = self.database.path + '/shots_std.csv' + p_shot_std = self.path + "/shots_std.csv" if os.path.exists(p_shot_std): self.load_shot_std(p_shot_std) for view in self.views: - self.populate_sequence_list(view) + view.populate_image_list() + view.bring_new_image(view.images_in_list[0]) - def create_ui(self, master, n_views): - self.master = master - - tools_frame = tk.Frame(master) - tools_frame.pack(side='left', expand=0, fill=tk.Y) + def create_ui(self): + tools_frame = tk.Frame(self.master) + tools_frame.pack(side="left", expand=0, fill=tk.Y) self.create_tools(tools_frame) - - self.views = [] - for view_ix in range(n_views): - new_view = tk.Toplevel(self.master, name=f"view_ix_{view_ix}") - new_view.title(f"View {view_ix+1}") - self.views.append(new_view) - - self.init_image_views() + self.create_image_views() + self.master.update_idletasks() self.arrange_ui_onerow() def arrange_ui_onerow(self): @@ -96,42 +67,52 @@ def arrange_ui_onerow(self): w = screen_width / len(self.views) h = screen_height - y for view in self.views: - view.geometry("%dx%d+%d+%d" % (w, h, x, y)) + view.window.geometry("%dx%d+%d+%d" % (w, h, x, y)) x += w def create_tools(self, master): gcp_list_frame = tk.Frame(master) - gcp_list_frame.pack(side='top', fill=tk.BOTH, expand=1) - self.gcp_list_box = CustomListbox(gcp_list_frame, font=FONT) - self.gcp_list_box.pack(side='left', expand=True, fill=tk.Y) - self.gcp_list_box.bind('<>', self.onclick_gcp_list) + gcp_list_frame.pack(side="top", fill=tk.BOTH, expand=1) + + self.gcp_list = tk.StringVar() + self.gcp_list_box = tk.Listbox( + gcp_list_frame, + font=FONT, + width=12, + selectmode="browse", + listvariable=self.gcp_list, + ) + self.gcp_list_box.pack(side="left", expand=True, fill=tk.BOTH) + self.gcp_list_box.bind("<>", self.onclick_gcp_list) plus_minus_frame = tk.Frame(master) - plus_minus_frame.pack(side='top') + plus_minus_frame.pack(side="top") add_button = tk.Button(plus_minus_frame, text="Add GCP", command=self.add_gcp) - add_button.pack(side='left') - remove_button = tk.Button(plus_minus_frame, text="Remove GCP", command=self.remove_gcp) - remove_button.pack(side='left') - - # self.if_show_epipolar = tk.IntVar(value=0) - # self.check_button = tk.Checkbutton(master, text="Epipolar lines", - # var=self.if_show_epipolar) - # self.check_button.pack(side='top') + add_button.pack(side="left") + remove_button = tk.Button( + plus_minus_frame, text="Remove GCP", command=self.remove_gcp + ) + remove_button.pack(side="left") + self.sticky_zoom = tk.BooleanVar(value=True) button = tk.Checkbutton(master, text="Sticky zoom (x)", var=self.sticky_zoom) - button.pack(side='top') + button.pack(side="top") self.show_gcp_names = tk.BooleanVar(value=False) button = tk.Checkbutton(master, text="Show GCP names", var=self.show_gcp_names) - button.pack(side='top') + button.pack(side="top") txt = tk.Label(master, text="Analysis") txt.pack(side="top") analysis_frame = tk.Frame(master) analysis_frame.pack(side="top") - button = tk.Button(analysis_frame, text="3d-to-3d", command=self.analyze_3d_to_3d) + button = tk.Button( + analysis_frame, text="3d-to-3d", command=self.analyze_3d_to_3d + ) button.pack(side="left") - button = tk.Button(analysis_frame, text="3d-to-2d", command=self.analyze_3d_to_2d) + button = tk.Button( + analysis_frame, text="3d-to-2d", command=self.analyze_3d_to_2d + ) button.pack(side="left") io_frame = tk.Frame(master) @@ -143,288 +124,72 @@ def create_tools(self, master): button = tk.Button(io_frame, text="Save As", command=self.save_gcps_as) button.pack(side="left") - def clear_artists(self, view): - for artist in view.plt_artists: - artist.set_visible(False) - del artist - - def set_title(self, view): - shot = view.current_image - seq_key = view.sequence_key - seq = self.database.seqs[seq_key] - seq_ix = seq.index(shot) - title = "{} [{}/{}]: {}".format(seq_key, seq_ix+1, len(seq), shot) - view.title(title) - - def populate_sequence_list(self, view): - image_keys = view.image_keys - view.sequence_list_box.delete(0, tk.END) - n_digits = len(str(len(image_keys))) - defaultbg = view.cget('bg') - ix_selected = None - for ix, shot in enumerate(image_keys): - points = self.database.get_visible_points_coords(shot) - txt = "{:0{n_digits}} {}".format(ix+1, len(points), n_digits=n_digits) - shot_std = self.shot_std.get(shot, None) - if shot_std: - txt += " {:.2f}".format(shot_std) - view.sequence_list_box.insert(tk.END, txt) - # Highlight current frame - if shot == view.current_image: - view.sequence_list_box.itemconfig(ix, bg='green') - ix_selected = ix - - # Focus on the current frame in the list - if ix_selected: - view.sequence_list_box.see(ix_selected) - - - def onclick_sequence_list(self, event): - widget = event.widget - sel = widget.curselection() - if not sel: - return - view = widget.master.master.master - self.go_to_image_index(view, int(sel[0])) - - def init_image_views(self, nav_buttons=False): - for idx, view in enumerate(self.views): - canvas_frame = tk.Frame(view) - canvas_frame.pack(side='left', fill=tk.BOTH, expand=1) - view.canvas_frame = canvas_frame - - if nav_buttons: - button_frame = tk.Frame(canvas_frame) - button_frame.pack(side='top') - prv_btn = tk.Button(button_frame, text=PREVIOUS_UNICODE, - command=lambda _idx=idx: self.go_to_prev_image(self.views[_idx])) - prv_btn.pack(side='left') - nxt_btn = tk.Button(button_frame, text=NEXT_UNICODE, - command=lambda _idx=idx: self.go_to_next_image(self.views[_idx])) - nxt_btn.pack(side='left') - - view.image_keys = list(self.database.seqs.values())[idx] - view.sequence_key = list(self.database.seqs.keys())[idx] - view.current_image = view.image_keys[0] - - toolbox = tk.Frame(canvas_frame) - toolbox.pack(side='left', expand=False, fill=tk.Y) - auto_gcp_button = tk.Button(toolbox, text="Auto GCP", command=lambda view=view: self.auto_gcp_show_tracks(view)) - auto_gcp_button.pack(side="top") - view.visible_tracks = None - view.sequence_list_box = CustomListbox(toolbox, font=FONT, width=12) - view.sequence_list_box.pack(side='top', expand=True, fill=tk.Y) - view.sequence_list_box.bind('<>', self.onclick_sequence_list) - - - view.figure = Figure() - view.subplot = view.figure.add_subplot(111) - view.subplot.imshow(self.database.get_image(view.current_image), aspect='auto') - view.subplot.axis('scaled') - view.figure.set_tight_layout(True) - view.subplot.axis('off') - - view.canvas = FigureCanvasTkAgg(view.figure, canvas_frame) - view.canvas.draw() - view.canvas.get_tk_widget().pack(side='top', fill=tk.BOTH, expand=1) - view.canvas.mpl_connect('button_press_event', - lambda event: - self.on_press(event)) - view.canvas.mpl_connect('scroll_event', - lambda event: - self.on_scroll(event)) - - view.zoomed_in = False - view.last_seen_px = {} - view.plt_artists = [] - self.set_title(view) - - - def auto_gcp_show_tracks(self, view): - h, w = self.database.get_image_size(view.current_image) - tracks = get_tracks_visible_in_image(self.database, view.current_image) - - # Select some tracks to plot, not all - # nice_tracks = sorted(tracks, key=lambda tid: -tracks[tid]['length']) - nice_tracks = tracks.keys() # all tracks - tracks = {k:tracks[k] for k in nice_tracks} - - if len(tracks) == 0: - print("No valid tracks found") - return - - # Draw track projections - points = [] - track_lengths = [] - for t in tracks.values(): - points.append(t[0]) - track_lengths.append(t[1]) - - points = np.array(points) - norm = matplotlib.colors.Normalize() - colors = plt.cm.viridis(norm(track_lengths)) - view.tracks_scatter = view.subplot.scatter(points[:,0], points[:,1], s=10, c=colors, marker='x') - view.canvas.draw_idle() - - # The next left or right click will be handled differently by setting this: - view.visible_tracks = tracks - - - def auto_gcp_create(self, view, x, y, add): - # Find track closest to click location and create a GCP from it - if add: - points, track_ids = [], [] - for tid, t in view.visible_tracks.items(): - points.append(t[0]) - track_ids.append(tid) - dists = np.linalg.norm(np.array(points) - np.array([[x,y]]), axis=1) - closest_track = track_ids[np.argmin(dists)] - observations = get_all_track_observations(self.database, closest_track) - - new_gcp = self.add_gcp() - print(f"Created new GCP {new_gcp} from track {closest_track} with {len(observations)} observations") - for shot_id, point in observations.items(): - self.database.add_point_observation(new_gcp, shot_id, point) - - view.visible_tracks = None - view.tracks_scatter.remove() - view.canvas.draw() - self.populate_sequence_list(view) - + def create_image_views(self): + self.views = [] + for sequence_key, image_keys in self.image_manager.seqs.items(): + v = ImageSequenceView( + self, f"view {len(self.views)+1}", sequence_key, image_keys + ) + self.views.append(v) def analyze_3d_to_3d(self): self.analyze(mode="3d_to_3d") + def analyze_3d_to_2d(self): self.analyze(mode="3d_to_2d") def analyze(self, mode): # Check that there is a recent ground_control_points.json file - t = time.time() - os.path.getmtime(self.database.path + '/ground_control_points.json') + t = time.time() - os.path.getmtime(self.path + "/ground_control_points.json") if t > 30: print("Please save before running the analysis") return # Call the run_ba script - subprocess.run([ - sys.executable, - os.path.dirname(__file__) + '/run_ba.py', - self.database.path, - '--mode', mode, + subprocess.run( + [ + sys.executable, + os.path.dirname(__file__) + "/run_ba.py", + self.path, + "--mode", + mode, ] ) self.shot_std = {} - self.load_shot_std(self.database.path + '/shots_std.csv') - p_gcp_errors = self.database.path + '/gcp_reprojections.json' - self.database.load_gcp_reprojections(p_gcp_errors) + self.load_shot_std(self.path + "/shots_std.csv") + p_gcp_errors = self.path + "/gcp_reprojections.json" + self.gcp_manager.load_gcp_reprojections(p_gcp_errors) for view in self.views: - self.populate_sequence_list(view) + view.populate_image_list() print("Done analyzing") - def load_shot_std(self, path): - with open(path, 'r') as f: + with open(path, "r") as f: for line in f: - shot, std = line[:-1].split(',') + shot, std = line[:-1].split(",") self.shot_std[shot] = float(std) def load_gcps(self, filename=None): if filename is None: filename = tk.filedialog.askopenfilename( title="Open GCP file", - initialdir=self.database.path, + initialdir=self.path, filetypes=(("JSON files", "*.json"), ("all files", "*.*")), ) if filename is None: return - points = read_gcp_file(filename) - self.last_saved_filename = filename - self.database.init_points(points) + self.quick_save_filename = filename + self.gcp_manager.load_from_file(filename) for view in self.views: - self.display_points(view) - # if self.if_show_epipolar.get(): - # self.show_epipolar_lines(view_ix) + view.display_points() self.populate_gcp_list() - def display_points(self, view): - visible_points_coords = self.database.get_visible_points_coords(view.current_image) - self.clear_artists(view) - - for point_id, coords in visible_points_coords.items(): - color = distinct_colors[divmod(hash(point_id), 19)[1]] - artists = [ - mpatches.Circle((coords[0], coords[1]), 5, color=color, fill=False), - ] - - if self.show_gcp_names.get() or point_id == self.curr_point: - text = matplotlib.text.Annotation( - point_id, - coords, - xytext=[0, 7], - #fontsize=9, - textcoords='offset points', - ha='center', - va='bottom', - color=color, - ) - text.set_path_effects([ - patheffects.Stroke(linewidth=3, foreground=comp_color(color)), - patheffects.Normal(), - ]) - artists.append(text) - - if point_id == self.curr_point: - artists.extend([ - mpatches.Circle((coords[0], coords[1]), 0.5, color=color, fill=True), - mpatches.Circle((coords[0], coords[1]), 10, color=color, fill=False), - mpatches.Circle((coords[0], coords[1]), 11, color=color, fill=False), - mpatches.Circle((coords[0], coords[1]), 12, color=color, fill=False), - ]) - for art in artists: - view.plt_artists.append(art) - view.subplot.add_artist(art) - - view.figure.canvas.draw() - def add_gcp(self): - new_id = id_generator() - while self.database.point_exists(new_id): - new_id = id_generator() - self.database.add_point(new_id) - self.curr_point = new_id + self.curr_point = self.gcp_manager.add_point() self.populate_gcp_list() - return new_id - - def which_view(self, event): - for view in self.views: - if event.canvas == view.canvas: - return view - return None - - def on_scroll(self, event): - view = self.which_view(event) - if event.xdata is None or event.ydata is None: - return - if event.button == 'up': - self.go_to_next_image(view) - elif event.button == 'down': - self.go_to_prev_image(view) - - def zoom_in(self, view, x, y): - if view.zoomed_in: - return - xlim = view.subplot.get_xlim() - width = max(xlim) - min(xlim) - window_size = width / 10 - window_size = 100 - view.subplot.set_xlim(x - window_size/2, x + window_size/2) - view.subplot.set_ylim(y + window_size/2, y - window_size/2) - view.zoomed_in = True - - def zoom_out(self, view): - view.subplot.autoscale() - view.zoomed_in = False + return self.curr_point def toggle_sticky_zoom(self): if self.sticky_zoom.get(): @@ -432,87 +197,45 @@ def toggle_sticky_zoom(self): else: self.sticky_zoom.set(True) - def toggle_zoom_on_key(self): + def toggle_zoom_all_views(self): # Zoom in/out on every view, centered on the location of the current GCP if self.curr_point is None: return any_zoomed_in = any(view.zoomed_in for view in self.views) for view in self.views: if any_zoomed_in: - self.zoom_out(view) + view.zoom_out() else: - for projection in self.database.points[self.curr_point]: + for projection in self.gcp_manager.points[self.curr_point]: if projection["shot_id"] == view.current_image: x, y = projection["projection"] - self.zoom_in(view, x, y) + view.zoom_in(x, y) break view.canvas.draw_idle() - - def add_move_or_remove_gcp(self, view, x, y, leftclick): - current_image = view.current_image - if self.curr_point is None: - return - self.database.remove_point_observation(self.curr_point, current_image) - - for line in view.subplot.lines: - line.remove() - - if leftclick: # Left click - self.database.add_point_observation(self.curr_point, current_image, (x, y)) - self.zoom_in(view, x, y) - - # if self.if_show_epipolar.get(): - # self.show_epipolar_lines(idx) - - - def on_press(self, event): - view = self.which_view(event) - x, y = event.xdata, event.ydata - if None in (x, y): - return - if event.button == 2: # Middle / wheel click: - if view.zoomed_in: - self.zoom_out(view) - else: - self.zoom_in(view, x, y) - view.figure.canvas.draw_idle() - elif event.button in (1, 3): - # Left click or right click - view.last_seen_px[self.curr_point] = x, y - if view.visible_tracks: - self.auto_gcp_create(view, x, y, event.button == 1) - elif self.curr_point is not None: - self.add_move_or_remove_gcp(view, x, y, event.button == 1) - self.populate_gcp_list() - self.populate_sequence_list(view) - self.display_points(view) - else: - return - - def populate_gcp_list(self): - self.gcp_list_box.delete(0, tk.END) - errors = self.database.compute_gcp_errors()[-1] - sorted_gcp_ids = sorted(errors, key=lambda k: -errors[k]) - self.gcps = sorted_gcp_ids - for point_id in self.gcps: - if point_id in self.database.points: - n_annotations = len(self.database.points[point_id]) - else: - n_annotations = 0 - self.gcp_list_box.insert(tk.END, f"{point_id} {n_annotations}") - self.gcp_list_box.insert(tk.END, "none") - self.gcp_list_box.selection_clear(0, tk.END) - - # Re-select the currently selected point - if self.curr_point: - for ix, gcp_id in enumerate(self.gcp_list_box.get(0, "end")): - if self.curr_point in gcp_id: - self.gcp_list_box.selection_set(ix) - break + self.update_gcp_list_text() + self.update_gcp_list_highlight() self.master.update_idletasks() + def update_gcp_list_text(self): + if not self.gcp_manager.points: + return + items = [] + n_digits = max(len(str(len(v))) for v in self.gcp_manager.points.values()) + for point_id in sorted(self.gcp_manager.points): + n_annotations = len(self.gcp_manager.points[point_id]) + txt = "{:0{n}} {}".format(n_annotations, point_id, n=n_digits) + items.append(txt) + self.gcp_list.set(items) + + def update_gcp_list_highlight(self): + if not self.curr_point: + return + defaultbg = self.master.cget("bg") + for ix, point_id in enumerate(sorted(self.gcp_manager.points)): + bg = "green" if self.curr_point == point_id else defaultbg + self.gcp_list_box.itemconfig(ix, bg=bg) def remove_gcp(self): to_be_removed_point = self.curr_point @@ -520,134 +243,84 @@ def remove_gcp(self): return self.curr_point = None - self.database.remove_gcp(to_be_removed_point) + self.gcp_manager.remove_gcp(to_be_removed_point) for view in self.views: - self.display_points(view) - # if self.if_show_epipolar.get(): - # self.show_epipolar_lines(image_idx) + view.display_points() self.populate_gcp_list() def onclick_gcp_list(self, event): widget = event.widget - selected_row = int(widget.curselection()[0]) - value = widget.get(selected_row) - if value == 'none': + selection = widget.curselection() + if not selection: + return + value = widget.get(int(selection[0])) + if value == "none": self.curr_point = None else: - self.curr_point = value.split(' ')[0] + self.curr_point = value.split(" ")[1] for view in self.views: - self.display_points(view) + view.display_points() if self.curr_point: - self.highlight_gcp_reprojection(view, self.curr_point, zoom=False) + view.highlight_gcp_reprojection(self.curr_point, zoom=False) + + self.update_gcp_list_highlight() def save_gcps(self): - if self.last_saved_filename is None: + if self.quick_save_filename is None: return self.save_gcps_as() else: - return self.database.write_to_file(self.last_saved_filename) + return self.gcp_manager.write_to_file(self.quick_save_filename) def save_gcps_as(self): filename = tk.filedialog.asksaveasfilename( initialfile="ground_control_points.json", title="Save GCP file", - initialdir=self.database.path, + initialdir=self.path, defaultextension=".json", ) if filename is None: return else: - self.last_saved_filename = filename + self.quick_save_filename = filename return self.save_gcps() - # def show_epipolar_lines(self, main_image_idx): - # if len(self.views) > 2: - # raise NotImplementedError("Not implemented yet for >2 views") - # img1 = self.views[0].current_image - # img2 = self.views[1].current_image - # img1_size = self.database.get_image_size(img1) - # img2_size = self.database.get_image_size(img2) - # matched_points = self.database.get_visible_points_coords(self.views[main_image_idx].current_image) - # matched_points_coords = convert_tuple_cords_to_list(matched_points) - # matched_points_coords = features.normalized_image_coordinates(matched_points_coords, img1_size[1], img1_size[0]) - # color_idx = 0 - # for point_idx, point in enumerate(matched_points_coords): - # image_pair = [img1, img2] - # line = calc_epipol_line(point, image_pair, self.database.path, main_image_idx) - # denormalized_lines = features.denormalized_image_coordinates(line, img2_size[1], img2_size[0]) - # for line_segment in denormalized_lines: - # circle = mpatches.Circle((line_segment[0], line_segment[1]), 3, - # color=distinct_colors[divmod(hash(list(matched_points.keys())[point_idx]), 19)[1]]) - # self.views[main_image_idx].plt_artists.append(circle) - # self.views[not main_image_idx].subplot.add_artist(circle) - # color_idx = color_idx + 1 - # self.views[not main_image_idx].figure.canvas.draw_idle() - - def go_to_image_index(self, view, idx, linked=True): - views_to_update = {view} - if linked: - groups_this_view = [g for g in self.sequence_groups if view.sequence_key in g] - for g in groups_this_view: - for v in self.views: - if v.sequence_key in g: - views_to_update.add(v) - for v in views_to_update: - new_image = self.database.bring_image(v.sequence_key, idx) - self.bring_new_image(new_image, v) - - def go_to_adjacent_image(self, view, offset, linked=True): - target_ix = self.database.get_image_index(view.current_image, view.sequence_key) + offset - self.go_to_image_index(view, target_ix, linked) - def go_to_next_image_all(self): for view in self.views: - self.go_to_adjacent_image(view, +1, linked=False) + view.go_to_adjacent_image(+1, linked=False) def go_to_prev_image_all(self): for view in self.views: - self.go_to_adjacent_image(view, -1, linked=False) - - def go_to_next_image(self, view): - self.go_to_adjacent_image(view, +1) - - def go_to_prev_image(self, view): - self.go_to_adjacent_image(view, -1) - - def highlight_gcp_reprojection(self, view, point_id, zoom=True): - if point_id not in self.database.gcp_reprojections: - return - shot = view.current_image - x, y = None, None - for obs in self.database.points[point_id]: - if obs['shot_id'] == shot: - x, y = obs['projection'] - if x is None: - return - - x2, y2 = self.database.gcp_reprojections[point_id][shot]['reprojection'] - artists = view.subplot.plot([x, x2], [y, y2], 'r-') - view.plt_artists.extend(artists) - if zoom: - self.zoom_in(view, x, y) - view.canvas.draw_idle() + view.go_to_adjacent_image(-1, linked=False) def go_to_current_gcp(self): """ Jumps to the currently selected GCP in all views where it was not visible """ - shots_gcp_seen = set(p['shot_id'] for p in self.database.points[self.curr_point]) + if not self.curr_point: + return + shots_gcp_seen = { + p["shot_id"] for p in self.gcp_manager.points[self.curr_point] + } for view in self.views: - shots_gcp_seen_this_view = list(shots_gcp_seen.intersection(view.image_keys)) - if len(shots_gcp_seen_this_view) > 0 and view.current_image not in shots_gcp_seen: + shots_gcp_seen_this_view = list( + shots_gcp_seen.intersection(view.image_keys) + ) + if ( + len(shots_gcp_seen_this_view) > 0 + and view.current_image not in shots_gcp_seen + ): target_shot = random.choice(shots_gcp_seen_this_view) - self.bring_new_image(target_shot, view) + view.bring_new_image(target_shot) def go_to_worst_gcp(self): - if len(self.database.gcp_reprojections) == 0: + if len(self.gcp_manager.gcp_reprojections) == 0: print("No GCP reprojections available. Can't jump to worst GCP") return - worst_gcp = self.database.compute_gcp_errors()[0] + worst_gcp = self.gcp_manager.compute_gcp_errors()[0] + if worst_gcp is None: + return self.curr_point = worst_gcp self.gcp_list_box.selection_clear(0, "end") @@ -658,55 +331,8 @@ def go_to_worst_gcp(self): for view in self.views: # Get the shot with worst reprojection error that in this view - shot_worst_gcp = self.database.shot_with_max_gcp_error(view.image_keys, worst_gcp) + shot_worst_gcp = self.gcp_manager.shot_with_max_gcp_error( + view.image_keys, worst_gcp + ) if shot_worst_gcp: - self.bring_new_image(shot_worst_gcp, view) - - def point_in_view(self, view, point): - if point is None: - return None - for projection in self.database.points[point]: - if projection["shot_id"] == view.current_image: - return projection["projection"] - return None - - def zoom_logic(self, view): - gcp_visible = self.point_in_view(view, self.curr_point) - if self.sticky_zoom.get() and self.curr_point in view.last_seen_px: - # Zoom in to the last seen location of this GCP - x, y = view.last_seen_px[self.curr_point] - self.zoom_in(view, x, y) - else: - # Show the whole image - view.subplot.axis("scaled") - view.figure.set_tight_layout(True) - view.subplot.axis("off") - - def bring_new_image(self, new_image, view): - if new_image == view.current_image: - return - t0 = time.time() - view.current_image = new_image - view.subplot.clear() - view.subplot.imshow(self.database.get_image(new_image)) - view.subplot.axis('off') - view.zoomed_in = False - self.set_title(view) - # if self.if_show_epipolar.get(): - # for idx, view in enumerate(self.views): - # self.show_epipolar_lines(idx) - - # Update 'last seen' coordinates for all gcps in this view - for gcp_id in self.database.points: - gcp_visible = self.point_in_view(view, gcp_id) - if gcp_visible is not None: - view.last_seen_px[gcp_id] = gcp_visible - - self.zoom_logic(view) - - self.populate_sequence_list(view) - - self.display_points(view) - - if self.curr_point: - self.highlight_gcp_reprojection(view, self.curr_point, zoom=False) + view.bring_new_image(shot_worst_gcp) diff --git a/annotation_gui_gcp/apps.py b/annotation_gui_gcp/apps.py deleted file mode 100644 index 16511f605..000000000 --- a/annotation_gui_gcp/apps.py +++ /dev/null @@ -1,138 +0,0 @@ -import numpy as np -import string -import random -from opensfm import types -import json -import opensfm.reconstruction as orec -from pprint import pprint -from opensfm import features -# from metrics import compute_distance -from tkinter import Listbox - - -class ref: - def __init__(self, obj): self.obj = obj - - def get(self): return self.obj - - def set(self, obj): self.obj = obj - - -distinct_colors = ['#46f0f0', '#f032e6', '#bcf60c', '#fabebe', '#008080', - '#9a6324', '#fffac8', '#800000', '#aaffc3', '#808000', - '#3cb44b', '#ffe119', '#4363d8', '#f58231', '#911eb4', - '#000075', '#808080', '#ffffff', '#000000'] - -words = None - -def id_generator(size=6, chars=string.ascii_uppercase + string.digits): - global words - if words is None: - try: - import urllib.request - word_url = "http://www.gutenberg.org/files/3201/files/NAMES.TXT" - response = urllib.request.urlopen(word_url) - long_txt = response.read().decode('unicode_escape') - words = long_txt.splitlines() - except Exception as e: - print(e) - words = [] - if len(words) > 0: - firstname = random.choice(words) - lastname = random.choice(words) - return '_'.join((firstname, lastname)).upper() - else: - return ''.join(random.choice(chars) for _ in range(size)) - - -def normalize_img_coords(pixel_coords, img_size): - norm_points = [] - for point in pixel_coords: - point = (point[0] / img_size[1] - 0.5, point[1] / img_size[0] - 0.5) - norm_points.append(point) - return norm_points - - -def denormalize_img_coords(norm_coords, img_size): - denorm_points = [] - for point in norm_coords: - point = ((point[0] + 0.5) * img_size[1], (point[1] + 0.5) * img_size[0]) - denorm_points.append(point) - return denorm_points - - -def pix_coords(x, image): - return features.denormalized_image_coordinates( - np.array([[x[0], x[1]]]), image.shape[1], image.shape[0])[0] - - -def convert_tuple_cords_to_list(matched_points): - list_form = [[] for i in range(2)] - for _, observations in matched_points.items(): - list_form[0].append(observations[0]) - list_form[1].append(observations[1]) - - return np.transpose(list_form) - - -def point_exists(reconstruction, point_id): - for point in reconstruction.points.values(): - if point_id == point.id: - return True - return False - - -class CustomListbox(Listbox): - def __contains__(self, str): - return str in self.get(0, "end") - - -def merge_two_reconstructions(reconstructions): - """"" MERGING POINTS, SHOTS AND CAMERAS""" - - combined_reconstruction = types.Reconstruction() - for point in reconstructions[0].points.values(): - combined_reconstruction.add_point(point) - for point2 in reconstructions[1].points.values(): - if not point_exists(reconstructions[0], point2.id): - combined_reconstruction.add_point(point2) - for recons in reconstructions: - for shot in recons.shots.values(): - combined_reconstruction.add_shot(shot) - - for camera in reconstructions[0].cameras.values(): - combined_reconstruction.add_camera(camera) - - return [combined_reconstruction] - - -def save_gcp(gcp_test, gcp_train, path): - for idx, gcp_set in enumerate([gcp_train, gcp_test]): - data = {'points': []} - for gcp in gcp_set: - data['points'].append( - {'id': gcp.id, - - 'observations': [ - {"shot_id": gcp.observations[0].shot_id, - "projection": list(gcp.observations[0].projection), # in normalized image coordinates - - }, - { - "shot_id": gcp.observations[1].shot_id, - "projection": list(gcp.observations[1].projection), # in normalized image coordinates - - } - ] - } - ) - set_type = '_test' if idx else '_train' - with open(path + '/ground_control_points' + set_type + '.json', 'w') as outfile: - json.dump(data, outfile) - - -def read_gcp_file(file_path): - if file_path: - with open(file_path, 'r') as f: - data = json.load(f) - return data['points'] diff --git a/annotation_gui_gcp/gcp_manager.py b/annotation_gui_gcp/gcp_manager.py new file mode 100644 index 000000000..e739e89a1 --- /dev/null +++ b/annotation_gui_gcp/gcp_manager.py @@ -0,0 +1,159 @@ +import json +import os +import random +import string +import urllib.request +from collections import OrderedDict + +words = None + + +def load_or_download_names(): + global words + path_local_words = os.path.expanduser("~/.annotation_ui_names.txt") + word_url = "http://www.gutenberg.org/files/3201/files/NAMES.TXT" + try: + if not os.path.isfile(path_local_words): + + print(f"Saving {word_url} to {path_local_words}") + urllib.request.urlretrieve(word_url, path_local_words) + + print(f"Loading {path_local_words}") + with open(path_local_words, "rb") as f: + words = f.read().decode("unicode_escape").splitlines() + + except Exception as e: + print(e) + words = [] + + +def id_generator(size=6, chars=string.ascii_uppercase + string.digits): + global words + if len(words) > 0: + firstname = random.choice(words) + lastname = random.choice(words) + return "_".join((firstname, lastname)).upper() + else: + return "".join(random.choice(chars) for _ in range(size)) + + +class GroundControlPointManager: + def __init__(self, path): + self.points = OrderedDict() + self.latlons = {} + self.path = path + self.image_cache = {} + load_or_download_names() + + p_gcp_errors = self.path + "/gcp_reprojections.json" + if os.path.exists(p_gcp_errors): + self.load_gcp_reprojections(p_gcp_errors) + else: + self.gcp_reprojections = {} + + def load_gcp_reprojections(self, filename): + with open(filename, "r") as f: + self.gcp_reprojections = json.load(f) + + def load_from_file(self, file_path): + with open(file_path, "r") as f: + input_points = json.load(f)["points"] + for point in input_points: + self.points[point["id"]] = point["observations"] + latlon = point.get("position") + if latlon: + if "altitude" in latlon: + raise NotImplementedError("Not supported: altitude in GCPs") + self.latlons[point["id"]] = latlon + + def write_to_file(self, filename): + output_points = [] + for point_id in self.points: + out_point = {"id": point_id, "observations": self.points[point_id]} + if out_point["id"] in self.latlons: + out_point["position"] = self.latlons[point_id] + output_points.append(out_point) + with open(filename, "wt") as fp: + json.dump({"points": output_points}, fp, indent=4, sort_keys=True) + + def get_visible_points_coords(self, main_image): + visible_points_coords = OrderedDict() + for point_id, observations in self.points.items(): + for observation in observations: + if observation["shot_id"] == main_image: + visible_points_coords[point_id] = observation["projection"] + + return visible_points_coords + + def point_exists(self, point_id): + return point_id in self.points + + def add_point(self): + new_id = id_generator() + while self.point_exists(new_id): + new_id = id_generator() + self.points[new_id] = [] + return new_id + + def add_point_observation(self, point_id, shot_id, projection, latlon=None): + if not self.point_exists(point_id): + raise ValueError(f"ERROR: trying to modify a non-existing point {point_id}") + + if latlon: + self.latlons[point_id] = { + "latitude": latlon[0], + "longitude": latlon[1], + } + self.points[point_id].append( + { + "shot_id": shot_id, + "projection": projection, + } + ) + + def compute_gcp_errors(self): + error_avg = {} + worst_gcp_error = 0 + worst_gcp = None + shot_worst_gcp = None + for gcp_id in self.points: + error_avg[gcp_id] = 0 + for gcp_id in self.gcp_reprojections: + if gcp_id not in self.points: + continue + for shot_id in self.gcp_reprojections[gcp_id]: + err = self.gcp_reprojections[gcp_id][shot_id]["error"] + error_avg[gcp_id] += err + if err > worst_gcp_error: + worst_gcp_error = err + shot_worst_gcp = shot_id + worst_gcp = gcp_id + error_avg[gcp_id] /= len(self.gcp_reprojections[gcp_id]) + + return worst_gcp, shot_worst_gcp, worst_gcp_error, error_avg + + def shot_with_max_gcp_error(self, image_keys, gcp): + # Return they key with most reprojection error for this GCP + annotated_images = set(self.gcp_reprojections[gcp]).intersection( + set(image_keys) + ) + errors = {k: self.gcp_reprojections[gcp][k]["error"] for k in annotated_images} + if len(errors) > 0: + return max(errors, key=lambda k: errors[k]) + else: + return None + + def remove_gcp(self, point_id): + if self.point_exists(point_id): + del self.points[point_id] + + def remove_point_observation(self, point_id, shot_id): + if not self.point_exists(point_id): + print("ERROR: trying to modify a non-existing point") + return + self.points[point_id] = [ + obs for obs in self.points[point_id] if obs["shot_id"] != shot_id + ] + if point_id in self.gcp_reprojections: + if shot_id in self.gcp_reprojections[point_id]: + self.gcp_reprojections[point_id][shot_id]["error"] = 0 diff --git a/annotation_gui_gcp/geometry.py b/annotation_gui_gcp/geometry.py index c5d2bfae9..34e8803dd 100644 --- a/annotation_gui_gcp/geometry.py +++ b/annotation_gui_gcp/geometry.py @@ -1,38 +1,13 @@ -import numpy as np -from opensfm import dataset, features +from opensfm import dataset -def calc_epipol_line(point_coord, img_pair, path, main_image_idx): - data = dataset.DataSet(path) - reconstruction = data.load_reconstruction() - if not reconstruction[0].get_shot(img_pair[main_image_idx]): - main_shot = reconstruction[1].get_shot(img_pair[main_image_idx]) - pair_shot = reconstruction[0].get_shot(img_pair[not main_image_idx]) - else : - main_shot = reconstruction[0].get_shot(img_pair[main_image_idx]) - pair_shot = reconstruction[0].get_shot(img_pair[not main_image_idx]) - -# CHANGE COMING FROM RECONS 1 - line_pts = [[] for i in range(2)] - for depth in np.arange(-100, 100, 5): - point3d = main_shot.back_project(point_coord, depth) - reprojection = pair_shot.project(point3d) - line_pts[0].append(reprojection[0]) - line_pts[1].append(reprojection[1]) - - return np.transpose(line_pts) - def get_all_track_observations(gcp_database, track_id): print(f"Getting all observations of track {track_id}") data = dataset.DataSet(gcp_database.path) tracks_manager = data.load_tracks_manager() track_obs = tracks_manager.get_track_observations(track_id) - out = {} - for shot_id, obs in track_obs.items(): - h, w = gcp_database.get_image_size(shot_id) - point_px = features.denormalized_image_coordinates(np.array([obs.point]), w, h)[0] - out[shot_id] = point_px - return out + return {shot_id: obs.point for shot_id, obs in track_obs.items()} + def get_tracks_visible_in_image(gcp_database, image_key, min_len=5): print(f"Getting track observations visible in {image_key}") @@ -50,7 +25,5 @@ def get_tracks_visible_in_image(gcp_database, image_key, min_len=5): continue for shot_id, obs in track_obs.items(): if shot_id == image_key: - h, w = gcp_database.get_image_size(shot_id) - point_px = features.denormalized_image_coordinates(np.array([obs.point]), w, h)[0] - out[track_id] = point_px, len(track_obs) + out[track_id] = obs.point, len(track_obs) return out diff --git a/annotation_gui_gcp/image_manager.py b/annotation_gui_gcp/image_manager.py new file mode 100644 index 000000000..4c1eb94ca --- /dev/null +++ b/annotation_gui_gcp/image_manager.py @@ -0,0 +1,56 @@ +import multiprocessing + +import numpy as np +from matplotlib.image import _rgb_to_rgba +from PIL import Image + +IMAGE_MAX_SIZE = 1000 + + +def load_image(path): + rgb = Image.open(path) + + # Reduce to some reasonable maximum size + scale = max(rgb.size) / IMAGE_MAX_SIZE + if scale > 1: + new_w = int(round(rgb.size[0] / scale)) + new_h = int(round(rgb.size[1] / scale)) + rgb = rgb.resize((new_w, new_h), resample=Image.BILINEAR) + # Matplotlib will transform to rgba when plotting + return _rgb_to_rgba(np.asarray(rgb)) + + +class ImageManager: + def __init__(self, seqs, path, preload_images=True): + self.seqs = seqs + self.path = path + self.image_cache = {} + + if preload_images: + self.preload_images() + + def image_path(self, image_name): + return f"{self.path}/images/{image_name}" + + def get_image(self, image_name): + if image_name not in self.image_cache: + path = self.image_path(image_name) + self.image_cache[image_name] = load_image(path) + return self.image_cache[image_name] + + def preload_images(self): + n_cpu = multiprocessing.cpu_count() + print(f"Preloading images with {n_cpu} processes") + paths = [] + image_names = [] + for keys in self.seqs.values(): + for k in keys: + image_names.append(k) + paths.append(self.image_path(k)) + pool = multiprocessing.Pool(processes=n_cpu) + images = pool.map(load_image, paths) + for image_name, im in zip(image_names, images): + self.image_cache[image_name] = im + + def get_image_size(self, image_name): + return self.get_image(image_name).shape[:2] diff --git a/annotation_gui_gcp/image_sequence_view.py b/annotation_gui_gcp/image_sequence_view.py new file mode 100644 index 000000000..65e372ef4 --- /dev/null +++ b/annotation_gui_gcp/image_sequence_view.py @@ -0,0 +1,116 @@ +import tkinter as tk +from typing import Tuple + +import matplotlib +import numpy as np +from matplotlib import pyplot as plt +from opensfm import features + +from geometry import get_all_track_observations, get_tracks_visible_in_image +from view import View + + +class ImageSequenceView(View): + def __init__(self, main_ui, name, sequence_key, image_keys): + self.group_name = sequence_key + self.current_image = image_keys[0] + self.image_keys = image_keys + self.zoom_window_size_px = 200 + self.image_manager = main_ui.image_manager + super(ImageSequenceView, self).__init__(main_ui, name) + + # Auto GCP - related stuff + auto_gcp_button = tk.Button( + self.toolbox, + text="Auto GCP", + command=lambda window=self.window: self.auto_gcp_show_tracks(), + ) + auto_gcp_button.pack(side="top") + + def get_image(self, new_image): + return self.image_manager.get_image(new_image) + + def get_candidate_images(self): + return self.image_keys + + def auto_gcp_show_tracks(self): + h, w = self.image_manager.get_image_size(self.current_image) + tracks = get_tracks_visible_in_image( + self.main_ui.gcp_manager, self.current_image + ) + + # Select some tracks to plot, not all + # nice_tracks = sorted(tracks, key=lambda tid: -tracks[tid]['length']) + nice_tracks = tracks.keys() # all tracks + tracks = {k: tracks[k] for k in nice_tracks} + + if len(tracks) == 0: + print("No valid tracks found") + return + + # Draw track projections + points = [] + track_lengths = [] + for point, track_length in tracks.values(): + points.append(self.gcp_to_pixel_coordinates(*point)) + track_lengths.append(track_length) + + points = np.array(points) + norm = matplotlib.colors.Normalize() + colors = plt.cm.viridis(norm(track_lengths)) + self.tracks_scatter = self.subplot.scatter( + points[:, 0], points[:, 1], s=10, c=colors, marker="x" + ) + self.canvas.draw_idle() + + # The next left or right click will be handled differently by setting this: + self.visible_tracks = tracks + + def auto_gcp_create(self, x, y, add): + # Find track closest to click location and create a GCP from it + x, y = self.pixel_to_gcp_coordinates(x, y) + if add: + points, track_ids = [], [] + for tid, t in self.visible_tracks.items(): + points.append(t[0]) + track_ids.append(tid) + dists = np.linalg.norm(np.array(points) - np.array([[x, y]]), axis=1) + closest_track = track_ids[np.argmin(dists)] + observations = get_all_track_observations( + self.main_ui.gcp_manager, closest_track + ) + + new_gcp = self.main_ui.add_gcp() + print(f"New GCP {new_gcp} with {len(observations)} observations") + for shot_id, point in observations.items(): + self.main_ui.gcp_manager.add_point_observation(new_gcp, shot_id, point) + + self.visible_tracks = None + self.tracks_scatter.remove() + self.canvas.draw() + self.update_image_list_text() + + def pixel_to_latlon(self, x: float, y: float): + return None + + def gcp_to_pixel_coordinates(self, x: float, y: float) -> Tuple[float, float]: + """ + Transforms from normalized coordinates to pixels + + The view displays images at a reduced resolution for speed. We use the image + manager to obtain the reduced coordinates to use for de-normalization. + """ + h, w = self.image_manager.get_image_size(self.current_image) + px = features.denormalized_image_coordinates(np.array([[x, y]]), w, h)[0] + return px.tolist() + + def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: + """ + Transforms from pixels to normalized coordinates + + The view displays images at a reduced resolution for speed. We use the image + manager to obtain the reduced coordinates to use for normalization. + """ + h, w = self.image_manager.get_image_size(self.current_image) + coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] + return coords.tolist() diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 3a18e2325..cfea5959f 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -3,9 +3,11 @@ from collections import OrderedDict, defaultdict from pathlib import Path -from Database import Database -from GUI import Gui -from opensfm import io, dataset +from opensfm import dataset, io + +import GUI +from gcp_manager import GroundControlPointManager +from image_manager import ImageManager def parse_args(): @@ -23,7 +25,7 @@ def parse_args(): parser.add_argument( "--sequence-file", help="dict-of-image-keys JSON file describing each sequence. " - "Format: {'sequence_key': ['im_key_1', 'im_key_2', ...], ...}", + "Format: {'sequence_key': ['im_key_1', 'im_key_2', ...], ...}", default="sequence_database.json", ) parser.add_argument( @@ -44,20 +46,16 @@ def parse_args(): def file_sanity_check(root, seq_dict, fname): # Images available under ./images for a sanity check - available_images = set([p.name for p in (root / "images").iterdir()]) - keys_in_seq_dict = set( - [im_key for seq_keys in seq_dict.values() for im_key in seq_keys] - ) + available_images = {p.name for p in (root / "images").iterdir()} + keys_in_seq_dict = {im_key for seq_keys in seq_dict.values() for im_key in seq_keys} images_not_in_seq_file = available_images - keys_in_seq_dict if len(images_not_in_seq_file) > 0: print(f"{len(images_not_in_seq_file)} images not in {fname}") - missing_image_files = keys_in_seq_dict - available_images - if len(missing_image_files) > 0: - print( - f"There are {len(missing_image_files)} images from {fname} missing in {(root/'images')}" - ) + n_missing = len(keys_in_seq_dict - available_images) + if n_missing > 0: + print(f"There are {n_missing} images from {fname} missing in {(root/'images')}") return available_images @@ -107,11 +105,14 @@ def group_images_by_reconstruction(path): if args.group_by_reconstruction: seqs = group_images_by_reconstruction(path) else: - seqs = load_sequence_database_from_file(path, args.sequence_file, skip_missing=True) - database = Database(seqs, path, preload_images=not args.no_preload) + seqs = load_sequence_database_from_file( + path, args.sequence_file, skip_missing=True + ) + image_manager = ImageManager(seqs, path, preload_images=not args.no_preload) + gcp_manager = GroundControlPointManager(path) root = tk.Tk() root.resizable(True, True) - my_gui = Gui(root, database, len(seqs), args.sequence_group) + GUI.Gui(root, gcp_manager, image_manager, args.sequence_group) root.grid_columnconfigure(0, weight=1) root.grid_rowconfigure(0, weight=1) root.title("Tools") diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py new file mode 100644 index 000000000..09503c4e6 --- /dev/null +++ b/annotation_gui_gcp/view.py @@ -0,0 +1,325 @@ +import colorsys +import tkinter as tk + +import matplotlib +import matplotlib.patches as mpatches + +matplotlib.use("TkAgg") +from matplotlib import patheffects +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg +from matplotlib.figure import Figure + +FONT = "TkFixedFont" + +distinct_colors = [ + "#46f0f0", + "#f032e6", + "#bcf60c", + "#fabebe", + "#008080", + "#9a6324", + "#fffac8", + "#800000", + "#aaffc3", + "#808000", + "#3cb44b", + "#ffe119", + "#4363d8", + "#f58231", + "#911eb4", + "#000075", + "#808080", + "#ffffff", + "#000000", +] + + +def comp_color(color): + r, g, b = color[1:3], color[3:5], color[5:7] + r, g, b = [int(c, 16) for c in (r, g, b)] + h, s, v = colorsys.rgb_to_hsv(r, g, b) + comp_color = "black" if v > 128 else "white" + return comp_color + + +class View: + def __init__(self, main_ui, name): + self.main_ui = main_ui + window = tk.Toplevel(self.main_ui.master, name=name) + self.window = window + + canvas_frame = tk.Frame(window) + canvas_frame.pack(side="left", fill=tk.BOTH, expand=1) + + self.toolbox = tk.Frame(canvas_frame) + self.toolbox.pack(side="left", expand=False, fill=tk.Y) + + self.visible_tracks = None + self.image_list = tk.StringVar() + self.image_list_box = tk.Listbox( + self.toolbox, + font=FONT, + width=12, + selectmode="browse", + listvariable=self.image_list, + ) + self.image_list_box.pack(side="bottom", expand=True, fill=tk.Y) + self.image_list_box.bind("<>", self.onclick_image_list) + + self.figure = Figure() + self.subplot = self.figure.add_subplot(111) + + self.canvas = FigureCanvasTkAgg(self.figure, canvas_frame) + self.canvas.get_tk_widget().pack(side="top", fill=tk.BOTH, expand=1) + self.canvas.mpl_connect( + "button_press_event", lambda event: self.onclick_image(event) + ) + self.canvas.mpl_connect("scroll_event", lambda event: self.on_scroll(event)) + + self.zoomed_in = False + self.last_seen_px = {} + self.plt_artists = [] + self.set_title() + + def onclick_image(self, event): + x, y = event.xdata, event.ydata + if None in (x, y): + return + if event.button == 2: # Middle / wheel click: + if self.zoomed_in: + self.zoom_out() + else: + self.zoom_in(x, y) + self.figure.canvas.draw_idle() + elif event.button in (1, 3): + # Left click or right click + self.last_seen_px[self.main_ui.curr_point] = x, y + if self.visible_tracks: + self.auto_gcp_create(x, y, event.button == 1) + elif self.main_ui.curr_point is not None: + self.add_move_or_remove_gcp(x, y, event.button == 1) + self.main_ui.populate_gcp_list() + self.update_image_list_text() + self.display_points() + else: + return + + def on_scroll(self, event): + if event.xdata is None or event.ydata is None: + return + if event.button == "up": + self.go_to_next_image() + elif event.button == "down": + self.go_to_prev_image() + + def onclick_image_list(self, event): + widget = event.widget + sel = widget.curselection() + if not sel: + return + self.go_to_image_index(int(sel[0])) + + def add_move_or_remove_gcp(self, x, y, add): + if self.main_ui.curr_point is None: + return + reproj = self.main_ui.gcp_manager.gcp_reprojections.get(self.main_ui.curr_point) + if reproj: + reproj.pop(self.current_image, None) + self.main_ui.gcp_manager.remove_point_observation( + self.main_ui.curr_point, self.current_image + ) + if add: + self.main_ui.gcp_manager.add_point_observation( + self.main_ui.curr_point, + self.current_image, + self.pixel_to_gcp_coordinates(x, y), + latlon=self.pixel_to_latlon(x, y), + ) + self.zoom_in(x, y) + else: + self.zoom_out() + + def zoom_in(self, x, y): + if self.zoomed_in: + return + radius = self.zoom_window_size_px / 2 + self.subplot.set_xlim(x - radius, x + radius) + self.subplot.set_ylim(y + radius, y - radius) + self.zoomed_in = True + + def zoom_out(self): + self.subplot.autoscale() + self.zoomed_in = False + + def zoom_logic(self): + point_has_been_seen = self.main_ui.curr_point in self.last_seen_px + if self.main_ui.sticky_zoom.get() and point_has_been_seen: + # Zoom in to the last seen location of this GCP + x, y = self.last_seen_px[self.main_ui.curr_point] + self.zoom_in(x, y) + else: + # Show the whole image + self.subplot.axis("scaled") + self.figure.set_tight_layout(True) + self.subplot.axis("off") + + def point_in_view(self, point): + if point is None: + return None + for projection in self.main_ui.gcp_manager.points[point]: + if projection["shot_id"] == self.current_image: + return self.gcp_to_pixel_coordinates(*projection["projection"]) + return None + + def display_points(self): + visible_points_coords = self.main_ui.gcp_manager.get_visible_points_coords( + self.current_image + ) + self.clear_artists() + + for point_id, coords in visible_points_coords.items(): + color = distinct_colors[divmod(hash(point_id), 19)[1]] + x, y = self.gcp_to_pixel_coordinates(*coords) + artists = [ + mpatches.Circle((x, y), 5, color=color, fill=False), + ] + + if self.main_ui.show_gcp_names.get() or point_id == self.main_ui.curr_point: + text = matplotlib.text.Annotation( + point_id, + (x, y), + xytext=[0, 7], + # fontsize=9, + textcoords="offset points", + ha="center", + va="bottom", + color=color, + ) + text.set_path_effects( + [ + patheffects.Stroke(linewidth=3, foreground=comp_color(color)), + patheffects.Normal(), + ] + ) + artists.append(text) + + if point_id == self.main_ui.curr_point: + artists.extend( + [ + mpatches.Circle((x, y), 0.5, color=color, fill=True), + mpatches.Circle((x, y), 10, color=color, fill=False), + mpatches.Circle((x, y), 11, color=color, fill=False), + mpatches.Circle((x, y), 12, color=color, fill=False), + ] + ) + for art in artists: + self.plt_artists.append(art) + self.subplot.add_artist(art) + + self.figure.canvas.draw() + + def clear_artists(self): + for artist in self.plt_artists: + artist.set_visible(False) + del artist + + def populate_image_list(self): + self.update_image_list_text() + self.update_image_list_highlight() + + def update_image_list_text(self): + items = [] + self.images_in_list = self.get_candidate_images() + n_digits = len(str(len(self.images_in_list))) + for ix, image_name in enumerate(self.images_in_list): + points = self.main_ui.gcp_manager.get_visible_points_coords(image_name) + txt = "{:0{n_digits}} {}".format(ix + 1, len(points), n_digits=n_digits) + shot_std = self.main_ui.shot_std.get(image_name, None) + if shot_std: + txt += " {:.2f}".format(shot_std) + items.append(txt) + self.image_list.set(items) + + def update_image_list_highlight(self): + defaultbg = self.window.cget("bg") + for ix, shot in enumerate(self.image_keys): + bg = "green" if shot == self.current_image else defaultbg + self.image_list_box.itemconfig(ix, bg=bg) + + def bring_new_image(self, new_image): + if new_image == self.current_image: + return + self.current_image = new_image + self.subplot.clear() + img = self.get_image(new_image) + self.subplot.imshow(img) + self.subplot.axis("on") + self.subplot.axis("scaled") + self.zoomed_in = False + self.set_title() + + # Update 'last seen' coordinates for all gcps in this view + for gcp_id in self.main_ui.gcp_manager.points: + gcp_visible = self.point_in_view(gcp_id) + if gcp_visible is not None: + self.last_seen_px[gcp_id] = gcp_visible + + self.zoom_logic() + + self.update_image_list_highlight() + + self.display_points() + + if self.main_ui.curr_point: + self.highlight_gcp_reprojection(self.main_ui.curr_point, zoom=False) + + def highlight_gcp_reprojection(self, point_id, zoom=True): + if point_id not in self.main_ui.gcp_manager.gcp_reprojections: + return + shot = self.current_image + x, y = None, None + for obs in self.main_ui.gcp_manager.points[point_id]: + if obs["shot_id"] == shot: + x, y = obs["projection"] + if x is None: + return + + reproj = self.main_ui.gcp_manager.gcp_reprojections[point_id].get(shot) + if not reproj: + return + x2, y2 = reproj["reprojection"] + artists = self.subplot.plot([x, x2], [y, y2], "r-") + self.plt_artists.extend(artists) + if zoom: + self.zoom_in(x, y) + self.canvas.draw_idle() + + def go_to_next_image(self): + self.go_to_adjacent_image(+1) + + def go_to_prev_image(self): + self.go_to_adjacent_image(-1) + + def go_to_adjacent_image(self, offset, linked=True): + target_ix = self.images_in_list.index(self.current_image) + offset + if 0 <= target_ix < len(self.images_in_list): + self.go_to_image_index(target_ix, linked) + + def go_to_image_index(self, idx, linked=True): + views_to_update = {self} + if linked: + groups_this_view = [ + g for g in self.main_ui.sequence_groups if self.group_name in g + ] + for g in groups_this_view: + for v in self.main_ui.views: + if v.group_name in g: + views_to_update.add(v) + for v in views_to_update: + v.bring_new_image(self.images_in_list[idx]) + + def set_title(self): + shot = self.current_image + seq_ix = self.image_keys.index(shot) + title = f"{self.group_name} [{seq_ix+1}/{len(self.image_keys)}]: {shot}" + self.window.title(title) From 298ae7619c1495bfa2761a4eedb5b915dfcfc8c1 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 16 Oct 2020 18:46:44 +0200 Subject: [PATCH 22/64] feat: Orthophoto views - We can now load/save latitude and longitude from GCPs - The GCP annotations in each orthophoto are saved as normalized pixels like all images. - Additionally, a view can be tagged as a 'geo reference'. In that case, the gcp annotations will also include the lat/lon --- annotation_gui_gcp/orthophoto_manager.py | 51 ++++++++++++++++ annotation_gui_gcp/orthophoto_view.py | 78 ++++++++++++++++++++++++ 2 files changed, 129 insertions(+) create mode 100644 annotation_gui_gcp/orthophoto_manager.py create mode 100644 annotation_gui_gcp/orthophoto_view.py diff --git a/annotation_gui_gcp/orthophoto_manager.py b/annotation_gui_gcp/orthophoto_manager.py new file mode 100644 index 000000000..d3b1bd10f --- /dev/null +++ b/annotation_gui_gcp/orthophoto_manager.py @@ -0,0 +1,51 @@ +from pathlib import Path + +import rasterio +import rasterio.features +import rasterio.plot +import rasterio.warp +import rasterio.windows +from rasterio.plot import reshape_as_image + + +class OrthoPhotoManager: + def __init__(self, path: str, size: float): + self.path = Path(path) + self.size = size + self.image_keys = [p.name for p in self.path.iterdir() if p.suffix == ".tif"] + + def check_latlon_covered(self, img: str, lat: float, lon: float): + t = rasterio.open(self.path / img) + xs, ys = rasterio.warp.transform("EPSG:4326", t.crs, [lon], [lat], zs=None) + row_center, col_center = t.index(xs[0], ys[0]) + window = rasterio.windows.Window(row_center, col_center, 1, 1) + mask = t.read_masks(1, window=window, boundless=True) + return mask.item != 0 + + def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float): + t = rasterio.open(self.path / img) + + # From latlon to the coordinate reference system of the image in m + xs, ys = rasterio.warp.transform("EPSG:4326", t.crs, [lon], [lat], zs=None) + + # Create the corners of the viewing window in pixels + top, right = t.index(xs[0] + size / 2, ys[0] + size / 2) + bottom, left = t.index(xs[0] - size / 2, ys[0] - size / 2) + + window = rasterio.windows.Window(left, top, right - left, bottom - top) + self.current_window = window + + # TODO downsample image if the zoom level is too low / the image too large + tw = reshape_as_image(t.read(window=window, boundless=True)) + return tw, window, t + + def get_candidate_images(self, lat: float, lon: float): + # Returns only the images that cover this lat/lon + # TODO actually filter around latlon, update list + return [k for k in self.image_keys if self.check_latlon_covered(k, lat, lon)] + + def get_image(self, img, lat: float, lon: float, size: float): + return self.read_image_around_latlon(img, lat, lon, size) + + def get_image_size(self, img): + return self.current_window.height, self.current_window.width diff --git a/annotation_gui_gcp/orthophoto_view.py b/annotation_gui_gcp/orthophoto_view.py new file mode 100644 index 000000000..d8f3a4f1f --- /dev/null +++ b/annotation_gui_gcp/orthophoto_view.py @@ -0,0 +1,78 @@ +from typing import Tuple + +import numpy as np +import rasterio.warp +from opensfm import features + +import GUI +from orthophoto_manager import OrthoPhotoManager +from view import View + + +class OrthoPhotoView(View): + def __init__(self, main_ui: GUI.Gui, path: str, is_geo_reference: bool = False): + """[summary] + + Args: + main_ui (GUI.Gui) + path (str): path containing geotiffs + """ + self.name = path.split("/")[-1] + self.image_manager = OrthoPhotoManager(path, 100.0) + self.image_keys = self.image_manager.image_keys + self.zoom_window_size_px = 500 + self.current_image = self.image_keys[0] + self.is_geo_reference = is_geo_reference + + lon, lat = -122.33500709776536, 47.61825766649998 + self.center_lat, self.center_lon = lat, lon + self.group_name = "Images covering lat:{:.4f}, lon:{:.4f}".format(lat, lon) + + self.size = 50 # TODO add widget for zoom level + super(OrthoPhotoView, self).__init__(main_ui, self.name) + + def get_image(self, new_image): + image, image_window, geot = self.image_manager.read_image_around_latlon( + new_image, self.center_lat, self.center_lon, self.size + ) + self.image_window = image_window + self.geot = geot + return image + + def get_candidate_images(self): + return self.image_manager.get_candidate_images(self.center_lat, self.center_lon) + + def pixel_to_latlon(self, x: float, y: float): + """ + From pixels (in the viewing window) to latlon + """ + if not self.is_geo_reference: + return None + + # Pixel to whatever crs the image is in + x, y = self.geot.xy(y, x) + # And then to WSG84 (lat/lon) + lons, lats = rasterio.warp.transform(self.geot.crs, "EPSG:4326", [x], [y]) + return lats[0], lons[0] + + def gcp_to_pixel_coordinates(self, x: float, y: float) -> Tuple[float, float]: + """ + Transforms from normalized coordinates (in the whole geotiff) to + pixels (in the viewing window) + """ + h, w = self.image_manager.get_image_size(self.current_image) + px = features.denormalized_image_coordinates(np.array([[x, y]]), w, h)[0] + x = px[0] - self.image_window.col_off + y = px[1] - self.image_window.row_off + return [x, y] + + def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: + """ + Transforms from pixels (in the viewing window) to normalized coordinates + (in the whole geotiff) + """ + x += self.image_window.col_off + y += self.image_window.row_off + h, w = self.image_manager.get_image_size(self.current_image) + coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] + return coords.tolist() From 11165163569e114e1d2c7749500ab43b8c07c139 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 19 Oct 2020 14:52:04 +0200 Subject: [PATCH 23/64] feat: Propose ortho views based on GPS --- annotation_gui_gcp/GUI.py | 86 ++++++++++++--------- annotation_gui_gcp/image_manager.py | 18 +++++ annotation_gui_gcp/image_sequence_view.py | 33 ++++++-- annotation_gui_gcp/main.py | 14 +++- annotation_gui_gcp/orthophoto_manager.py | 8 +- annotation_gui_gcp/orthophoto_view.py | 61 ++++++++++++--- annotation_gui_gcp/view.py | 94 +++++++++++++---------- 7 files changed, 218 insertions(+), 96 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index dacf16cec..d1553c5cb 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -10,12 +10,15 @@ matplotlib.use("TkAgg") from image_sequence_view import ImageSequenceView +from orthophoto_view import OrthoPhotoView FONT = "TkFixedFont" class Gui: - def __init__(self, master, gcp_manager, image_manager, sequence_groups=()): + def __init__( + self, master, gcp_manager, image_manager, sequence_groups=(), ortho_paths=[] + ): self.master = master self.gcp_manager = gcp_manager self.image_manager = image_manager @@ -29,9 +32,7 @@ def __init__(self, master, gcp_manager, image_manager, sequence_groups=()): master.bind_all("z", lambda event: self.toggle_zoom_all_views()) master.bind_all("x", lambda event: self.toggle_sticky_zoom()) master.bind_all("a", lambda event: self.go_to_current_gcp()) - master.bind_all("w", lambda event: self.go_to_next_image_all()) - master.bind_all("s", lambda event: self.go_to_prev_image_all()) - self.create_ui() + self.create_ui(ortho_paths) master.lift() p_default_gcp = self.path + "/ground_control_points.json" @@ -41,15 +42,17 @@ def __init__(self, master, gcp_manager, image_manager, sequence_groups=()): if os.path.exists(p_shot_std): self.load_shot_std(p_shot_std) - for view in self.views: - view.populate_image_list() - view.bring_new_image(view.images_in_list[0]) - - def create_ui(self): + def create_ui(self, ortho_paths): tools_frame = tk.Frame(self.master) tools_frame.pack(side="left", expand=0, fill=tk.Y) self.create_tools(tools_frame) - self.create_image_views() + self.create_sequence_views(show_ortho_track=len(ortho_paths) > 0) + self.ortho_views = [] + if ortho_paths: + v = self.sequence_views[0] + k = v.current_image + latlon = v.latlons[k] + self.create_ortho_views(ortho_paths, latlon["lat"], latlon["lon"]) self.master.update_idletasks() self.arrange_ui_onerow() @@ -64,9 +67,9 @@ def arrange_ui_onerow(self): x, y = 0, y + h screen_width = master.winfo_screenwidth() screen_height = master.winfo_screenheight() - w = screen_width / len(self.views) + w = screen_width / len(self.sequence_views) h = screen_height - y - for view in self.views: + for view in self.sequence_views: view.window.geometry("%dx%d+%d+%d" % (w, h, x, y)) x += w @@ -124,13 +127,22 @@ def create_tools(self, master): button = tk.Button(io_frame, text="Save As", command=self.save_gcps_as) button.pack(side="left") - def create_image_views(self): - self.views = [] - for sequence_key, image_keys in self.image_manager.seqs.items(): - v = ImageSequenceView( - self, f"view {len(self.views)+1}", sequence_key, image_keys + def create_ortho_views(self, ortho_paths, lat, lon): + for ortho_p in ortho_paths: + v = OrthoPhotoView( + self, + ortho_p, + init_lat=lat, + init_lon=lon, + is_geo_reference=ortho_p is ortho_paths[0], ) - self.views.append(v) + self.ortho_views.append(v) + + def create_sequence_views(self, show_ortho_track): + self.sequence_views = [] + for sequence_key, image_keys in self.image_manager.seqs.items(): + v = ImageSequenceView(self, sequence_key, image_keys, show_ortho_track) + self.sequence_views.append(v) def analyze_3d_to_3d(self): self.analyze(mode="3d_to_3d") @@ -160,7 +172,7 @@ def analyze(self, mode): p_gcp_errors = self.path + "/gcp_reprojections.json" self.gcp_manager.load_gcp_reprojections(p_gcp_errors) - for view in self.views: + for view in self.sequence_views: view.populate_image_list() print("Done analyzing") @@ -182,7 +194,7 @@ def load_gcps(self, filename=None): return self.quick_save_filename = filename self.gcp_manager.load_from_file(filename) - for view in self.views: + for view in self.sequence_views: view.display_points() self.populate_gcp_list() @@ -201,8 +213,8 @@ def toggle_zoom_all_views(self): # Zoom in/out on every view, centered on the location of the current GCP if self.curr_point is None: return - any_zoomed_in = any(view.zoomed_in for view in self.views) - for view in self.views: + any_zoomed_in = any(view.zoomed_in for view in self.sequence_views) + for view in self.sequence_views: if any_zoomed_in: view.zoom_out() else: @@ -244,7 +256,7 @@ def remove_gcp(self): self.curr_point = None self.gcp_manager.remove_gcp(to_be_removed_point) - for view in self.views: + for view in self.sequence_views: view.display_points() self.populate_gcp_list() @@ -260,7 +272,7 @@ def onclick_gcp_list(self, event): else: self.curr_point = value.split(" ")[1] - for view in self.views: + for view in self.sequence_views: view.display_points() if self.curr_point: view.highlight_gcp_reprojection(self.curr_point, zoom=False) @@ -286,14 +298,6 @@ def save_gcps_as(self): self.quick_save_filename = filename return self.save_gcps() - def go_to_next_image_all(self): - for view in self.views: - view.go_to_adjacent_image(+1, linked=False) - - def go_to_prev_image_all(self): - for view in self.views: - view.go_to_adjacent_image(-1, linked=False) - def go_to_current_gcp(self): """ Jumps to the currently selected GCP in all views where it was not visible @@ -303,9 +307,9 @@ def go_to_current_gcp(self): shots_gcp_seen = { p["shot_id"] for p in self.gcp_manager.points[self.curr_point] } - for view in self.views: + for view in self.sequence_views: shots_gcp_seen_this_view = list( - shots_gcp_seen.intersection(view.image_keys) + shots_gcp_seen.intersection(view.images_in_list) ) if ( len(shots_gcp_seen_this_view) > 0 @@ -329,10 +333,20 @@ def go_to_worst_gcp(self): self.gcp_list_box.selection_set(ix) break - for view in self.views: + for view in self.sequence_views: # Get the shot with worst reprojection error that in this view shot_worst_gcp = self.gcp_manager.shot_with_max_gcp_error( - view.image_keys, worst_gcp + view.images_in_list, worst_gcp ) if shot_worst_gcp: view.bring_new_image(shot_worst_gcp) + + def clear_latlon_sources(self, view): + # The user has activated the 'Track this' checkbox in some view + for v in self.sequence_views + self.ortho_views: + if v is not view: + v.is_latlon_source.set(False) + + def refocus_overhead_views(self, lat, lon): + for view in self.ortho_views: + view.refocus(lat, lon) diff --git a/annotation_gui_gcp/image_manager.py b/annotation_gui_gcp/image_manager.py index 4c1eb94ca..82ed25a9b 100644 --- a/annotation_gui_gcp/image_manager.py +++ b/annotation_gui_gcp/image_manager.py @@ -2,6 +2,7 @@ import numpy as np from matplotlib.image import _rgb_to_rgba +from opensfm import dataset from PIL import Image IMAGE_MAX_SIZE = 1000 @@ -38,6 +39,23 @@ def get_image(self, image_name): self.image_cache[image_name] = load_image(path) return self.image_cache[image_name] + def load_latlons(self): + data = dataset.DataSet(self.path) + latlons = {} + for keys in self.seqs.values(): + for k in keys: + exif = data.load_exif(k) + if "l" in exif: + latlons[k] = exif["l"] + elif "gps" in exif: + latlons[k] = { + "lat": exif["gps"]["latitude"], + "lon": exif["gps"]["longitude"], + } + elif "cl" in exif: + latlons[k] = exif["cl"] + return latlons + def preload_images(self): n_cpu = multiprocessing.cpu_count() print(f"Preloading images with {n_cpu} processes") diff --git a/annotation_gui_gcp/image_sequence_view.py b/annotation_gui_gcp/image_sequence_view.py index 65e372ef4..067c34e05 100644 --- a/annotation_gui_gcp/image_sequence_view.py +++ b/annotation_gui_gcp/image_sequence_view.py @@ -11,13 +11,12 @@ class ImageSequenceView(View): - def __init__(self, main_ui, name, sequence_key, image_keys): + def __init__(self, main_ui, sequence_key, image_keys, show_ortho_track): self.group_name = sequence_key - self.current_image = image_keys[0] - self.image_keys = image_keys + self.images_in_list = image_keys self.zoom_window_size_px = 200 self.image_manager = main_ui.image_manager - super(ImageSequenceView, self).__init__(main_ui, name) + super(ImageSequenceView, self).__init__(main_ui, show_ortho_track) # Auto GCP - related stuff auto_gcp_button = tk.Button( @@ -27,11 +26,15 @@ def __init__(self, main_ui, name, sequence_key, image_keys): ) auto_gcp_button.pack(side="top") + self.populate_image_list() + self.bring_new_image(self.images_in_list[0]) + self.set_title() + def get_image(self, new_image): return self.image_manager.get_image(new_image) def get_candidate_images(self): - return self.image_keys + return self.images_in_list def auto_gcp_show_tracks(self): h, w = self.image_manager.get_image_size(self.current_image) @@ -58,7 +61,7 @@ def auto_gcp_show_tracks(self): points = np.array(points) norm = matplotlib.colors.Normalize() colors = plt.cm.viridis(norm(track_lengths)) - self.tracks_scatter = self.subplot.scatter( + self.tracks_scatter = self.ax.scatter( points[:, 0], points[:, 1], s=10, c=colors, marker="x" ) self.canvas.draw_idle() @@ -114,3 +117,21 @@ def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: h, w = self.image_manager.get_image_size(self.current_image) coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] return coords.tolist() + + def set_title(self): + shot = self.current_image + seq_ix = self.images_in_list.index(shot) + title = f"{self.group_name} [{seq_ix+1}/{len(self.images_in_list)}]: {shot}" + self.window.title(title) + + def go_to_image_index(self, idx): + views_to_update = {self} + groups_this_view = [ + g for g in self.main_ui.sequence_groups if self.group_name in g + ] + for g in groups_this_view: + for v in self.main_ui.sequence_views: + if v.group_name in g and v.images_in_list: + views_to_update.add(v) + for v in views_to_update: + v.bring_new_image(v.images_in_list[idx]) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index cfea5959f..190b226f3 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -41,6 +41,18 @@ def parse_args(): "Can be used multiple times to define several groups", default=[], ) + parser.add_argument( + "-o", + "--ortho", + type=str, + action="append", + help="Specify one or more groups of linked sequences. " + "Linked sequences are synchronized such that all views " + "from the group will always show the same frame index. " + "Useful for camera rigs. usage: -g sequence_key_1 sequence_key_2. " + "Can be used multiple times to define several groups", + default=[], + ) return parser.parse_args() @@ -112,7 +124,7 @@ def group_images_by_reconstruction(path): gcp_manager = GroundControlPointManager(path) root = tk.Tk() root.resizable(True, True) - GUI.Gui(root, gcp_manager, image_manager, args.sequence_group) + ui = GUI.Gui(root, gcp_manager, image_manager, args.sequence_group, args.ortho) root.grid_columnconfigure(0, weight=1) root.grid_rowconfigure(0, weight=1) root.title("Tools") diff --git a/annotation_gui_gcp/orthophoto_manager.py b/annotation_gui_gcp/orthophoto_manager.py index d3b1bd10f..23380584b 100644 --- a/annotation_gui_gcp/orthophoto_manager.py +++ b/annotation_gui_gcp/orthophoto_manager.py @@ -20,7 +20,11 @@ def check_latlon_covered(self, img: str, lat: float, lon: float): row_center, col_center = t.index(xs[0], ys[0]) window = rasterio.windows.Window(row_center, col_center, 1, 1) mask = t.read_masks(1, window=window, boundless=True) - return mask.item != 0 + return mask.item() != 0 + + def load_latlons(self): + # We don't have a 'canonical' lat/lon for orthophotos + return {} def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float): t = rasterio.open(self.path / img) @@ -40,8 +44,6 @@ def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float return tw, window, t def get_candidate_images(self, lat: float, lon: float): - # Returns only the images that cover this lat/lon - # TODO actually filter around latlon, update list return [k for k in self.image_keys if self.check_latlon_covered(k, lat, lon)] def get_image(self, img, lat: float, lon: float, size: float): diff --git a/annotation_gui_gcp/orthophoto_view.py b/annotation_gui_gcp/orthophoto_view.py index d8f3a4f1f..04b1b85d5 100644 --- a/annotation_gui_gcp/orthophoto_view.py +++ b/annotation_gui_gcp/orthophoto_view.py @@ -4,32 +4,38 @@ import rasterio.warp from opensfm import features -import GUI from orthophoto_manager import OrthoPhotoManager from view import View class OrthoPhotoView(View): - def __init__(self, main_ui: GUI.Gui, path: str, is_geo_reference: bool = False): + def __init__( + self, + main_ui, + path: str, + init_lat: float, + init_lon: float, + is_geo_reference: bool = False, + ): """[summary] Args: main_ui (GUI.Gui) path (str): path containing geotiffs """ - self.name = path.split("/")[-1] self.image_manager = OrthoPhotoManager(path, 100.0) - self.image_keys = self.image_manager.image_keys + self.images_in_list = self.image_manager.image_keys self.zoom_window_size_px = 500 - self.current_image = self.image_keys[0] self.is_geo_reference = is_geo_reference - lon, lat = -122.33500709776536, 47.61825766649998 - self.center_lat, self.center_lon = lat, lon - self.group_name = "Images covering lat:{:.4f}, lon:{:.4f}".format(lat, lon) - self.size = 50 # TODO add widget for zoom level - super(OrthoPhotoView, self).__init__(main_ui, self.name) + + super(OrthoPhotoView, self).__init__(main_ui, False) + self.refocus(init_lat, init_lon) + self.populate_image_list() + if self.images_in_list: + self.bring_new_image(self.images_in_list[0]) + self.set_title() def get_image(self, new_image): image, image_window, geot = self.image_manager.read_image_around_latlon( @@ -76,3 +82,38 @@ def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: h, w = self.image_manager.get_image_size(self.current_image) coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] return coords.tolist() + + def refocus(self, lat, lon): + self.center_lat = lat + self.center_lon = lon + self.populate_image_list() + if self.images_in_list: + if self.current_image not in self.images_in_list: + self.bring_new_image(self.images_in_list[0]) + else: + self.bring_new_image(self.current_image) + self.set_title() + + def bring_new_image(self, new_image): + super(OrthoPhotoView, self).bring_new_image(new_image, force=True) + xlim = self.ax.get_xlim() + ylim = self.ax.get_ylim() + artists = self.ax.plot(np.mean(xlim), np.mean(ylim), "rx") + self.plt_artists.extend(artists) + self.canvas.draw_idle() + + def set_title(self): + lat, lon = self.center_lat, self.center_lon + if self.images_in_list: + t = "Images covering lat:{:.4f}, lon:{:.4f}".format(lat, lon) + shot = self.current_image + seq_ix = self.images_in_list.index(shot) + title = f"{t} [{seq_ix+1}/{len(self.images_in_list)}]: {shot}" + else: + title = f"No orthophotos around {lat}, {lon}" + self.current_image = None + self.ax.clear() + self.ax.axis("off") + self.canvas.draw_idle() + + self.window.title(title) diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py index 09503c4e6..1549d9d16 100644 --- a/annotation_gui_gcp/view.py +++ b/annotation_gui_gcp/view.py @@ -43,16 +43,31 @@ def comp_color(color): class View: - def __init__(self, main_ui, name): + def __init__(self, main_ui, show_ortho_track): self.main_ui = main_ui - window = tk.Toplevel(self.main_ui.master, name=name) + window = tk.Toplevel(self.main_ui.master) self.window = window + self.current_image = None canvas_frame = tk.Frame(window) canvas_frame.pack(side="left", fill=tk.BOTH, expand=1) self.toolbox = tk.Frame(canvas_frame) - self.toolbox.pack(side="left", expand=False, fill=tk.Y) + self.toolbox.pack(side="left", expand=False, fill=tk.BOTH) + + self.is_latlon_source = tk.BooleanVar(value=False) + if show_ortho_track: + self.latlons = self.load_latlons() + if self.latlons: + button = tk.Checkbutton( + self.toolbox, + text="Overhead focus", + var=self.is_latlon_source, + command=self.trackToggle, + ) + button.pack(side="top") + else: + self.latlons = {} self.visible_tracks = None self.image_list = tk.StringVar() @@ -67,7 +82,7 @@ def __init__(self, main_ui, name): self.image_list_box.bind("<>", self.onclick_image_list) self.figure = Figure() - self.subplot = self.figure.add_subplot(111) + self.ax = self.figure.add_subplot(111) self.canvas = FigureCanvasTkAgg(self.figure, canvas_frame) self.canvas.get_tk_widget().pack(side="top", fill=tk.BOTH, expand=1) @@ -79,11 +94,18 @@ def __init__(self, main_ui, name): self.zoomed_in = False self.last_seen_px = {} self.plt_artists = [] - self.set_title() + + def load_latlons(self): + """Loads a latlon associated with each image key""" + return self.image_manager.load_latlons() + + def trackToggle(self): + if self.is_latlon_source.get(): + self.main_ui.clear_latlon_sources(self) def onclick_image(self, event): x, y = event.xdata, event.ydata - if None in (x, y): + if None in (x, y) or self.current_image is None: return if event.button == 2: # Middle / wheel click: if self.zoomed_in: @@ -143,12 +165,12 @@ def zoom_in(self, x, y): if self.zoomed_in: return radius = self.zoom_window_size_px / 2 - self.subplot.set_xlim(x - radius, x + radius) - self.subplot.set_ylim(y + radius, y - radius) + self.ax.set_xlim(x - radius, x + radius) + self.ax.set_ylim(y + radius, y - radius) self.zoomed_in = True def zoom_out(self): - self.subplot.autoscale() + self.ax.autoscale() self.zoomed_in = False def zoom_logic(self): @@ -159,9 +181,9 @@ def zoom_logic(self): self.zoom_in(x, y) else: # Show the whole image - self.subplot.axis("scaled") + self.ax.axis("scaled") self.figure.set_tight_layout(True) - self.subplot.axis("off") + self.ax.axis("off") def point_in_view(self, point): if point is None: @@ -214,7 +236,7 @@ def display_points(self): ) for art in artists: self.plt_artists.append(art) - self.subplot.add_artist(art) + self.ax.add_artist(art) self.figure.canvas.draw() @@ -242,19 +264,19 @@ def update_image_list_text(self): def update_image_list_highlight(self): defaultbg = self.window.cget("bg") - for ix, shot in enumerate(self.image_keys): + for ix, shot in enumerate(self.images_in_list): bg = "green" if shot == self.current_image else defaultbg self.image_list_box.itemconfig(ix, bg=bg) - def bring_new_image(self, new_image): - if new_image == self.current_image: + def bring_new_image(self, new_image, force=False): + if new_image == self.current_image and not force: return self.current_image = new_image - self.subplot.clear() + self.ax.clear() img = self.get_image(new_image) - self.subplot.imshow(img) - self.subplot.axis("on") - self.subplot.axis("scaled") + self.ax.imshow(img) + self.ax.axis("on") + self.ax.axis("scaled") self.zoomed_in = False self.set_title() @@ -273,6 +295,10 @@ def bring_new_image(self, new_image): if self.main_ui.curr_point: self.highlight_gcp_reprojection(self.main_ui.curr_point, zoom=False) + latlon = self.latlons.get(new_image) + if self.is_latlon_source.get() and latlon: + self.main_ui.refocus_overhead_views(latlon["lat"], latlon["lon"]) + def highlight_gcp_reprojection(self, point_id, zoom=True): if point_id not in self.main_ui.gcp_manager.gcp_reprojections: return @@ -288,7 +314,9 @@ def highlight_gcp_reprojection(self, point_id, zoom=True): if not reproj: return x2, y2 = reproj["reprojection"] - artists = self.subplot.plot([x, x2], [y, y2], "r-") + x, y = self.gcp_to_pixel_coordinates(x, y) + x2, y2 = self.gcp_to_pixel_coordinates(x2, y2) + artists = self.ax.plot([x, x2], [y, y2], "r-") self.plt_artists.extend(artists) if zoom: self.zoom_in(x, y) @@ -300,26 +328,12 @@ def go_to_next_image(self): def go_to_prev_image(self): self.go_to_adjacent_image(-1) - def go_to_adjacent_image(self, offset, linked=True): + def go_to_adjacent_image(self, offset): + if not self.images_in_list: + return target_ix = self.images_in_list.index(self.current_image) + offset if 0 <= target_ix < len(self.images_in_list): - self.go_to_image_index(target_ix, linked) + self.go_to_image_index(target_ix) - def go_to_image_index(self, idx, linked=True): - views_to_update = {self} - if linked: - groups_this_view = [ - g for g in self.main_ui.sequence_groups if self.group_name in g - ] - for g in groups_this_view: - for v in self.main_ui.views: - if v.group_name in g: - views_to_update.add(v) - for v in views_to_update: - v.bring_new_image(self.images_in_list[idx]) - - def set_title(self): - shot = self.current_image - seq_ix = self.image_keys.index(shot) - title = f"{self.group_name} [{seq_ix+1}/{len(self.image_keys)}]: {shot}" - self.window.title(title) + def go_to_image_index(self, idx): + self.bring_new_image(self.images_in_list[idx]) From 986564dd97016ba7883bbd830d6602e17d97f262 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 19 Oct 2020 17:24:32 +0200 Subject: [PATCH 24/64] fix: display annotation counts on start --- annotation_gui_gcp/GUI.py | 1 + 1 file changed, 1 insertion(+) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index d1553c5cb..1db57e95c 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -196,6 +196,7 @@ def load_gcps(self, filename=None): self.gcp_manager.load_from_file(filename) for view in self.sequence_views: view.display_points() + view.populate_image_list() self.populate_gcp_list() def add_gcp(self): From 7d43b3d1f90b8fd75d7b9117eb128bce23358de1 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 20 Oct 2020 11:41:54 +0200 Subject: [PATCH 25/64] chore: formatting of run_ba.py --- annotation_gui_gcp/run_ba.py | 218 +++++++++++++++++++---------------- 1 file changed, 121 insertions(+), 97 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index bd1300bc2..004187587 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -1,16 +1,11 @@ import argparse import json +import logging import os import numpy as np -import logging - import opensfm.reconstruction as orec -from opensfm import align -from opensfm import dataset -from opensfm import log -from opensfm import multiview -from opensfm import pygeometry +from opensfm import align, dataset, log, multiview, pygeometry from opensfm import transformations as tf from opensfm import types @@ -91,8 +86,10 @@ def reproject_gcps(gcps, reconstruction): shot = reconstruction.shots[observation.shot_id] reproj = shot.project(point) error = np.linalg.norm(reproj - observation.projection) - output[gcp.id][observation.shot_id] = {'error': error, - 'reprojection': [reproj[0], reproj[1]]} + output[gcp.id][observation.shot_id] = { + "error": error, + "reprojection": [reproj[0], reproj[1]], + } return output @@ -100,16 +97,16 @@ def get_all_reprojection_errors(gcp_reprojections): output = [] for gcp_id in gcp_reprojections: for shot_id in gcp_reprojections[gcp_id]: - e = gcp_reprojections[gcp_id][shot_id]['error'] + e = gcp_reprojections[gcp_id][shot_id]["error"] output.append((gcp_id, shot_id, e)) - return sorted(output, key=lambda t:-t[2]) + return sorted(output, key=lambda t: -t[2]) def get_number_of_wrong_annotations_per_gcp(gcp_reprojections, wrong_threshold): output = {} for gcp_id, reprojections in gcp_reprojections.items(): - errors = [reprojections[shot_id]['error'] for shot_id in reprojections] - output[gcp_id] = sum(e>wrong_threshold for e in errors) + errors = [reprojections[shot_id]["error"] for shot_id in reprojections] + output[gcp_id] = sum(e > wrong_threshold for e in errors) return output @@ -117,10 +114,10 @@ def compute_gcp_std(gcp_errors): """Compute the standard deviation of reprojection errors of all gcps.""" all_errors = [] for gcp_id in gcp_errors: - errors = [e['error'] for e in gcp_errors[gcp_id].values()] + errors = [e["error"] for e in gcp_errors[gcp_id].values()] logger.info(f"gcp {gcp_id} mean reprojection error = {np.mean(errors)}") all_errors.extend(errors) - return np.sqrt(np.mean(np.array(all_errors)**2)) + return np.sqrt(np.mean(np.array(all_errors) ** 2)) def find_alignment(points0, points1): @@ -136,7 +133,7 @@ def find_alignment(points0, points1): v0 = np.array(v0).T v1 = np.array(v1).T M = tf.affine_matrix_from_points(v0, v1, shear=False) - s = np.linalg.det(M[:3, :3])**(1. / 3.) + s = np.linalg.det(M[:3, :3]) ** (1.0 / 3.0) A = M[:3, :3] / s b = M[:3, 3] return s, A, b @@ -145,15 +142,16 @@ def find_alignment(points0, points1): def add_gcp_to_bundle(ba, gcp, gcp_std, shots): """Add Ground Control Points constraints to the bundle problem.""" for point in gcp: - point_id = 'gcp-' + point.id + point_id = "gcp-" + point.id coordinates = orec.triangulate_gcp(point, shots) if coordinates is None: if point.coordinates is not None: coordinates = point.coordinates else: - logger.warning("Cannot initialize GCP '{}'." - " Ignoring it".format(point.id)) + logger.warning( + "Cannot initialize GCP '{}'." " Ignoring it".format(point.id) + ) continue ba.add_point(point_id, coordinates, False) @@ -165,11 +163,13 @@ def add_gcp_to_bundle(ba, gcp, gcp_std, shots): point_id, observation.projection[0], observation.projection[1], - gcp_std) + gcp_std, + ) -def bundle_with_fixed_images(reconstruction, camera_priors, gcp, gcp_std, - fixed_images, config): +def bundle_with_fixed_images( + reconstruction, camera_priors, gcp, gcp_std, fixed_images, config +): """Bundle adjust a reconstruction while keeping some images fixed.""" fix_cameras = True @@ -195,32 +195,35 @@ def bundle_with_fixed_images(reconstruction, camera_priors, gcp, gcp_std, for point in shot.get_valid_landmarks(): obs = shot.get_landmark_observation(point) ba.add_point_projection_observation( - shot.id, point.id, obs.point[0], obs.point[1], obs.scale) + shot.id, point.id, obs.point[0], obs.point[1], obs.scale + ) add_gcp_to_bundle(ba, gcp, gcp_std, reconstruction.shots) - ba.set_point_projection_loss_function(config['loss_function'], - config['loss_function_threshold']) + ba.set_point_projection_loss_function( + config["loss_function"], config["loss_function_threshold"] + ) ba.set_internal_parameters_prior_sd( - config['exif_focal_sd'], - config['principal_point_sd'], - config['radial_distortion_k1_sd'], - config['radial_distortion_k2_sd'], - config['tangential_distortion_p1_sd'], - config['tangential_distortion_p2_sd'], - config['radial_distortion_k3_sd'], - config['radial_distortion_k4_sd']) - ba.set_num_threads(config['processes']) - ba.set_max_num_iterations(config['bundle_max_iterations']) + config["exif_focal_sd"], + config["principal_point_sd"], + config["radial_distortion_k1_sd"], + config["radial_distortion_k2_sd"], + config["tangential_distortion_p1_sd"], + config["tangential_distortion_p2_sd"], + config["radial_distortion_k3_sd"], + config["radial_distortion_k4_sd"], + ) + ba.set_num_threads(config["processes"]) + ba.set_max_num_iterations(config["bundle_max_iterations"]) ba.set_linear_solver_type("SPARSE_SCHUR") ba.set_compute_covariances(True) - chrono.lap('setup') + chrono.lap("setup") ba.run() - chrono.lap('run') + chrono.lap("run") if not ba.get_covariance_estimation_valid(): - logger.warning('Could not compute covariance') + logger.warning("Could not compute covariance") for camera in reconstruction.cameras.values(): orec._get_camera_from_bundle(ba, camera) @@ -229,21 +232,22 @@ def bundle_with_fixed_images(reconstruction, camera_priors, gcp, gcp_std, s = ba.get_shot(shot.id) shot.pose.rotation = [s.r[0], s.r[1], s.r[2]] shot.pose.translation = [s.t[0], s.t[1], s.t[2]] - shot.covariance = np.array([[s.get_covariance_inv_param(i, j) - for j in range(6)] for i in range(6)]) + shot.covariance = np.array( + [[s.get_covariance_inv_param(i, j) for j in range(6)] for i in range(6)] + ) for point in reconstruction.points.values(): p = ba.get_point(point.id) point.coordinates = [p.p[0], p.p[1], p.p[2]] point.reprojection_errors = p.reprojection_errors - chrono.lap('teardown') + chrono.lap("teardown") logger.info(ba.full_report()) logger.info(ba.brief_report()) report = { - 'wall_times': dict(chrono.lap_times()), - 'brief_report': ba.brief_report(), + "wall_times": dict(chrono.lap_times()), + "brief_report": ba.brief_report(), } return report @@ -334,7 +338,7 @@ def resect_image(im, camera, gcps, reconstruction, data, dst_reconstruction=None R = T[:, :3].T t = -R.dot(T[:, 3]) dst_reconstruction.add_camera(camera) - shot = dst_reconstruction.create_shot(im, camera.id, pygeometry.Pose(R,t)) + shot = dst_reconstruction.create_shot(im, camera.id, pygeometry.Pose(R, t)) shot.metadata = orec.get_image_metadata(data, im) return shot else: @@ -343,27 +347,30 @@ def resect_image(im, camera, gcps, reconstruction, data, dst_reconstruction=None def parse_args(): - parser = argparse.ArgumentParser(description='Merge reconstructions and run BA with GCPs') + parser = argparse.ArgumentParser( + description="Merge reconstructions and run BA with GCPs" + ) parser.add_argument( - 'dataset', - help='dataset to process', + "dataset", + help="dataset to process", ) parser.add_argument( - '--mode', + "--mode", default="3d_to_3d", help="If '3d_to_2d': the largest reconstruction is used as a fixed reference: " - "Images not belonging to it are resected into it using the ground control points. " - "If '3d_to_3d': The two largest reconstructions are aligned to each other using the ground control points." + "Images not belonging to it are resected into it using the GCPs. " + "If '3d_to_3d': The two largest reconstructions are aligned to" + " each other using the ground control points.", ) parser.add_argument( - '--std-threshold', + "--std-threshold", default=0.3, - help='positional threshold (m) above which we consider an image to be badly localized', + help="Positional threshold (m) to classify images as well-localized", ) parser.add_argument( - '--px-threshold', - default=0.016, # a little bit over 10 pixels at VGA resolution - help='threshold in normalized pixels above which we consider a GCP annotation to be wrong', + "--px-threshold", + default=0.016, # a little bit over 10 pixels at VGA resolution + help="threshold in normalized pixels to classify a GCP annotation as correct", ) args = parser.parse_args() assert args.mode in ("3d_to_3d", "3d_to_2d") @@ -374,11 +381,7 @@ def main(): args = parse_args() path = args.dataset data = dataset.DataSet(path) - for fn in ( - 'reconstruction.json', - 'ground_control_points.json', - 'tracks.csv' - ): + for fn in ("reconstruction.json", "ground_control_points.json", "tracks.csv"): if not (os.path.exists(os.path.join(path, fn))): logger.error(f"Missing file: {fn}") return @@ -403,47 +406,54 @@ def main(): base = reconstructions[0] resected = resect_annotated_single_images(base, gcps, camera_models, data) for shot in resected.shots.values(): - shot.metadata.gps_accuracy.value = 1E12 + shot.metadata.gps_accuracy.value = 1e12 shot.metadata.gps_position.value = shot.pose.get_origin() reconstructions = [base, resected] - data.save_reconstruction(reconstructions, 'reconstruction_gcp_rigid.json') + data.save_reconstruction(reconstructions, "reconstruction_gcp_rigid.json") merged = merge_reconstructions(reconstructions, tracks_manager) - data.save_reconstruction([merged], 'reconstruction_merged.json') + data.save_reconstruction([merged], "reconstruction_merged.json") - data.config['bundle_max_iterations'] = 200 - data.config['bundle_use_gcp'] = True + data.config["bundle_max_iterations"] = 200 + data.config["bundle_use_gcp"] = True orec.bundle(merged, camera_models, gcp=gcps, config=data.config) # rigid rotation to put images on the ground orec.align_reconstruction(merged, None, data.config) - data.save_reconstruction([merged], 'reconstruction_gcp_ba.json') + data.save_reconstruction([merged], "reconstruction_gcp_ba.json") gcp_reprojections = reproject_gcps(gcps, merged) reprojection_errors = get_all_reprojection_errors(gcp_reprojections) err_values = [t[2] for t in reprojection_errors] - mean_reprojection_error = np.mean(err_values) max_reprojection_error = np.max(err_values) median_reprojection_error = np.median(err_values) - with open(data.data_path + '/gcp_reprojections.json', 'w') as f: + with open(data.data_path + "/gcp_reprojections.json", "w") as f: json.dump(gcp_reprojections, f, indent=4, sort_keys=True) gcp_std = compute_gcp_std(gcp_reprojections) logger.info(f"GCP reprojection error STD: {gcp_std}") resplit = resplit_reconstruction(merged, reconstructions) - data.save_reconstruction(resplit, 'reconstruction_gcp_ba_resplit.json') + data.save_reconstruction(resplit, "reconstruction_gcp_ba_resplit.json") all_shots_std = [] # We run bundle by fixing one reconstruction. # If we have two reconstructions, we do this twice, fixing each one. - _rec_ixs = [(0,1), (1,0)] if args.mode == "3d_to_3d" else [(0,1)] + _rec_ixs = [(0, 1), (1, 0)] if args.mode == "3d_to_3d" else [(0, 1)] for rec_ixs in _rec_ixs: fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) - bundle_with_fixed_images(merged, camera_models, gcp=gcps, gcp_std=gcp_std, - fixed_images=fixed_images, config=data.config) + bundle_with_fixed_images( + merged, + camera_models, + gcp=gcps, + gcp_std=gcp_std, + fixed_images=fixed_images, + config=data.config, + ) - logger.info(f"STD in the position of shots in rec {rec_ixs[1]} w.r.t rec {rec_ixs[0]}") + logger.info( + f"STD in the position of shots in rec {rec_ixs[1]} w.r.t rec {rec_ixs[0]}" + ) for shot in merged.shots.values(): if shot.id in reconstructions[rec_ixs[1]].shots: u, std = decompose_covariance(shot.covariance[3:, 3:]) @@ -459,34 +469,34 @@ def main(): n_bad_std = len(std_values) # Average positional STD - average_shot_std = np.mean([t[1] for t in all_shots_std]) median_shot_std = np.median([t[1] for t in all_shots_std]) # Save the shot STD to a file - with open(data.data_path + '/shots_std.csv', 'w') as f: - s = sorted(all_shots_std, key=lambda t:-t[-1]) + with open(data.data_path + "/shots_std.csv", "w") as f: + s = sorted(all_shots_std, key=lambda t: -t[-1]) for t in s: line = "{}, {}".format(*t) - f.write(line + '\n') - + f.write(line + "\n") - n_bad_gcp_annotations = int(sum(t[2] > args.px_threshold for t in reprojection_errors)) + n_bad_gcp_annotations = int( + sum(t[2] > args.px_threshold for t in reprojection_errors) + ) metrics = { - 'n_reconstructions': len(all_reconstructions), - 'median_shot_std' : median_shot_std, - 'max_shot_std': s[0][1], - 'max_reprojection_error': max_reprojection_error, - 'median_reprojection_error': median_reprojection_error, - 'n_gcp': len(gcps), - 'n_bad_gcp_annotations': n_bad_gcp_annotations, - 'n_bad_position_std': int(n_bad_std), - 'n_good_position_std': int(n_good_std), + "n_reconstructions": len(all_reconstructions), + "median_shot_std": median_shot_std, + "max_shot_std": s[0][1], + "max_reprojection_error": max_reprojection_error, + "median_reprojection_error": median_reprojection_error, + "n_gcp": len(gcps), + "n_bad_gcp_annotations": n_bad_gcp_annotations, + "n_bad_position_std": int(n_bad_std), + "n_good_position_std": int(n_good_std), } logger.info(metrics) - p_metrics = data.data_path + '/run_ba_metrics.json' + p_metrics = data.data_path + "/run_ba_metrics.json" logger.info(f"Saved metrics to {p_metrics}") - with open(p_metrics, 'w') as f: + with open(p_metrics, "w") as f: json.dump(metrics, f, indent=4, sort_keys=True) logger.info("========================================") @@ -500,17 +510,31 @@ def main(): else: if np.isnan(all_shots_std[0][1]): logger.info( - f"Could not get positional uncertainty. It could be because: a) there are not enough GCPs." - " b) they are badly distributed in 3D. c) there are some wrong annotations" + "Could not get positional uncertainty. It could be because:" + "\na) there are not enough GCPs." + "\nb) they are badly distributed in 3D." + "\nc) there are some wrong annotations" ) else: - logger.info(f"{n_bad_std} badly localized images (error>{args.std_threshold}). Use the frame list on each view to find these") - logger.info(f"{n_bad_gcp_annotations} annotations with large reprojection error. Press Q to jump to the worst, correct it and repeat. Here are the top 5:") + logger.info( + f"{n_bad_std} badly localized images (error>{args.std_threshold})." + " Use the frame list on each view to find these" + ) + logger.info( + f"{n_bad_gcp_annotations} annotations with large reprojection error." + " Press Q to jump to the worst, correct it and repeat. Here are the top 5:" + ) - stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp(gcp_reprojections, args.px_threshold) - gcps_sorted = sorted(stats_bad_reprojections, key=lambda k:-stats_bad_reprojections[k])[:5] + stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp( + gcp_reprojections, args.px_threshold + ) + gcps_sorted = sorted( + stats_bad_reprojections, key=lambda k: -stats_bad_reprojections[k] + )[:5] for ix, gcp_id in enumerate(gcps_sorted): - logger.info(f"#{ix+1}: GCP: {gcp_id} with {stats_bad_reprojections[gcp_id]} bad annotations") + logger.info( + f"#{ix+1} - {gcp_id}: {stats_bad_reprojections[gcp_id]} bad annotations" + ) if __name__ == "__main__": From 21f00f66dcec75dd871ea622749bd2a3ee6d1f36 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 20 Oct 2020 13:40:08 +0200 Subject: [PATCH 26/64] feat: Allow picking the reconstructions to analyze - run_ba.py takes the indices of the two reconstructions to analyze. If no index is passed for the second reconstruction, it runs 3d-to-2d analysis by resecting annotated images. - The UI has been updated with dropdown menus to select the reconstructions accordingly --- annotation_gui_gcp/GUI.py | 85 ++++++++++++++++++++++++------------ annotation_gui_gcp/run_ba.py | 40 +++++++++-------- 2 files changed, 80 insertions(+), 45 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 1db57e95c..f582736aa 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -4,8 +4,10 @@ import sys import time import tkinter as tk +from collections import defaultdict import matplotlib +from opensfm import dataset matplotlib.use("TkAgg") @@ -32,6 +34,7 @@ def __init__( master.bind_all("z", lambda event: self.toggle_zoom_all_views()) master.bind_all("x", lambda event: self.toggle_sticky_zoom()) master.bind_all("a", lambda event: self.go_to_current_gcp()) + self.get_reconstruction_options() self.create_ui(ortho_paths) master.lift() @@ -42,6 +45,23 @@ def __init__( if os.path.exists(p_shot_std): self.load_shot_std(p_shot_std) + def get_reconstruction_options(self): + p_recs = self.path + "/reconstruction.json" + print(p_recs) + if not os.path.exists(p_recs): + return {} + data = dataset.DataSet(self.path) + recs = data.load_reconstruction() + options = [] + for ix, rec in enumerate(recs): + camcount = defaultdict(int) + for shot in rec.shots.values(): + camcount[shot.camera.id] += 1 + str_repr = f"#{ix}: " + ", ".join(f"{k}({v})" for k, v in camcount.items()) + options.append(str_repr) + options.append("None (3d-to-2d)") + self.reconstruction_options = options + def create_ui(self, ortho_paths): tools_frame = tk.Frame(self.master) tools_frame.pack(side="left", expand=0, fill=tk.Y) @@ -74,6 +94,7 @@ def arrange_ui_onerow(self): x += w def create_tools(self, master): + width = 15 gcp_list_frame = tk.Frame(master) gcp_list_frame.pack(side="top", fill=tk.BOTH, expand=1) @@ -81,7 +102,7 @@ def create_tools(self, master): self.gcp_list_box = tk.Listbox( gcp_list_frame, font=FONT, - width=12, + width=width, selectmode="browse", listvariable=self.gcp_list, ) @@ -109,14 +130,22 @@ def create_tools(self, master): txt.pack(side="top") analysis_frame = tk.Frame(master) analysis_frame.pack(side="top") - button = tk.Button( - analysis_frame, text="3d-to-3d", command=self.analyze_3d_to_3d - ) - button.pack(side="left") - button = tk.Button( - analysis_frame, text="3d-to-2d", command=self.analyze_3d_to_2d - ) - button.pack(side="left") + + options = self.reconstruction_options + self.rec_a = tk.StringVar(master) + self.rec_a.set(options[0]) + w = tk.OptionMenu(analysis_frame, self.rec_a, *options[:-1]) + w.pack(side="top", fill=tk.X) + w.config(width=width) + + self.rec_b = tk.StringVar(master) + self.rec_b.set(options[1]) + w = tk.OptionMenu(analysis_frame, self.rec_b, *options) + w.pack(side="top", fill=tk.X) + w.config(width=width) + + button = tk.Button(analysis_frame, text="Analyze", command=self.analyze) + button.pack(side="top") io_frame = tk.Frame(master) io_frame.pack(side="top") @@ -144,29 +173,29 @@ def create_sequence_views(self, show_ortho_track): v = ImageSequenceView(self, sequence_key, image_keys, show_ortho_track) self.sequence_views.append(v) - def analyze_3d_to_3d(self): - self.analyze(mode="3d_to_3d") - - def analyze_3d_to_2d(self): - self.analyze(mode="3d_to_2d") - - def analyze(self, mode): - # Check that there is a recent ground_control_points.json file + def analyze(self): t = time.time() - os.path.getmtime(self.path + "/ground_control_points.json") - if t > 30: - print("Please save before running the analysis") + ix_a = self.reconstruction_options.index(self.rec_a.get()) + ix_b = self.reconstruction_options.index(self.rec_b.get()) + if t > 30 or ix_a == ix_b: + print( + "Please select different reconstructions in the drop-down menus" + " and save before running the analysis" + ) return + args = [ + sys.executable, + os.path.dirname(__file__) + "/run_ba.py", + self.path, + "--rec_a", + str(ix_a), + ] + if ix_b < len(self.reconstruction_options) - 1: + args.extend(("--rec_b", str(ix_b))) + # Call the run_ba script - subprocess.run( - [ - sys.executable, - os.path.dirname(__file__) + "/run_ba.py", - self.path, - "--mode", - mode, - ] - ) + subprocess.run(args) self.shot_std = {} self.load_shot_std(self.path + "/shots_std.csv") p_gcp_errors = self.path + "/gcp_reprojections.json" diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 004187587..e988a35f3 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -355,12 +355,20 @@ def parse_args(): help="dataset to process", ) parser.add_argument( - "--mode", - default="3d_to_3d", - help="If '3d_to_2d': the largest reconstruction is used as a fixed reference: " - "Images not belonging to it are resected into it using the GCPs. " - "If '3d_to_3d': The two largest reconstructions are aligned to" - " each other using the ground control points.", + "--rec_a", + default=0, + type=int, + help="Index of reconstruction A." + "\nIf rec_b is set to None, the rec_a is used as a fixed reference:" + " all annotated images not belonging to it are resected into it using the GCPs." + "\nIf reconstruction B is set to a number, the pair of reconstructions (A,B)" + " will be aligned to each other using the ground control points.", + ) + parser.add_argument( + "--rec_b", + default=None, + type=int, + help="Index of reconstruction B. Read the help for rec_a", ) parser.add_argument( "--std-threshold", @@ -373,7 +381,6 @@ def parse_args(): help="threshold in normalized pixels to classify a GCP annotation as correct", ) args = parser.parse_args() - assert args.mode in ("3d_to_3d", "3d_to_2d") return args @@ -389,21 +396,20 @@ def main(): camera_models = data.load_camera_models() tracks_manager = data.load_tracks_manager() - all_reconstructions = data.load_reconstruction() - - if len(all_reconstructions) > 2: - logger.warning(f"There are more than two reconstructions in {path}") - - reconstructions = all_reconstructions[:2] + reconstructions = data.load_reconstruction() + n_reconstructions = len(reconstructions) gcps = data.load_ground_control_points() - if args.mode == "3d_to_3d": + assert args.rec_a != args.rec_b, "rec_a and rec_b should be different" + + if args.rec_b: + reconstructions = [reconstructions[args.rec_a], reconstructions[args.rec_b]] coords0 = triangulate_gcps(gcps, reconstructions[0]) coords1 = triangulate_gcps(gcps, reconstructions[1]) s, A, b = find_alignment(coords1, coords0) align.apply_similarity(reconstructions[1], s, A, b) else: - base = reconstructions[0] + base = reconstructions[args.rec_a] resected = resect_annotated_single_images(base, gcps, camera_models, data) for shot in resected.shots.values(): shot.metadata.gps_accuracy.value = 1e12 @@ -439,7 +445,7 @@ def main(): all_shots_std = [] # We run bundle by fixing one reconstruction. # If we have two reconstructions, we do this twice, fixing each one. - _rec_ixs = [(0, 1), (1, 0)] if args.mode == "3d_to_3d" else [(0, 1)] + _rec_ixs = [(0, 1), (1, 0)] if args.rec_b else [(0, 1)] for rec_ixs in _rec_ixs: fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) bundle_with_fixed_images( @@ -482,7 +488,7 @@ def main(): sum(t[2] > args.px_threshold for t in reprojection_errors) ) metrics = { - "n_reconstructions": len(all_reconstructions), + "n_reconstructions": n_reconstructions, "median_shot_std": median_shot_std, "max_shot_std": s[0][1], "max_reprojection_error": max_reprojection_error, From 444b6705759128457807fba38a88b19c6e899171 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 20 Oct 2020 18:23:53 +0200 Subject: [PATCH 27/64] chore: docs and output text --- annotation_gui_gcp/main.py | 6 +----- annotation_gui_gcp/run_ba.py | 10 +++++----- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 190b226f3..6cfd388e6 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -46,11 +46,7 @@ def parse_args(): "--ortho", type=str, action="append", - help="Specify one or more groups of linked sequences. " - "Linked sequences are synchronized such that all views " - "from the group will always show the same frame index. " - "Useful for camera rigs. usage: -g sequence_key_1 sequence_key_2. " - "Can be used multiple times to define several groups", + help="Specify one or more directories containing geotiffs", default=[], ) return parser.parse_args() diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index e988a35f3..c06947078 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -147,7 +147,7 @@ def add_gcp_to_bundle(ba, gcp, gcp_std, shots): coordinates = orec.triangulate_gcp(point, shots) if coordinates is None: if point.coordinates is not None: - coordinates = point.coordinates + coordinates = point.coordinates.value else: logger.warning( "Cannot initialize GCP '{}'." " Ignoring it".format(point.id) @@ -528,7 +528,7 @@ def main(): ) logger.info( f"{n_bad_gcp_annotations} annotations with large reprojection error." - " Press Q to jump to the worst, correct it and repeat. Here are the top 5:" + " Press Q to jump to the worst, correct it and repeat. Worst offenders:" ) stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp( @@ -538,9 +538,9 @@ def main(): stats_bad_reprojections, key=lambda k: -stats_bad_reprojections[k] )[:5] for ix, gcp_id in enumerate(gcps_sorted): - logger.info( - f"#{ix+1} - {gcp_id}: {stats_bad_reprojections[gcp_id]} bad annotations" - ) + n = stats_bad_reprojections[gcp_id] + if n > 0: + logger.info(f"#{ix+1} - {gcp_id}: {n} bad annotations") if __name__ == "__main__": From 282dacc72f9351886323596358e25d5aedf594d1 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 21 Oct 2020 15:08:46 +0200 Subject: [PATCH 28/64] chore: moved pixel rescaling to parent View --- annotation_gui_gcp/image_sequence_view.py | 24 --------------------- annotation_gui_gcp/view.py | 26 +++++++++++++++++++++++ 2 files changed, 26 insertions(+), 24 deletions(-) diff --git a/annotation_gui_gcp/image_sequence_view.py b/annotation_gui_gcp/image_sequence_view.py index 067c34e05..5f5905507 100644 --- a/annotation_gui_gcp/image_sequence_view.py +++ b/annotation_gui_gcp/image_sequence_view.py @@ -1,10 +1,8 @@ import tkinter as tk -from typing import Tuple import matplotlib import numpy as np from matplotlib import pyplot as plt -from opensfm import features from geometry import get_all_track_observations, get_tracks_visible_in_image from view import View @@ -96,28 +94,6 @@ def auto_gcp_create(self, x, y, add): def pixel_to_latlon(self, x: float, y: float): return None - def gcp_to_pixel_coordinates(self, x: float, y: float) -> Tuple[float, float]: - """ - Transforms from normalized coordinates to pixels - - The view displays images at a reduced resolution for speed. We use the image - manager to obtain the reduced coordinates to use for de-normalization. - """ - h, w = self.image_manager.get_image_size(self.current_image) - px = features.denormalized_image_coordinates(np.array([[x, y]]), w, h)[0] - return px.tolist() - - def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: - """ - Transforms from pixels to normalized coordinates - - The view displays images at a reduced resolution for speed. We use the image - manager to obtain the reduced coordinates to use for normalization. - """ - h, w = self.image_manager.get_image_size(self.current_image) - coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] - return coords.tolist() - def set_title(self): shot = self.current_image seq_ix = self.images_in_list.index(shot) diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py index 1549d9d16..ed79f78eb 100644 --- a/annotation_gui_gcp/view.py +++ b/annotation_gui_gcp/view.py @@ -3,11 +3,15 @@ import matplotlib import matplotlib.patches as mpatches +import numpy as np matplotlib.use("TkAgg") +from typing import Tuple + from matplotlib import patheffects from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg from matplotlib.figure import Figure +from opensfm import features FONT = "TkFixedFont" @@ -322,6 +326,28 @@ def highlight_gcp_reprojection(self, point_id, zoom=True): self.zoom_in(x, y) self.canvas.draw_idle() + def gcp_to_pixel_coordinates(self, x: float, y: float) -> Tuple[float, float]: + """ + Transforms from normalized coordinates to pixels + + The view displays images at a reduced resolution for speed. We use the image + manager to obtain the reduced coordinates to use for de-normalization. + """ + h, w = self.image_manager.get_image_size(self.current_image) + px = features.denormalized_image_coordinates(np.array([[x, y]]), w, h)[0] + return px.tolist() + + def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: + """ + Transforms from pixels to normalized coordinates + + The view displays images at a reduced resolution for speed. We use the image + manager to obtain the reduced coordinates to use for normalization. + """ + h, w = self.image_manager.get_image_size(self.current_image) + coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] + return coords.tolist() + def go_to_next_image(self): self.go_to_adjacent_image(+1) From 35b822144ecc04acb99f03069e6d4f233cf39bd5 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 6 Nov 2020 18:06:18 +0100 Subject: [PATCH 29/64] fix: convert auto-gcps from numpy to list --- annotation_gui_gcp/image_sequence_view.py | 1 + annotation_gui_gcp/orthophoto_manager.py | 1 + annotation_gui_gcp/run_ba.py | 2 +- annotation_gui_gcp/view.py | 1 + 4 files changed, 4 insertions(+), 1 deletion(-) diff --git a/annotation_gui_gcp/image_sequence_view.py b/annotation_gui_gcp/image_sequence_view.py index 5f5905507..804e28403 100644 --- a/annotation_gui_gcp/image_sequence_view.py +++ b/annotation_gui_gcp/image_sequence_view.py @@ -84,6 +84,7 @@ def auto_gcp_create(self, x, y, add): new_gcp = self.main_ui.add_gcp() print(f"New GCP {new_gcp} with {len(observations)} observations") for shot_id, point in observations.items(): + point = point.tolist() self.main_ui.gcp_manager.add_point_observation(new_gcp, shot_id, point) self.visible_tracks = None diff --git a/annotation_gui_gcp/orthophoto_manager.py b/annotation_gui_gcp/orthophoto_manager.py index 23380584b..ad4a45f51 100644 --- a/annotation_gui_gcp/orthophoto_manager.py +++ b/annotation_gui_gcp/orthophoto_manager.py @@ -44,6 +44,7 @@ def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float return tw, window, t def get_candidate_images(self, lat: float, lon: float): + # lat, lon -> nearmap file id + nearmap pixel coordinates return [k for k in self.image_keys if self.check_latlon_covered(k, lat, lon)] def get_image(self, img, lat: float, lon: float, size: float): diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index c06947078..dceb895bb 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -268,7 +268,7 @@ def resect_annotated_single_images(reconstruction, gcps, camera_models, data): for gcp in gcps: for obs in gcp.observations: im = obs.shot_id - if im not in reconstruction.shots: + if im not in reconstruction.shots and im in data.images(): not_in_rec.add(im) resected = types.Reconstruction() diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py index ed79f78eb..f75c16d45 100644 --- a/annotation_gui_gcp/view.py +++ b/annotation_gui_gcp/view.py @@ -11,6 +11,7 @@ from matplotlib import patheffects from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg from matplotlib.figure import Figure +from matplotlib.transforms import Affine2D from opensfm import features FONT = "TkFixedFont" From 813e0facda1e7e52cb9e82056287f8d0a7926669 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 11 Nov 2020 13:52:24 +0100 Subject: [PATCH 30/64] feat: split same-reconstruction sequences in views Before this commit, if --group-by-reconstruction was set, images would be grouped attending only to the reconstruction that they end up in. This caused rig images to be put into the same view which was uncomfortable for annotation. Now, if group-by-reconstruction is set and sequence-group has sequences in it that should be grouped (e.g. rig sequences), we ensure that they end up in different views. --- annotation_gui_gcp/GUI.py | 4 +- annotation_gui_gcp/main.py | 102 +++++++++++++++++++++++++++++-------- 2 files changed, 85 insertions(+), 21 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index f582736aa..4f6f0d16b 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -57,7 +57,9 @@ def get_reconstruction_options(self): camcount = defaultdict(int) for shot in rec.shots.values(): camcount[shot.camera.id] += 1 - str_repr = f"#{ix}: " + ", ".join(f"{k}({v})" for k, v in camcount.items()) + str_repr = f"REC#{ix}: " + ", ".join( + f"{k}({v})" for k, v in camcount.items() + ) options.append(str_repr) options.append("None (3d-to-2d)") self.reconstruction_options = options diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 6cfd388e6..eeb473a24 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -22,6 +22,13 @@ def parse_args(): help="If set, the UI will show one window per reconstruction, " "otherwise, it will use sequences as specified by 'sequence-file'", ) + parser.add_argument( + "--min-images-in-reconstruction", + type=int, + default=50, + help="Reconstructions with fewer images than this value will not be " + "displayed in the UI", + ) parser.add_argument( "--sequence-file", help="dict-of-image-keys JSON file describing each sequence. " @@ -38,7 +45,9 @@ def parse_args(): "Linked sequences are synchronized such that all views " "from the group will always show the same frame index. " "Useful for camera rigs. usage: -g sequence_key_1 sequence_key_2. " - "Can be used multiple times to define several groups", + "Can be used multiple times to define several groups. " + "Groups defined here will be split into different views even if they " + "belong to the same reconstruction. ", default=[], ) parser.add_argument( @@ -77,7 +86,8 @@ def load_sequence_database_from_file( """ root = Path(root) p_json = root / fname - assert p_json.exists() + if not p_json.exists(): + return None seq_dict = OrderedDict(io.json_load(open(p_json, "r"))) available_images = file_sanity_check(root, seq_dict, fname) @@ -93,34 +103,86 @@ def load_sequence_database_from_file( return seq_dict -def group_images_by_reconstruction(path): +def load_shots_from_reconstructions(path, min_ims): data = dataset.DataSet(path) reconstructions = data.load_reconstruction() - seq_dict = defaultdict(list) - for recons in reconstructions: - for shot in recons.shots.values(): - skey = shot.metadata.sequence_key.value - seq_dict[skey].append(shot) - for skey in seq_dict: - sorted_ims = sorted(seq_dict[skey], key=lambda x: x.metadata.capture_time.value) - seq_dict[skey] = [shot.id for shot in sorted_ims] - return seq_dict + + # Replace sequence keys for those in sequence_database.json + + n_recs = len(reconstructions) + if len(reconstructions) > 2: + reconstructions = [r for r in reconstructions if len(r) >= min_ims] + if len(reconstructions) < n_recs: + print( + "Kept {}/{} reconstructions (min images: {})".format( + len(reconstructions), + n_recs, + min_ims, + ) + ) + + output = [] + for rec in reconstructions: + shots = sorted(rec.shots.values(), key=lambda x: x.metadata.capture_time.value) + output.append([shot.id for shot in shots]) + return output + + +def group_by_reconstruction(args, groups_from_sequence_database): + all_recs_shots = load_shots_from_reconstructions(path, min_ims=50) + grouped_skeys = [skey for g in args.sequence_group for skey in g] + + map_key_to_skey = {} + for skey, keys in groups_from_sequence_database.items(): + for k in keys: + map_key_to_skey[k] = skey + + groups = defaultdict(list) + sequence_groups = [] + for ix_rec, rec_shots in enumerate(all_recs_shots): + sequence_groups.append( + [f"REC#{ix_rec}_{skey}" for group in args.sequence_group for skey in group] + ) + for key in rec_shots: + skey = map_key_to_skey[key] + if skey in grouped_skeys: + # If this sequence is in any of the sequence groups, we want to + # re-split each reconstruction into different views if they belong + # to a sequence group. (Typical case: a rig of cameras) + group_key = f"REC#{ix_rec}_{skey}" + else: + group_key = f"REC#{ix_rec}" + groups[group_key].append(key) + + return groups, sequence_groups + + +def group_images(args): + groups_from_sequence_database = load_sequence_database_from_file( + args.dataset, args.sequence_file, skip_missing=True + ) + if args.group_by_reconstruction: + return group_by_reconstruction(args, groups_from_sequence_database) + else: + # We only group by sequence key + if groups_from_sequence_database is None: + print( + f"No sequence database file at {args.sequence_file}" + " and --group-by-reconstruction is disabled. Quitting" + ) + exit() + return groups_from_sequence_database, args.sequence_group if __name__ == "__main__": args = parse_args() path = args.dataset - if args.group_by_reconstruction: - seqs = group_images_by_reconstruction(path) - else: - seqs = load_sequence_database_from_file( - path, args.sequence_file, skip_missing=True - ) - image_manager = ImageManager(seqs, path, preload_images=not args.no_preload) + groups, sequence_groups = group_images(args) + image_manager = ImageManager(groups, path, preload_images=not args.no_preload) gcp_manager = GroundControlPointManager(path) root = tk.Tk() root.resizable(True, True) - ui = GUI.Gui(root, gcp_manager, image_manager, args.sequence_group, args.ortho) + ui = GUI.Gui(root, gcp_manager, image_manager, sequence_groups, args.ortho) root.grid_columnconfigure(0, weight=1) root.grid_rowconfigure(0, weight=1) root.title("Tools") From d0eadddc3713f0d25b24d2f87d9088ed0988af34 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 18 Nov 2020 10:40:58 +0100 Subject: [PATCH 31/64] fix: crash if groups_from_sequence_database={} --- annotation_gui_gcp/main.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index eeb473a24..0584e2653 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -133,9 +133,10 @@ def group_by_reconstruction(args, groups_from_sequence_database): grouped_skeys = [skey for g in args.sequence_group for skey in g] map_key_to_skey = {} - for skey, keys in groups_from_sequence_database.items(): - for k in keys: - map_key_to_skey[k] = skey + if groups_from_sequence_database: + for skey, keys in groups_from_sequence_database.items(): + for k in keys: + map_key_to_skey[k] = skey groups = defaultdict(list) sequence_groups = [] @@ -144,7 +145,7 @@ def group_by_reconstruction(args, groups_from_sequence_database): [f"REC#{ix_rec}_{skey}" for group in args.sequence_group for skey in group] ) for key in rec_shots: - skey = map_key_to_skey[key] + skey = map_key_to_skey.get(key) if skey in grouped_skeys: # If this sequence is in any of the sequence groups, we want to # re-split each reconstruction into different views if they belong From 532d8b5a1e96c3c8727897bbfee1134b137e13fd Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 20 Nov 2020 15:15:41 +0100 Subject: [PATCH 32/64] feat: Images can be rotated in the UI --- annotation_gui_gcp/image_sequence_view.py | 11 +++++++++++ annotation_gui_gcp/view.py | 20 +++++++++++++++++--- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/annotation_gui_gcp/image_sequence_view.py b/annotation_gui_gcp/image_sequence_view.py index 804e28403..7988a4a3c 100644 --- a/annotation_gui_gcp/image_sequence_view.py +++ b/annotation_gui_gcp/image_sequence_view.py @@ -24,6 +24,13 @@ def __init__(self, main_ui, sequence_key, image_keys, show_ortho_track): ) auto_gcp_button.pack(side="top") + rotate_button = tk.Button( + self.toolbox, + text="Rotate", + command=lambda window=self.window: self.rotate(), + ) + rotate_button.pack(side="top") + self.populate_image_list() self.bring_new_image(self.images_in_list[0]) self.set_title() @@ -34,6 +41,10 @@ def get_image(self, new_image): def get_candidate_images(self): return self.images_in_list + def rotate(self): + self.rotation = (self.rotation + 1) % 4 + self.bring_new_image(self.current_image, force=True) + def auto_gcp_show_tracks(self): h, w = self.image_manager.get_image_size(self.current_image) tracks = get_tracks_visible_in_image( diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py index f75c16d45..59f0dc0b1 100644 --- a/annotation_gui_gcp/view.py +++ b/annotation_gui_gcp/view.py @@ -11,7 +11,6 @@ from matplotlib import patheffects from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg from matplotlib.figure import Figure -from matplotlib.transforms import Affine2D from opensfm import features FONT = "TkFixedFont" @@ -53,6 +52,7 @@ def __init__(self, main_ui, show_ortho_track): window = tk.Toplevel(self.main_ui.master) self.window = window self.current_image = None + self.rotation = 0 canvas_frame = tk.Frame(window) canvas_frame.pack(side="left", fill=tk.BOTH, expand=1) @@ -279,6 +279,7 @@ def bring_new_image(self, new_image, force=False): self.current_image = new_image self.ax.clear() img = self.get_image(new_image) + img = np.rot90(img, k=-self.rotation) self.ax.imshow(img) self.ax.axis("on") self.ax.axis("scaled") @@ -327,6 +328,18 @@ def highlight_gcp_reprojection(self, point_id, zoom=True): self.zoom_in(x, y) self.canvas.draw_idle() + def rotate_point(self, x, y, h, w, reverse): + if self.rotation == 0: + return (x, y) + elif self.rotation == 1: + return (y, h - x) if reverse else (h - y, x) + elif self.rotation == 2: + return (w - x, h - y) + elif self.rotation == 3: + return (w - y, x) if reverse else (y, w - x) + else: + raise ValueError + def gcp_to_pixel_coordinates(self, x: float, y: float) -> Tuple[float, float]: """ Transforms from normalized coordinates to pixels @@ -336,7 +349,7 @@ def gcp_to_pixel_coordinates(self, x: float, y: float) -> Tuple[float, float]: """ h, w = self.image_manager.get_image_size(self.current_image) px = features.denormalized_image_coordinates(np.array([[x, y]]), w, h)[0] - return px.tolist() + return self.rotate_point(px[0], px[1], h, w, reverse=False) def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: """ @@ -346,7 +359,8 @@ def pixel_to_gcp_coordinates(self, x: float, y: float) -> Tuple[float, float]: manager to obtain the reduced coordinates to use for normalization. """ h, w = self.image_manager.get_image_size(self.current_image) - coords = features.normalized_image_coordinates(np.array([[x, y]]), w, h)[0] + point = self.rotate_point(x, y, h, w, reverse=True) + coords = features.normalized_image_coordinates(np.array([point]), w, h)[0] return coords.tolist() def go_to_next_image(self): From 9c1abc63dc488a6dec2f7f02ef8f684d58f54cb2 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 30 Nov 2020 16:42:50 +0100 Subject: [PATCH 33/64] feat: arg for strict checking of missing files --- annotation_gui_gcp/main.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 0584e2653..6fa3f7d1e 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -22,6 +22,10 @@ def parse_args(): help="If set, the UI will show one window per reconstruction, " "otherwise, it will use sequences as specified by 'sequence-file'", ) + parser.add_argument( + "--strict-missing", + action="store_true", + ) parser.add_argument( "--min-images-in-reconstruction", type=int, @@ -111,7 +115,7 @@ def load_shots_from_reconstructions(path, min_ims): n_recs = len(reconstructions) if len(reconstructions) > 2: - reconstructions = [r for r in reconstructions if len(r) >= min_ims] + reconstructions = [r for r in reconstructions if len(r.shots) >= min_ims] if len(reconstructions) < n_recs: print( "Kept {}/{} reconstructions (min images: {})".format( @@ -160,7 +164,7 @@ def group_by_reconstruction(args, groups_from_sequence_database): def group_images(args): groups_from_sequence_database = load_sequence_database_from_file( - args.dataset, args.sequence_file, skip_missing=True + args.dataset, args.sequence_file, skip_missing=not args.strict_missing, ) if args.group_by_reconstruction: return group_by_reconstruction(args, groups_from_sequence_database) From 6495c76ef65803b4e346c431442c8bad97c7b74c Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 1 Dec 2020 09:19:58 +0100 Subject: [PATCH 34/64] fix: sort by filename as well This sorts out out-of-order frames when the capture time resolution is insufficient. --- annotation_gui_gcp/main.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 6fa3f7d1e..74a486c31 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -127,7 +127,9 @@ def load_shots_from_reconstructions(path, min_ims): output = [] for rec in reconstructions: - shots = sorted(rec.shots.values(), key=lambda x: x.metadata.capture_time.value) + shots = sorted( + rec.shots.values(), key=lambda x: (x.metadata.capture_time.value, x.id) + ) output.append([shot.id for shot in shots]) return output @@ -164,7 +166,9 @@ def group_by_reconstruction(args, groups_from_sequence_database): def group_images(args): groups_from_sequence_database = load_sequence_database_from_file( - args.dataset, args.sequence_file, skip_missing=not args.strict_missing, + args.dataset, + args.sequence_file, + skip_missing=not args.strict_missing, ) if args.group_by_reconstruction: return group_by_reconstruction(args, groups_from_sequence_database) From e0ad03635bec4c4afea697d6ef260922ef93215c Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 2 Dec 2020 14:48:26 +0100 Subject: [PATCH 35/64] fix: load replaces instead of merging --- annotation_gui_gcp/gcp_manager.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/annotation_gui_gcp/gcp_manager.py b/annotation_gui_gcp/gcp_manager.py index e739e89a1..b45a8280f 100644 --- a/annotation_gui_gcp/gcp_manager.py +++ b/annotation_gui_gcp/gcp_manager.py @@ -56,6 +56,8 @@ def load_gcp_reprojections(self, filename): self.gcp_reprojections = json.load(f) def load_from_file(self, file_path): + self.points = OrderedDict() + self.latlons = {} with open(file_path, "r") as f: input_points = json.load(f)["points"] for point in input_points: From 1003051743ba6099d020494e2e5646fbcfac1fa9 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 2 Dec 2020 14:48:50 +0100 Subject: [PATCH 36/64] feat: fast analysis mode (skip BA) --- annotation_gui_gcp/GUI.py | 17 ++++- annotation_gui_gcp/run_ba.py | 138 +++++++++++++++++++++-------------- 2 files changed, 97 insertions(+), 58 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 4f6f0d16b..314504f75 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -146,8 +146,14 @@ def create_tools(self, master): w.pack(side="top", fill=tk.X) w.config(width=width) - button = tk.Button(analysis_frame, text="Analyze", command=self.analyze) - button.pack(side="top") + analysis_buttons_frame = tk.Frame(analysis_frame) + analysis_buttons_frame.pack(side="top") + button = tk.Button( + analysis_buttons_frame, text="Fast", command=self.analyze_fast + ) + button.pack(side="left") + button = tk.Button(analysis_buttons_frame, text="Full", command=self.analyze) + button.pack(side="right") io_frame = tk.Frame(master) io_frame.pack(side="top") @@ -175,7 +181,10 @@ def create_sequence_views(self, show_ortho_track): v = ImageSequenceView(self, sequence_key, image_keys, show_ortho_track) self.sequence_views.append(v) - def analyze(self): + def analyze_fast(self): + self.analyze(fast=True) + + def analyze(self, fast=False): t = time.time() - os.path.getmtime(self.path + "/ground_control_points.json") ix_a = self.reconstruction_options.index(self.rec_a.get()) ix_b = self.reconstruction_options.index(self.rec_b.get()) @@ -195,6 +204,8 @@ def analyze(self): ] if ix_b < len(self.reconstruction_options) - 1: args.extend(("--rec_b", str(ix_b))) + if fast: + args.extend(["--fast"]) # Call the run_ba script subprocess.run(args) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index dceb895bb..947fa6257 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -380,6 +380,11 @@ def parse_args(): default=0.016, # a little bit over 10 pixels at VGA resolution help="threshold in normalized pixels to classify a GCP annotation as correct", ) + parser.add_argument( + "--fast", + action="store_true", + help="Skip BA", + ) args = parser.parse_args() return args @@ -415,17 +420,19 @@ def main(): shot.metadata.gps_accuracy.value = 1e12 shot.metadata.gps_position.value = shot.pose.get_origin() reconstructions = [base, resected] - data.save_reconstruction(reconstructions, "reconstruction_gcp_rigid.json") + data.save_reconstruction(reconstructions, "reconstruction_gcp_rigid.json") merged = merge_reconstructions(reconstructions, tracks_manager) - data.save_reconstruction([merged], "reconstruction_merged.json") + # data.save_reconstruction([merged], "reconstruction_merged.json") - data.config["bundle_max_iterations"] = 200 - data.config["bundle_use_gcp"] = True - orec.bundle(merged, camera_models, gcp=gcps, config=data.config) - # rigid rotation to put images on the ground - orec.align_reconstruction(merged, None, data.config) - data.save_reconstruction([merged], "reconstruction_gcp_ba.json") + if not args.fast: + data.config["bundle_max_iterations"] = 200 + data.config["bundle_use_gcp"] = True + print("Running BA ...") + orec.bundle(merged, camera_models, gcp=gcps, config=data.config) + # rigid rotation to put images on the ground + orec.align_reconstruction(merged, None, data.config) + # data.save_reconstruction([merged], "reconstruction_gcp_ba.json") gcp_reprojections = reproject_gcps(gcps, merged) reprojection_errors = get_all_reprojection_errors(gcp_reprojections) @@ -439,58 +446,70 @@ def main(): gcp_std = compute_gcp_std(gcp_reprojections) logger.info(f"GCP reprojection error STD: {gcp_std}") - resplit = resplit_reconstruction(merged, reconstructions) - data.save_reconstruction(resplit, "reconstruction_gcp_ba_resplit.json") - - all_shots_std = [] - # We run bundle by fixing one reconstruction. - # If we have two reconstructions, we do this twice, fixing each one. - _rec_ixs = [(0, 1), (1, 0)] if args.rec_b else [(0, 1)] - for rec_ixs in _rec_ixs: - fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) - bundle_with_fixed_images( - merged, - camera_models, - gcp=gcps, - gcp_std=gcp_std, - fixed_images=fixed_images, - config=data.config, - ) + if not args.fast: + resplit = resplit_reconstruction(merged, reconstructions) + data.save_reconstruction(resplit, "reconstruction_gcp_ba_resplit.json") + all_shots_std = [] + # We run bundle by fixing one reconstruction. + # If we have two reconstructions, we do this twice, fixing each one. + _rec_ixs = [(0, 1), (1, 0)] if args.rec_b else [(0, 1)] + for rec_ixs in _rec_ixs: + fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) + bundle_with_fixed_images( + merged, + camera_models, + gcp=gcps, + gcp_std=gcp_std, + fixed_images=fixed_images, + config=data.config, + ) - logger.info( - f"STD in the position of shots in rec {rec_ixs[1]} w.r.t rec {rec_ixs[0]}" - ) - for shot in merged.shots.values(): - if shot.id in reconstructions[rec_ixs[1]].shots: - u, std = decompose_covariance(shot.covariance[3:, 3:]) - all_shots_std.append((shot.id, np.linalg.norm(std))) - logger.info(f"{shot.id} position std: {np.linalg.norm(std)}") - - # If the STD of all shots is the same, replace by nan - std_values = [x[1] for x in all_shots_std] - n_bad_std = sum(std > args.std_threshold for std in std_values) - n_good_std = sum(std <= args.std_threshold for std in std_values) - if np.allclose(std_values, std_values[0]): - all_shots_std = [(x[0], np.nan) for x in all_shots_std] - n_bad_std = len(std_values) - - # Average positional STD - median_shot_std = np.median([t[1] for t in all_shots_std]) - - # Save the shot STD to a file - with open(data.data_path + "/shots_std.csv", "w") as f: - s = sorted(all_shots_std, key=lambda t: -t[-1]) - for t in s: - line = "{}, {}".format(*t) - f.write(line + "\n") + logger.info( + f"STD in the position of shots in rec {rec_ixs[1]} w.r.t rec {rec_ixs[0]}" + ) + for shot in merged.shots.values(): + if shot.id in reconstructions[rec_ixs[1]].shots: + u, std = decompose_covariance(shot.covariance[3:, 3:]) + all_shots_std.append((shot.id, np.linalg.norm(std))) + logger.info(f"{shot.id} position std: {np.linalg.norm(std)}") + + # If the STD of all shots is the same, replace by nan + std_values = [x[1] for x in all_shots_std] + n_bad_std = sum(std > args.std_threshold for std in std_values) + n_good_std = sum(std <= args.std_threshold for std in std_values) + if np.allclose(std_values, std_values[0]): + all_shots_std = [(x[0], np.nan) for x in all_shots_std] + n_bad_std = len(std_values) + + # Average positional STD + median_shot_std = np.median([t[1] for t in all_shots_std]) + + # Save the shot STD to a file + with open(data.data_path + "/shots_std.csv", "w") as f: + s = sorted(all_shots_std, key=lambda t: -t[-1]) + for t in s: + line = "{}, {}".format(*t) + f.write(line + "\n") + + max_shot_std = s[0][1] + got_shots_std = not np.isnan(all_shots_std[0][1]) + else: + n_bad_std = -1 + n_good_std = -1 + median_shot_std = -1 + max_shot_std = -1 + got_shots_std = False n_bad_gcp_annotations = int( sum(t[2] > args.px_threshold for t in reprojection_errors) ) + for t in reprojection_errors: + if t[2] > args.px_threshold: + print(t) metrics = { "n_reconstructions": n_reconstructions, "median_shot_std": median_shot_std, - "max_shot_std": s[0][1], + "max_shot_std": max_shot_std, "max_reprojection_error": max_reprojection_error, "median_reprojection_error": median_reprojection_error, "n_gcp": len(gcps), @@ -513,8 +532,17 @@ def main(): f"No issues. All gcp reprojections are under {args.px_threshold}" f" and all frames are localized within {args.std_threshold}m" ) - else: - if np.isnan(all_shots_std[0][1]): + if n_bad_std == 0 and n_bad_gcp_annotations == 0: + logger.info( + f"No issues. All gcp reprojections are under {args.px_threshold}" + f" and all frames are localized within {args.std_threshold}m" + ) + if n_bad_std != 0 or n_bad_gcp_annotations != 0: + if args.fast: + logger.info( + "Positional uncertainty unknown since analysis ran in fast mode." + ) + elif not got_shots_std: logger.info( "Could not get positional uncertainty. It could be because:" "\na) there are not enough GCPs." @@ -528,7 +556,7 @@ def main(): ) logger.info( f"{n_bad_gcp_annotations} annotations with large reprojection error." - " Press Q to jump to the worst, correct it and repeat. Worst offenders:" + " Worst offenders:" ) stats_bad_reprojections = get_number_of_wrong_annotations_per_gcp( From 5a9fa35a7b049f4909c7813cc55b4f26d40f5464 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 2 Dec 2020 15:57:15 +0100 Subject: [PATCH 37/64] fix: gracefully ignore missing shots_std file --- annotation_gui_gcp/GUI.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 314504f75..eabb16a94 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -220,10 +220,12 @@ def analyze(self, fast=False): print("Done analyzing") def load_shot_std(self, path): - with open(path, "r") as f: - for line in f: - shot, std = line[:-1].split(",") - self.shot_std[shot] = float(std) + self.shot_std = {} + if os.path.isfile(path): + with open(path, "r") as f: + for line in f: + shot, std = line[:-1].split(",") + self.shot_std[shot] = float(std) def load_gcps(self, filename=None): if filename is None: From 95c1903352fb441af8b220bad780afbd81c77e68 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 3 Dec 2020 17:26:20 +0100 Subject: [PATCH 38/64] feat: 'fast' analysis will re-use bundle-adjusted reconstructions --- annotation_gui_gcp/run_ba.py | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 947fa6257..ec268c644 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -398,22 +398,27 @@ def main(): logger.error(f"Missing file: {fn}") return + assert args.rec_a != args.rec_b, "rec_a and rec_b should be different" + camera_models = data.load_camera_models() tracks_manager = data.load_tracks_manager() - - reconstructions = data.load_reconstruction() - n_reconstructions = len(reconstructions) gcps = data.load_ground_control_points() - assert args.rec_a != args.rec_b, "rec_a and rec_b should be different" + fn_resplit = f"reconstruction_gcp_ba_resplit_{args.rec_a}x{args.rec_b}.json" + fn_rigid = f"reconstruction_gcp_rigid_{args.rec_a}x{args.rec_b}.json" - if args.rec_b: - reconstructions = [reconstructions[args.rec_a], reconstructions[args.rec_b]] + if args.rec_b: # reconstruction - to - reconstruction annotation + if args.fast and os.path.exists(data._reconstruction_file(fn_resplit)): + reconstructions = data.load_reconstruction(fn_resplit) + else: + reconstructions = data.load_reconstruction() + reconstructions = [reconstructions[args.rec_a], reconstructions[args.rec_b]] coords0 = triangulate_gcps(gcps, reconstructions[0]) coords1 = triangulate_gcps(gcps, reconstructions[1]) s, A, b = find_alignment(coords1, coords0) align.apply_similarity(reconstructions[1], s, A, b) - else: + else: # Image - to - reconstruction annotation + reconstructions = data.load_reconstruction() base = reconstructions[args.rec_a] resected = resect_annotated_single_images(base, gcps, camera_models, data) for shot in resected.shots.values(): @@ -421,9 +426,9 @@ def main(): shot.metadata.gps_position.value = shot.pose.get_origin() reconstructions = [base, resected] - data.save_reconstruction(reconstructions, "reconstruction_gcp_rigid.json") + data.save_reconstruction(reconstructions, fn_rigid) merged = merge_reconstructions(reconstructions, tracks_manager) - # data.save_reconstruction([merged], "reconstruction_merged.json") + # data.save_reconstruction([merged], f"reconstruction_merged_{args.rec_a}x{args.rec_b}.json") if not args.fast: data.config["bundle_max_iterations"] = 200 @@ -432,7 +437,7 @@ def main(): orec.bundle(merged, camera_models, gcp=gcps, config=data.config) # rigid rotation to put images on the ground orec.align_reconstruction(merged, None, data.config) - # data.save_reconstruction([merged], "reconstruction_gcp_ba.json") + # data.save_reconstruction([merged], "reconstruction_gcp_ba_{args.rec_a}x{args.rec_b}.json") gcp_reprojections = reproject_gcps(gcps, merged) reprojection_errors = get_all_reprojection_errors(gcp_reprojections) @@ -448,7 +453,7 @@ def main(): if not args.fast: resplit = resplit_reconstruction(merged, reconstructions) - data.save_reconstruction(resplit, "reconstruction_gcp_ba_resplit.json") + data.save_reconstruction(resplit, fn_resplit) all_shots_std = [] # We run bundle by fixing one reconstruction. # If we have two reconstructions, we do this twice, fixing each one. @@ -507,7 +512,7 @@ def main(): if t[2] > args.px_threshold: print(t) metrics = { - "n_reconstructions": n_reconstructions, + "n_reconstructions": len(data.load_reconstruction()), "median_shot_std": median_shot_std, "max_shot_std": max_shot_std, "max_reprojection_error": max_reprojection_error, From eaa512bae45cb8130a09e22dc5a22fc112a63ba8 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 4 Dec 2020 16:34:45 +0100 Subject: [PATCH 39/64] fix: orthophoto annotations were wrongly saved --- annotation_gui_gcp/orthophoto_manager.py | 7 +++---- annotation_gui_gcp/orthophoto_view.py | 5 ++--- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/annotation_gui_gcp/orthophoto_manager.py b/annotation_gui_gcp/orthophoto_manager.py index ad4a45f51..986908ed8 100644 --- a/annotation_gui_gcp/orthophoto_manager.py +++ b/annotation_gui_gcp/orthophoto_manager.py @@ -35,13 +35,11 @@ def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float # Create the corners of the viewing window in pixels top, right = t.index(xs[0] + size / 2, ys[0] + size / 2) bottom, left = t.index(xs[0] - size / 2, ys[0] - size / 2) - window = rasterio.windows.Window(left, top, right - left, bottom - top) - self.current_window = window # TODO downsample image if the zoom level is too low / the image too large tw = reshape_as_image(t.read(window=window, boundless=True)) - return tw, window, t + return tw, window def get_candidate_images(self, lat: float, lon: float): # lat, lon -> nearmap file id + nearmap pixel coordinates @@ -51,4 +49,5 @@ def get_image(self, img, lat: float, lon: float, size: float): return self.read_image_around_latlon(img, lat, lon, size) def get_image_size(self, img): - return self.current_window.height, self.current_window.width + t = rasterio.open(self.path / img) + return t.height, t.width diff --git a/annotation_gui_gcp/orthophoto_view.py b/annotation_gui_gcp/orthophoto_view.py index 04b1b85d5..be5fc8a9a 100644 --- a/annotation_gui_gcp/orthophoto_view.py +++ b/annotation_gui_gcp/orthophoto_view.py @@ -38,12 +38,11 @@ def __init__( self.set_title() def get_image(self, new_image): - image, image_window, geot = self.image_manager.read_image_around_latlon( + crop, image_window = self.image_manager.read_image_around_latlon( new_image, self.center_lat, self.center_lon, self.size ) self.image_window = image_window - self.geot = geot - return image + return crop def get_candidate_images(self): return self.image_manager.get_candidate_images(self.center_lat, self.center_lon) From 7e7bb192d1ed1b5f83db7d0690ac6dddeb0477ba Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 7 Dec 2020 09:48:38 +0100 Subject: [PATCH 40/64] fix: better search for candidate orthophotos --- annotation_gui_gcp/orthophoto_manager.py | 35 ++++++++++++++---------- annotation_gui_gcp/orthophoto_view.py | 4 ++- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/annotation_gui_gcp/orthophoto_manager.py b/annotation_gui_gcp/orthophoto_manager.py index 986908ed8..975c67c6c 100644 --- a/annotation_gui_gcp/orthophoto_manager.py +++ b/annotation_gui_gcp/orthophoto_manager.py @@ -6,6 +6,7 @@ import rasterio.warp import rasterio.windows from rasterio.plot import reshape_as_image +from rasterio.io import DatasetReader class OrthoPhotoManager: @@ -14,13 +15,23 @@ def __init__(self, path: str, size: float): self.size = size self.image_keys = [p.name for p in self.path.iterdir() if p.suffix == ".tif"] - def check_latlon_covered(self, img: str, lat: float, lon: float): - t = rasterio.open(self.path / img) + def window_around_latlon( + self, t: DatasetReader, lat: float, lon: float, size: float + ): + # From latlon to the coordinate reference system of the image in m xs, ys = rasterio.warp.transform("EPSG:4326", t.crs, [lon], [lat], zs=None) - row_center, col_center = t.index(xs[0], ys[0]) - window = rasterio.windows.Window(row_center, col_center, 1, 1) + + # Create the corners of the viewing window in pixels + top, right = t.index(xs[0] + size / 2, ys[0] + size / 2) + bottom, left = t.index(xs[0] - size / 2, ys[0] - size / 2) + window = rasterio.windows.Window(left, top, right - left, bottom - top) + return window + + def check_latlon_covered(self, img: str, lat: float, lon: float, size: float): + t = rasterio.open(self.path / img) + window = self.window_around_latlon(t, lat, lon, size) mask = t.read_masks(1, window=window, boundless=True) - return mask.item() != 0 + return mask.sum() > 0 def load_latlons(self): # We don't have a 'canonical' lat/lon for orthophotos @@ -29,21 +40,17 @@ def load_latlons(self): def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float): t = rasterio.open(self.path / img) - # From latlon to the coordinate reference system of the image in m - xs, ys = rasterio.warp.transform("EPSG:4326", t.crs, [lon], [lat], zs=None) - - # Create the corners of the viewing window in pixels - top, right = t.index(xs[0] + size / 2, ys[0] + size / 2) - bottom, left = t.index(xs[0] - size / 2, ys[0] - size / 2) - window = rasterio.windows.Window(left, top, right - left, bottom - top) + window = self.window_around_latlon(t, lat, lon, size) # TODO downsample image if the zoom level is too low / the image too large tw = reshape_as_image(t.read(window=window, boundless=True)) return tw, window - def get_candidate_images(self, lat: float, lon: float): + def get_candidate_images(self, lat: float, lon: float, size: float): # lat, lon -> nearmap file id + nearmap pixel coordinates - return [k for k in self.image_keys if self.check_latlon_covered(k, lat, lon)] + return [ + k for k in self.image_keys if self.check_latlon_covered(k, lat, lon, size) + ] def get_image(self, img, lat: float, lon: float, size: float): return self.read_image_around_latlon(img, lat, lon, size) diff --git a/annotation_gui_gcp/orthophoto_view.py b/annotation_gui_gcp/orthophoto_view.py index be5fc8a9a..2431648f2 100644 --- a/annotation_gui_gcp/orthophoto_view.py +++ b/annotation_gui_gcp/orthophoto_view.py @@ -45,7 +45,9 @@ def get_image(self, new_image): return crop def get_candidate_images(self): - return self.image_manager.get_candidate_images(self.center_lat, self.center_lon) + return self.image_manager.get_candidate_images( + self.center_lat, self.center_lon, self.size + ) def pixel_to_latlon(self, x: float, y: float): """ From d04b4875205cbf63b25ce5174a51bd6d15a74e89 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 7 Dec 2020 16:57:20 +0100 Subject: [PATCH 41/64] feat: add orthophoto coverage cache --- annotation_gui_gcp/orthophoto_manager.py | 13 +++++++++---- annotation_gui_gcp/orthophoto_view.py | 3 ++- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/annotation_gui_gcp/orthophoto_manager.py b/annotation_gui_gcp/orthophoto_manager.py index 975c67c6c..cf3f0300f 100644 --- a/annotation_gui_gcp/orthophoto_manager.py +++ b/annotation_gui_gcp/orthophoto_manager.py @@ -14,6 +14,7 @@ def __init__(self, path: str, size: float): self.path = Path(path) self.size = size self.image_keys = [p.name for p in self.path.iterdir() if p.suffix == ".tif"] + self.candidate_cache = {} def window_around_latlon( self, t: DatasetReader, lat: float, lon: float, size: float @@ -44,13 +45,17 @@ def read_image_around_latlon(self, img: str, lat: float, lon: float, size: float # TODO downsample image if the zoom level is too low / the image too large tw = reshape_as_image(t.read(window=window, boundless=True)) - return tw, window + return tw, window, t def get_candidate_images(self, lat: float, lon: float, size: float): # lat, lon -> nearmap file id + nearmap pixel coordinates - return [ - k for k in self.image_keys if self.check_latlon_covered(k, lat, lon, size) - ] + if (lat, lon, size) not in self.candidate_cache: + self.candidate_cache[(lat, lon, size)] = [ + k + for k in self.image_keys + if self.check_latlon_covered(k, lat, lon, size) + ] + return self.candidate_cache[(lat, lon, size)] def get_image(self, img, lat: float, lon: float, size: float): return self.read_image_around_latlon(img, lat, lon, size) diff --git a/annotation_gui_gcp/orthophoto_view.py b/annotation_gui_gcp/orthophoto_view.py index 2431648f2..4b7e213b5 100644 --- a/annotation_gui_gcp/orthophoto_view.py +++ b/annotation_gui_gcp/orthophoto_view.py @@ -38,10 +38,11 @@ def __init__( self.set_title() def get_image(self, new_image): - crop, image_window = self.image_manager.read_image_around_latlon( + crop, image_window, geot = self.image_manager.read_image_around_latlon( new_image, self.center_lat, self.center_lon, self.size ) self.image_window = image_window + self.geot = geot return crop def get_candidate_images(self): From e170b0517d4d17af3d60bf3a18a0c6f1e477c174 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 7 Dec 2020 16:58:17 +0100 Subject: [PATCH 42/64] fix: warn if GCP doesn't have enough annotations to triangulate --- annotation_gui_gcp/run_ba.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index ec268c644..b421acba3 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -77,9 +77,11 @@ def reproject_gcps(gcps, reconstruction): output = {} for gcp in gcps: point = orec.triangulate_gcp(gcp, reconstruction.shots) - if point is None: - point = np.nan output[gcp.id] = {} + n_obs = len(gcp.observations) + if point is None: + print(f"Could not triangulate {gcp.id} with {n_obs} annotations") + continue for observation in gcp.observations: if observation.shot_id not in reconstruction.shots: continue @@ -459,6 +461,7 @@ def main(): # If we have two reconstructions, we do this twice, fixing each one. _rec_ixs = [(0, 1), (1, 0)] if args.rec_b else [(0, 1)] for rec_ixs in _rec_ixs: + print(f"Running BA with fixed images. Fixing rec #{rec_ixs[0]}") fixed_images = set(reconstructions[rec_ixs[0]].shots.keys()) bundle_with_fixed_images( merged, @@ -470,7 +473,7 @@ def main(): ) logger.info( - f"STD in the position of shots in rec {rec_ixs[1]} w.r.t rec {rec_ixs[0]}" + f"STD in the position of shots in R#{rec_ixs[1]} w.r.t R#{rec_ixs[0]}" ) for shot in merged.shots.values(): if shot.id in reconstructions[rec_ixs[1]].shots: From d019131aa2f01507bd3ee3db5bac75b0c9a754d1 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 7 Jan 2021 15:46:26 +0100 Subject: [PATCH 43/64] fix: always keep at least 2 reconstructions --- annotation_gui_gcp/main.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 74a486c31..5be9cd9c4 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -115,7 +115,11 @@ def load_shots_from_reconstructions(path, min_ims): n_recs = len(reconstructions) if len(reconstructions) > 2: - reconstructions = [r for r in reconstructions if len(r.shots) >= min_ims] + reconstructions = [ + r + for ix_r, r in enumerate(reconstructions) + if len(r.shots) >= min_ims or ix_r < 2 + ] if len(reconstructions) < n_recs: print( "Kept {}/{} reconstructions (min images: {})".format( @@ -135,7 +139,9 @@ def load_shots_from_reconstructions(path, min_ims): def group_by_reconstruction(args, groups_from_sequence_database): - all_recs_shots = load_shots_from_reconstructions(path, min_ims=50) + all_recs_shots = load_shots_from_reconstructions( + path, min_ims=args.min_images_in_reconstruction + ) grouped_skeys = [skey for g in args.sequence_group for skey in g] map_key_to_skey = {} From 3c31b595e8bd18f2a8e8c5b795adf3f6b25005a0 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 8 Jan 2021 08:27:47 +0100 Subject: [PATCH 44/64] save GPC file also with the directory name --- annotation_gui_gcp/GUI.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index eabb16a94..729e524f9 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -326,9 +326,12 @@ def onclick_gcp_list(self, event): def save_gcps(self): if self.quick_save_filename is None: - return self.save_gcps_as() + self.save_gcps_as() else: - return self.gcp_manager.write_to_file(self.quick_save_filename) + self.gcp_manager.write_to_file(self.quick_save_filename) + parent = os.path.dirname(self.quick_save_filename) + dirname = os.path.basename(parent) + self.gcp_manager.write_to_file(os.path.join(parent, dirname + ".json")) def save_gcps_as(self): filename = tk.filedialog.asksaveasfilename( From 12c025de32cfece9ad14ef2ad5e4f906aabbcebd Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 12 Jan 2021 18:54:13 +0100 Subject: [PATCH 45/64] don't initialize window positions --- annotation_gui_gcp/GUI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 729e524f9..eb567982d 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -76,7 +76,7 @@ def create_ui(self, ortho_paths): latlon = v.latlons[k] self.create_ortho_views(ortho_paths, latlon["lat"], latlon["lon"]) self.master.update_idletasks() - self.arrange_ui_onerow() + # self.arrange_ui_onerow() def arrange_ui_onerow(self): master = self.master From 2510f22efa69cecc573c86887f489fc18c505867 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Tue, 19 Jan 2021 09:59:17 +0100 Subject: [PATCH 46/64] Save analysis metrics separately for each pair --- annotation_gui_gcp/run_ba.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index b421acba3..64a326244 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -524,13 +524,19 @@ def main(): "n_bad_gcp_annotations": n_bad_gcp_annotations, "n_bad_position_std": int(n_bad_std), "n_good_position_std": int(n_good_std), + "rec_a": args.rec_a, + "rec_b": args.rec_b, } logger.info(metrics) - p_metrics = data.data_path + "/run_ba_metrics.json" - logger.info(f"Saved metrics to {p_metrics}") - with open(p_metrics, "w") as f: - json.dump(metrics, f, indent=4, sort_keys=True) + for out_name in ( + "run_ba_metrics.json", + f"run_ba_metrics_{args.rec_a}x{args.rec_b}.json", + ): + p_metrics = data.data_path + "/" + out_name + with open(p_metrics, "w") as f: + json.dump(metrics, f, indent=4, sort_keys=True) + logger.info(f"Saved metrics to {p_metrics}") logger.info("========================================") logger.info("=============== Summary ================") From 6a9b1d4918872f97faa81593ab9ac0727d811153 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Tue, 19 Jan 2021 23:09:52 +0100 Subject: [PATCH 47/64] build: use c++14 --- opensfm/src/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opensfm/src/CMakeLists.txt b/opensfm/src/CMakeLists.txt index 9f87e4856..a23fcd84a 100644 --- a/opensfm/src/CMakeLists.txt +++ b/opensfm/src/CMakeLists.txt @@ -18,8 +18,8 @@ set(CMAKE_CXX_VISIBILITY_INLINES ON) # fPIC set(CMAKE_POSITION_INDEPENDENT_CODE ON) -# C++11 -set(CMAKE_CXX_STANDARD 11) +# C++14 +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # For compiling VLFeat From c1a6186b7450c944b7ad42f8b1622720136e06e8 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Wed, 20 Jan 2021 08:58:01 +0100 Subject: [PATCH 48/64] use sys.exit() --- annotation_gui_gcp/run_ba.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 64a326244..590f0693b 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -2,6 +2,7 @@ import json import logging import os +import sys import numpy as np import opensfm.reconstruction as orec @@ -430,7 +431,9 @@ def main(): data.save_reconstruction(reconstructions, fn_rigid) merged = merge_reconstructions(reconstructions, tracks_manager) - # data.save_reconstruction([merged], f"reconstruction_merged_{args.rec_a}x{args.rec_b}.json") + # data.save_reconstruction( + # [merged], f"reconstruction_merged_{args.rec_a}x{args.rec_b}.json" + # ) if not args.fast: data.config["bundle_max_iterations"] = 200 @@ -439,7 +442,9 @@ def main(): orec.bundle(merged, camera_models, gcp=gcps, config=data.config) # rigid rotation to put images on the ground orec.align_reconstruction(merged, None, data.config) - # data.save_reconstruction([merged], "reconstruction_gcp_ba_{args.rec_a}x{args.rec_b}.json") + # data.save_reconstruction( + # [merged], "reconstruction_gcp_ba_{args.rec_a}x{args.rec_b}.json" + # ) gcp_reprojections = reproject_gcps(gcps, merged) reprojection_errors = get_all_reprojection_errors(gcp_reprojections) @@ -587,4 +592,4 @@ def main(): if __name__ == "__main__": log.setup() - exit(main()) + sys.exit(main()) From 030e37da5959808dced72f11731f7b267cf8bb76 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 28 Jan 2021 14:22:54 +0100 Subject: [PATCH 49/64] chore: clearer error messages --- annotation_gui_gcp/GUI.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index eb567982d..ea1299164 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -188,10 +188,15 @@ def analyze(self, fast=False): t = time.time() - os.path.getmtime(self.path + "/ground_control_points.json") ix_a = self.reconstruction_options.index(self.rec_a.get()) ix_b = self.reconstruction_options.index(self.rec_b.get()) - if t > 30 or ix_a == ix_b: + if ix_a == ix_b: print( "Please select different reconstructions in the drop-down menus" - " and save before running the analysis" + " before running the analysis" + ) + return + if t > 30: + print( + "Please save to ground_control_points.json before running the analysis" ) return From a7a8293c47cda4a10953d41516528b777fb9de42 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 28 Jan 2021 15:38:06 +0100 Subject: [PATCH 50/64] save results per rec. pair + simple GCP ids The results from run_ba.py are saved separately for each reconstruction pair, and the UI expects them to be named in that way. GCP ids are now simple incremental integers. --- annotation_gui_gcp/GUI.py | 33 +++++++++++++----- annotation_gui_gcp/gcp_manager.py | 58 +++++++++---------------------- annotation_gui_gcp/run_ba.py | 20 +++++------ 3 files changed, 51 insertions(+), 60 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index ea1299164..6cbee6cd7 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -41,9 +41,7 @@ def __init__( p_default_gcp = self.path + "/ground_control_points.json" if os.path.exists(p_default_gcp): self.load_gcps(p_default_gcp) - p_shot_std = self.path + "/shots_std.csv" - if os.path.exists(p_shot_std): - self.load_shot_std(p_shot_std) + self.load_analysis_results(0, 1) def get_reconstruction_options(self): p_recs = self.path + "/reconstruction.json" @@ -78,6 +76,17 @@ def create_ui(self, ortho_paths): self.master.update_idletasks() # self.arrange_ui_onerow() + def rec_ix_changed(self, *args): + # Load analysis for the new reconstruction pair if it exists + ix_a = self.reconstruction_options.index(self.rec_a.get()) + ix_b = self.reconstruction_options.index(self.rec_b.get()) + if ix_b == len(self.reconstruction_options) - 1: + ix_b = None + print(f"Loading analysis results for {self.rec_a.get()} vs {self.rec_b.get()}") + self.load_analysis_results(ix_a, ix_b) + for view in self.sequence_views: + view.populate_image_list() + def arrange_ui_onerow(self): master = self.master # Arrange views on the screen. All views on one single row @@ -120,7 +129,7 @@ def create_tools(self, master): ) remove_button.pack(side="left") - self.sticky_zoom = tk.BooleanVar(value=True) + self.sticky_zoom = tk.BooleanVar(value=False) button = tk.Checkbutton(master, text="Sticky zoom (x)", var=self.sticky_zoom) button.pack(side="top") @@ -136,12 +145,14 @@ def create_tools(self, master): options = self.reconstruction_options self.rec_a = tk.StringVar(master) self.rec_a.set(options[0]) + self.rec_a.trace("w", self.rec_ix_changed) w = tk.OptionMenu(analysis_frame, self.rec_a, *options[:-1]) w.pack(side="top", fill=tk.X) w.config(width=width) self.rec_b = tk.StringVar(master) self.rec_b.set(options[1]) + self.rec_b.trace("w", self.rec_ix_changed) w = tk.OptionMenu(analysis_frame, self.rec_b, *options) w.pack(side="top", fill=tk.X) w.config(width=width) @@ -209,21 +220,27 @@ def analyze(self, fast=False): ] if ix_b < len(self.reconstruction_options) - 1: args.extend(("--rec_b", str(ix_b))) + else: + ix_b = None + if fast: args.extend(["--fast"]) # Call the run_ba script subprocess.run(args) - self.shot_std = {} - self.load_shot_std(self.path + "/shots_std.csv") - p_gcp_errors = self.path + "/gcp_reprojections.json" - self.gcp_manager.load_gcp_reprojections(p_gcp_errors) + # Load the results + self.load_analysis_results(ix_a, ix_b) for view in self.sequence_views: view.populate_image_list() print("Done analyzing") + def load_analysis_results(self, ix_a, ix_b): + self.load_shot_std(f"{self.path}/shots_std_{ix_a}x{ix_b}.csv") + p_gcp_errors = f"{self.path}/gcp_reprojections_{ix_a}x{ix_b}.json" + self.gcp_manager.load_gcp_reprojections(p_gcp_errors) + def load_shot_std(self, path): self.shot_std = {} if os.path.isfile(path): diff --git a/annotation_gui_gcp/gcp_manager.py b/annotation_gui_gcp/gcp_manager.py index b45a8280f..ca2cb7ca5 100644 --- a/annotation_gui_gcp/gcp_manager.py +++ b/annotation_gui_gcp/gcp_manager.py @@ -1,40 +1,13 @@ import json import os -import random -import string -import urllib.request from collections import OrderedDict -words = None - -def load_or_download_names(): - global words - path_local_words = os.path.expanduser("~/.annotation_ui_names.txt") - word_url = "http://www.gutenberg.org/files/3201/files/NAMES.TXT" - try: - if not os.path.isfile(path_local_words): - - print(f"Saving {word_url} to {path_local_words}") - urllib.request.urlretrieve(word_url, path_local_words) - - print(f"Loading {path_local_words}") - with open(path_local_words, "rb") as f: - words = f.read().decode("unicode_escape").splitlines() - - except Exception as e: - print(e) - words = [] - - -def id_generator(size=6, chars=string.ascii_uppercase + string.digits): - global words - if len(words) > 0: - firstname = random.choice(words) - lastname = random.choice(words) - return "_".join((firstname, lastname)).upper() - else: - return "".join(random.choice(chars) for _ in range(size)) +def id_generator(): + i = 0 + while True: + yield str(i) + i += 1 class GroundControlPointManager: @@ -43,17 +16,18 @@ def __init__(self, path): self.latlons = {} self.path = path self.image_cache = {} - load_or_download_names() + self.id_generator = id_generator() - p_gcp_errors = self.path + "/gcp_reprojections.json" - if os.path.exists(p_gcp_errors): - self.load_gcp_reprojections(p_gcp_errors) - else: - self.gcp_reprojections = {} + # p_gcp_errors = self.path + "/gcp_reprojections_0x1.json" + # self.load_gcp_reprojections(p_gcp_errors) def load_gcp_reprojections(self, filename): - with open(filename, "r") as f: - self.gcp_reprojections = json.load(f) + if os.path.isfile(filename): + with open(filename, "r") as f: + self.gcp_reprojections = json.load(f) + else: + self.gcp_reprojections = {} + print(f"Not found: {filename}") def load_from_file(self, file_path): self.points = OrderedDict() @@ -91,9 +65,9 @@ def point_exists(self, point_id): return point_id in self.points def add_point(self): - new_id = id_generator() + new_id = next(self.id_generator) while self.point_exists(new_id): - new_id = id_generator() + new_id = next(self.id_generator) self.points[new_id] = [] return new_id diff --git a/annotation_gui_gcp/run_ba.py b/annotation_gui_gcp/run_ba.py index 590f0693b..31c3a0a74 100644 --- a/annotation_gui_gcp/run_ba.py +++ b/annotation_gui_gcp/run_ba.py @@ -452,7 +452,9 @@ def main(): max_reprojection_error = np.max(err_values) median_reprojection_error = np.median(err_values) - with open(data.data_path + "/gcp_reprojections.json", "w") as f: + with open( + f"{data.data_path}/gcp_reprojections_{args.rec_a}x{args.rec_b}.json", "w" + ) as f: json.dump(gcp_reprojections, f, indent=4, sort_keys=True) gcp_std = compute_gcp_std(gcp_reprojections) @@ -498,7 +500,9 @@ def main(): median_shot_std = np.median([t[1] for t in all_shots_std]) # Save the shot STD to a file - with open(data.data_path + "/shots_std.csv", "w") as f: + with open( + f"{data.data_path}/shots_std_{args.rec_a}x{args.rec_b}.csv", "w" + ) as f: s = sorted(all_shots_std, key=lambda t: -t[-1]) for t in s: line = "{}, {}".format(*t) @@ -534,14 +538,10 @@ def main(): } logger.info(metrics) - for out_name in ( - "run_ba_metrics.json", - f"run_ba_metrics_{args.rec_a}x{args.rec_b}.json", - ): - p_metrics = data.data_path + "/" + out_name - with open(p_metrics, "w") as f: - json.dump(metrics, f, indent=4, sort_keys=True) - logger.info(f"Saved metrics to {p_metrics}") + p_metrics = f"{data.data_path}/run_ba_metrics_{args.rec_a}x{args.rec_b}.json" + with open(p_metrics, "w") as f: + json.dump(metrics, f, indent=4, sort_keys=True) + logger.info(f"Saved metrics to {p_metrics}") logger.info("========================================") logger.info("=============== Summary ================") From 3f5f882c1ff8672b5f7b19ae5155f904f7a8a6be Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 4 Feb 2021 18:05:43 +0100 Subject: [PATCH 51/64] feat: more informative message when jumping to worst GCP --- annotation_gui_gcp/GUI.py | 6 +++--- annotation_gui_gcp/gcp_manager.py | 17 ++++++++--------- annotation_gui_gcp/image_sequence_view.py | 4 ++-- annotation_gui_gcp/main.py | 6 +++--- annotation_gui_gcp/orthophoto_view.py | 4 ++-- annotation_gui_gcp/view.py | 3 --- 6 files changed, 18 insertions(+), 22 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 62a4fd4ce..73b12d9f8 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -11,8 +11,8 @@ matplotlib.use("TkAgg") -from .image_sequence_view import ImageSequenceView -from .orthophoto_view import OrthoPhotoView +from image_sequence_view import ImageSequenceView +from orthophoto_view import OrthoPhotoView FONT = "TkFixedFont" @@ -392,7 +392,7 @@ def go_to_worst_gcp(self): if len(self.gcp_manager.gcp_reprojections) == 0: print("No GCP reprojections available. Can't jump to worst GCP") return - worst_gcp = self.gcp_manager.compute_gcp_errors()[0] + worst_gcp = self.gcp_manager.get_worst_gcp() if worst_gcp is None: return diff --git a/annotation_gui_gcp/gcp_manager.py b/annotation_gui_gcp/gcp_manager.py index ca2cb7ca5..168ac5abc 100644 --- a/annotation_gui_gcp/gcp_manager.py +++ b/annotation_gui_gcp/gcp_manager.py @@ -87,26 +87,25 @@ def add_point_observation(self, point_id, shot_id, projection, latlon=None): } ) - def compute_gcp_errors(self): - error_avg = {} + def get_worst_gcp(self): worst_gcp_error = 0 worst_gcp = None - shot_worst_gcp = None - for gcp_id in self.points: - error_avg[gcp_id] = 0 for gcp_id in self.gcp_reprojections: if gcp_id not in self.points: continue for shot_id in self.gcp_reprojections[gcp_id]: err = self.gcp_reprojections[gcp_id][shot_id]["error"] - error_avg[gcp_id] += err if err > worst_gcp_error: worst_gcp_error = err - shot_worst_gcp = shot_id worst_gcp = gcp_id - error_avg[gcp_id] /= len(self.gcp_reprojections[gcp_id]) - return worst_gcp, shot_worst_gcp, worst_gcp_error, error_avg + errors_worst_gcp = [ + x["error"] for x in self.gcp_reprojections[worst_gcp].values() + ] + n = len(errors_worst_gcp) + n_checked = sum(x == 0 for x in errors_worst_gcp) + print(f"Worst GCP: {worst_gcp} checked in {n_checked}/{n} images") + return worst_gcp def shot_with_max_gcp_error(self, image_keys, gcp): # Return they key with most reprojection error for this GCP diff --git a/annotation_gui_gcp/image_sequence_view.py b/annotation_gui_gcp/image_sequence_view.py index 312805221..7988a4a3c 100644 --- a/annotation_gui_gcp/image_sequence_view.py +++ b/annotation_gui_gcp/image_sequence_view.py @@ -4,8 +4,8 @@ import numpy as np from matplotlib import pyplot as plt -from .geometry import get_all_track_observations, get_tracks_visible_in_image -from .view import View +from geometry import get_all_track_observations, get_tracks_visible_in_image +from view import View class ImageSequenceView(View): diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 65700f2d0..5be9cd9c4 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -5,9 +5,9 @@ from opensfm import dataset, io -from . import GUI -from .gcp_manager import GroundControlPointManager -from .image_manager import ImageManager +import GUI +from gcp_manager import GroundControlPointManager +from image_manager import ImageManager def parse_args(): diff --git a/annotation_gui_gcp/orthophoto_view.py b/annotation_gui_gcp/orthophoto_view.py index 4a3594fd5..4b7e213b5 100644 --- a/annotation_gui_gcp/orthophoto_view.py +++ b/annotation_gui_gcp/orthophoto_view.py @@ -4,8 +4,8 @@ import rasterio.warp from opensfm import features -from .orthophoto_manager import OrthoPhotoManager -from .view import View +from orthophoto_manager import OrthoPhotoManager +from view import View class OrthoPhotoView(View): diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py index 59f0dc0b1..55031e3c6 100644 --- a/annotation_gui_gcp/view.py +++ b/annotation_gui_gcp/view.py @@ -149,9 +149,6 @@ def onclick_image_list(self, event): def add_move_or_remove_gcp(self, x, y, add): if self.main_ui.curr_point is None: return - reproj = self.main_ui.gcp_manager.gcp_reprojections.get(self.main_ui.curr_point) - if reproj: - reproj.pop(self.current_image, None) self.main_ui.gcp_manager.remove_point_observation( self.main_ui.curr_point, self.current_image ) From abfa80cd9449ba1c99eb1ab851e34f641900de50 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 4 Feb 2021 19:05:59 +0100 Subject: [PATCH 52/64] fix: remove reprojections after confirming --- annotation_gui_gcp/gcp_manager.py | 3 +-- annotation_gui_gcp/view.py | 3 +++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/annotation_gui_gcp/gcp_manager.py b/annotation_gui_gcp/gcp_manager.py index 168ac5abc..5ed735e01 100644 --- a/annotation_gui_gcp/gcp_manager.py +++ b/annotation_gui_gcp/gcp_manager.py @@ -103,8 +103,7 @@ def get_worst_gcp(self): x["error"] for x in self.gcp_reprojections[worst_gcp].values() ] n = len(errors_worst_gcp) - n_checked = sum(x == 0 for x in errors_worst_gcp) - print(f"Worst GCP: {worst_gcp} checked in {n_checked}/{n} images") + print(f"Worst GCP: {worst_gcp} unconfirmed in {n} images") return worst_gcp def shot_with_max_gcp_error(self, image_keys, gcp): diff --git a/annotation_gui_gcp/view.py b/annotation_gui_gcp/view.py index 55031e3c6..59f0dc0b1 100644 --- a/annotation_gui_gcp/view.py +++ b/annotation_gui_gcp/view.py @@ -149,6 +149,9 @@ def onclick_image_list(self, event): def add_move_or_remove_gcp(self, x, y, add): if self.main_ui.curr_point is None: return + reproj = self.main_ui.gcp_manager.gcp_reprojections.get(self.main_ui.curr_point) + if reproj: + reproj.pop(self.current_image, None) self.main_ui.gcp_manager.remove_point_observation( self.main_ui.curr_point, self.current_image ) From 1a784523f708cf7d4385ec1b71207eca964c5771 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 8 Feb 2021 08:21:49 +0100 Subject: [PATCH 53/64] fix: UI launches without reconstruction.json --- annotation_gui_gcp/GUI.py | 6 +++--- annotation_gui_gcp/gcp_manager.py | 2 -- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index 73b12d9f8..bf5f71cfe 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -34,7 +34,7 @@ def __init__( master.bind_all("z", lambda event: self.toggle_zoom_all_views()) master.bind_all("x", lambda event: self.toggle_sticky_zoom()) master.bind_all("a", lambda event: self.go_to_current_gcp()) - self.get_reconstruction_options() + self.reconstruction_options = self.get_reconstruction_options() self.create_ui(ortho_paths) master.lift() @@ -47,7 +47,7 @@ def get_reconstruction_options(self): p_recs = self.path + "/reconstruction.json" print(p_recs) if not os.path.exists(p_recs): - return {} + return [] data = dataset.DataSet(self.path) recs = data.load_reconstruction() options = [] @@ -60,7 +60,7 @@ def get_reconstruction_options(self): ) options.append(str_repr) options.append("None (3d-to-2d)") - self.reconstruction_options = options + return options def create_ui(self, ortho_paths): tools_frame = tk.Frame(self.master) diff --git a/annotation_gui_gcp/gcp_manager.py b/annotation_gui_gcp/gcp_manager.py index 5ed735e01..1ef940cf3 100644 --- a/annotation_gui_gcp/gcp_manager.py +++ b/annotation_gui_gcp/gcp_manager.py @@ -38,8 +38,6 @@ def load_from_file(self, file_path): self.points[point["id"]] = point["observations"] latlon = point.get("position") if latlon: - if "altitude" in latlon: - raise NotImplementedError("Not supported: altitude in GCPs") self.latlons[point["id"]] = latlon def write_to_file(self, filename): From 4a1254f030d02b6d6b902df4a753071b6847eb82 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Mon, 8 Feb 2021 08:54:30 +0100 Subject: [PATCH 54/64] fix: UI launches without reconstruction.json --- annotation_gui_gcp/GUI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/annotation_gui_gcp/GUI.py b/annotation_gui_gcp/GUI.py index bf5f71cfe..051bdfa66 100644 --- a/annotation_gui_gcp/GUI.py +++ b/annotation_gui_gcp/GUI.py @@ -47,7 +47,7 @@ def get_reconstruction_options(self): p_recs = self.path + "/reconstruction.json" print(p_recs) if not os.path.exists(p_recs): - return [] + return ["NONE", "NONE"] data = dataset.DataSet(self.path) recs = data.load_reconstruction() options = [] From a76fd4b107b9ff224a3330b816aa990c89d1f0b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Manuel=20L=C3=B3pez=20Antequera?= Date: Thu, 11 Feb 2021 14:46:50 +0100 Subject: [PATCH 55/64] fix: build on fedora --- opensfm/src/geometry/test/camera_test.cc | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/opensfm/src/geometry/test/camera_test.cc b/opensfm/src/geometry/test/camera_test.cc index 58d039e94..e8989f1a1 100644 --- a/opensfm/src/geometry/test/camera_test.cc +++ b/opensfm/src/geometry/test/camera_test.cc @@ -83,15 +83,15 @@ class CameraFixture : public ::testing::Test { } } - static constexpr int pixels_count{10000}; - static constexpr int pixel_width{4000}; - static constexpr int pixel_height{3000}; - - static constexpr double max_width{0.90}; - static constexpr double max_height{0.75}; - static constexpr double focal{0.4}; - static constexpr double new_focal{0.8}; - static constexpr double new_ar{0.9}; + const int pixels_count{10000}; + const int pixel_width{4000}; + const int pixel_height{3000}; + + const double max_width{0.90}; + const double max_height{0.75}; + const double focal{0.4}; + const double new_focal{0.8}; + const double new_ar{0.9}; MatX2d pixels; VecXd distortion; From 17420cd7d022f0abe3fb8a80655dab8c72b25fc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Manuel=20L=C3=B3pez=20Antequera?= Date: Thu, 11 Feb 2021 14:47:03 +0100 Subject: [PATCH 56/64] fix: missing rasterio requirement for UI --- annotation_gui_gcp/requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/annotation_gui_gcp/requirements.txt b/annotation_gui_gcp/requirements.txt index 6ccafc3f9..796e83d84 100644 --- a/annotation_gui_gcp/requirements.txt +++ b/annotation_gui_gcp/requirements.txt @@ -1 +1,2 @@ matplotlib +rasterio From 4ec625746aba6cd05c7176f9f6afa678106f28a0 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Thu, 18 Feb 2021 18:43:51 +0200 Subject: [PATCH 57/64] Read sequences from different folders --- annotation_gui_gcp/image_manager.py | 29 +++++- annotation_gui_gcp/main.py | 31 +++++- data/berlin/ground_control_points.json | 131 ++++++++++++++++++++++--- requirements.txt | 2 +- 4 files changed, 169 insertions(+), 24 deletions(-) diff --git a/annotation_gui_gcp/image_manager.py b/annotation_gui_gcp/image_manager.py index 82ed25a9b..1ac0b173f 100644 --- a/annotation_gui_gcp/image_manager.py +++ b/annotation_gui_gcp/image_manager.py @@ -1,5 +1,6 @@ import multiprocessing +import os import numpy as np from matplotlib.image import _rgb_to_rgba from opensfm import dataset @@ -9,6 +10,8 @@ def load_image(path): + if '.json' in path: + print(path) rgb = Image.open(path) # Reduce to some reasonable maximum size @@ -17,12 +20,16 @@ def load_image(path): new_w = int(round(rgb.size[0] / scale)) new_h = int(round(rgb.size[1] / scale)) rgb = rgb.resize((new_w, new_h), resample=Image.BILINEAR) + + if rgb.mode == 'L': + rgb = rgb.convert("RGB") # Matplotlib will transform to rgba when plotting return _rgb_to_rgba(np.asarray(rgb)) class ImageManager: - def __init__(self, seqs, path, preload_images=True): + def __init__(self, seqs, path, preload_images=True, skip_frames=1): + self.skip_frames = skip_frames self.seqs = seqs self.path = path self.image_cache = {} @@ -31,7 +38,8 @@ def __init__(self, seqs, path, preload_images=True): self.preload_images() def image_path(self, image_name): - return f"{self.path}/images/{image_name}" + print(image_name) + return f"{self.path}/{image_name}" def get_image(self, image_name): if image_name not in self.image_cache: @@ -63,8 +71,21 @@ def preload_images(self): image_names = [] for keys in self.seqs.values(): for k in keys: - image_names.append(k) - paths.append(self.image_path(k)) + image_path = os.path.join(self.path, k) + if os.path.isfile(image_path): + image_filename = os.path.basename(image_path) + # image_folder = os.path.dirname(image_path) + image_names.append(image_filename) + paths.append(image_path) + else: + image_names.append(k) + paths.append(self.image_path(k)) + + # skip frames + if self.skip_frames > 1: + image_names = image_names[::self.skip_frames] + paths = paths[::self.skip_frames] + pool = multiprocessing.Pool(processes=n_cpu) images = pool.map(load_image, paths) for image_name, im in zip(image_names, images): diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index 5be9cd9c4..b5c01c5cb 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -1,8 +1,9 @@ import argparse +import os import tkinter as tk from collections import OrderedDict, defaultdict from pathlib import Path - +from glob import glob from opensfm import dataset, io import GUI @@ -22,6 +23,12 @@ def parse_args(): help="If set, the UI will show one window per reconstruction, " "otherwise, it will use sequences as specified by 'sequence-file'", ) + parser.add_argument( + "--skip_frames", + type=int, + default=1, + help="Skip frames in sequences", + ) parser.add_argument( "--strict-missing", action="store_true", @@ -67,6 +74,19 @@ def parse_args(): def file_sanity_check(root, seq_dict, fname): # Images available under ./images for a sanity check + is_path_glob_expression = False + for seq_keys in seq_dict.values(): + if '*' in seq_keys: + is_path_glob_expression = True + + if is_path_glob_expression: + available_images = set() + for seq_keys in seq_dict.values(): + seq_images = sorted(glob(str(root / seq_keys))) + available_images.update(seq_images) + available_images = {x.replace(str(root) + '/', '') for x in available_images} + return available_images + available_images = {p.name for p in (root / "images").iterdir()} keys_in_seq_dict = {im_key for seq_keys in seq_dict.values() for im_key in seq_keys} @@ -98,7 +118,12 @@ def load_sequence_database_from_file( for skey in seq_dict: available_image_keys = [] - for k in seq_dict[skey]: + image_list = seq_dict[skey] + if '*' in seq_dict[skey]: + image_list = sorted(glob(str(root / seq_dict[skey]))) + seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) + continue + for k in image_list: if k in available_images: available_image_keys.append(k) elif not skip_missing: @@ -193,7 +218,7 @@ def group_images(args): args = parse_args() path = args.dataset groups, sequence_groups = group_images(args) - image_manager = ImageManager(groups, path, preload_images=not args.no_preload) + image_manager = ImageManager(groups, path, preload_images=not args.no_preload, skip_frames=args.skip_frames) gcp_manager = GroundControlPointManager(path) root = tk.Tk() root.resizable(True, True) diff --git a/data/berlin/ground_control_points.json b/data/berlin/ground_control_points.json index f5c080696..0d9d86f91 100644 --- a/data/berlin/ground_control_points.json +++ b/data/berlin/ground_control_points.json @@ -1,28 +1,35 @@ { "points": [ { - "position": { - "latitude": 52.51926834404209, - "altitude": 14.946108041331172, - "longitude": 13.400703631118825 - }, "id": "0", "observations": [ { "projection": [ - -0.015689, - -0.0625362 + -0.004333333333333258, + -0.035 + ], + "shot_id": "01.jpg" + }, + { + "projection": [ + 0.028222222222222284, + -0.171 ], "shot_id": "02.jpg" }, { "projection": [ - -0.0624397, - 0.0440716 + -0.0062222222222221716, + -0.08377777777777777 ], "shot_id": "03.jpg" } - ] + ], + "position": { + "altitude": 14.946108041331172, + "latitude": 52.51926834404209, + "longitude": 13.400703631118825 + } }, { "id": "1", @@ -44,21 +51,113 @@ ] }, { + "id": "2", + "observations": [ + { + "projection": [ + -0.2716666666666667, + -0.2441111111111111 + ], + "shot_id": "03.jpg" + }, + { + "projection": [ + -0.16166666666666663, + -0.25666666666666677 + ], + "shot_id": "02.jpg" + }, + { + "projection": [ + -0.16077777777777771, + -0.08133333333333331 + ], + "shot_id": "01.jpg" + } + ] + }, + { + "id": "3", + "observations": [ + { + "projection": [ + 0.20333333333333337, + 0.26333333333333325 + ], + "shot_id": "01.jpg" + }, + { + "projection": [ + 0.2955555555555557, + 0.16088888888888891 + ], + "shot_id": "02.jpg" + }, + { + "projection": [ + 0.44955555555555565, + 0.32411111111111107 + ], + "shot_id": "03.jpg" + } + ], "position": { - "latitude": 52.5192651808067, "altitude": 12.859175567515194, + "latitude": 52.5192651808067, "longitude": 13.400764257288497 - }, - "id": "3", + } + }, + { + "id": "4", + "observations": [ + { + "projection": [ + -0.22377777777777783, + 0.1567777777777777 + ], + "shot_id": "03.jpg" + }, + { + "projection": [ + -0.13666666666666663, + 0.01733333333333337 + ], + "shot_id": "02.jpg" + }, + { + "projection": [ + -0.15211111111111109, + 0.1323333333333333 + ], + "shot_id": "01.jpg" + } + ] + }, + { + "id": "5", "observations": [ { "projection": [ - 0.0608681, - -0.00730461 + 0.16333333333333336, + -0.048333333333333374 + ], + "shot_id": "01.jpg" + }, + { + "projection": [ + 0.2450000000000001, + -0.21666666666666673 ], "shot_id": "02.jpg" + }, + { + "projection": [ + 0.2771111111111112, + -0.19377777777777777 + ], + "shot_id": "03.jpg" } ] } ] -} +} \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index b3e3eb7ac..18c01485d 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,7 +5,7 @@ joblib==0.14.1 matplotlib networkx==1.11 numpy -Pillow==7.1.0 +Pillow pyproj>=1.9.5.1 pytest==3.0.7 python-dateutil==2.6.0 From 4635670d0830804d2b9b0743c7d63c46220a5ab0 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Sun, 21 Feb 2021 12:12:15 +0200 Subject: [PATCH 58/64] Different frame rates for different sequences --- annotation_gui_gcp/__init__.py | 0 annotation_gui_gcp/image_manager.py | 12 +++--------- annotation_gui_gcp/main.py | 22 +++++++++++++++++++--- 3 files changed, 22 insertions(+), 12 deletions(-) create mode 100644 annotation_gui_gcp/__init__.py diff --git a/annotation_gui_gcp/__init__.py b/annotation_gui_gcp/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/annotation_gui_gcp/image_manager.py b/annotation_gui_gcp/image_manager.py index 1ac0b173f..8bd77259f 100644 --- a/annotation_gui_gcp/image_manager.py +++ b/annotation_gui_gcp/image_manager.py @@ -28,8 +28,7 @@ def load_image(path): class ImageManager: - def __init__(self, seqs, path, preload_images=True, skip_frames=1): - self.skip_frames = skip_frames + def __init__(self, seqs, path, preload_images=True): self.seqs = seqs self.path = path self.image_cache = {} @@ -38,7 +37,7 @@ def __init__(self, seqs, path, preload_images=True, skip_frames=1): self.preload_images() def image_path(self, image_name): - print(image_name) + # print(image_name) return f"{self.path}/{image_name}" def get_image(self, image_name): @@ -73,19 +72,14 @@ def preload_images(self): for k in keys: image_path = os.path.join(self.path, k) if os.path.isfile(image_path): + # image_filename = image_path.replace(str(self.path) + '/', '') image_filename = os.path.basename(image_path) - # image_folder = os.path.dirname(image_path) image_names.append(image_filename) paths.append(image_path) else: image_names.append(k) paths.append(self.image_path(k)) - # skip frames - if self.skip_frames > 1: - image_names = image_names[::self.skip_frames] - paths = paths[::self.skip_frames] - pool = multiprocessing.Pool(processes=n_cpu) images = pool.map(load_image, paths) for image_name, im in zip(image_names, images): diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index b5c01c5cb..d57d6fab0 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -76,13 +76,21 @@ def file_sanity_check(root, seq_dict, fname): # Images available under ./images for a sanity check is_path_glob_expression = False for seq_keys in seq_dict.values(): - if '*' in seq_keys: + if '*' in seq_keys or isinstance(seq_keys, dict): is_path_glob_expression = True if is_path_glob_expression: available_images = set() for seq_keys in seq_dict.values(): - seq_images = sorted(glob(str(root / seq_keys))) + if isinstance(seq_keys, dict): + folder_glob_expression = seq_keys["folder"] + skip_frames = max(seq_keys["skip_frames"], 1) + seq_images = sorted(glob(str(root / folder_glob_expression))) + seq_images = seq_images[::skip_frames] + elif '*' in seq_keys: + seq_images = sorted(glob(str(root / seq_keys))) + else: + raise Exception(f"Input format not supported: {seq_dict}") available_images.update(seq_images) available_images = {x.replace(str(root) + '/', '') for x in available_images} return available_images @@ -123,6 +131,14 @@ def load_sequence_database_from_file( image_list = sorted(glob(str(root / seq_dict[skey]))) seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) continue + elif isinstance(seq_dict[skey], dict): + folder_glob_expression = seq_dict[skey]["folder"] + skip_frames = max(seq_dict[skey]["skip_frames"], 1) + image_list = sorted(glob(str(root / folder_glob_expression))) + image_list = image_list[::skip_frames] + assert len(image_list) > 0, f"No valid images found for: {folder_glob_expression}" + seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) + continue for k in image_list: if k in available_images: available_image_keys.append(k) @@ -218,7 +234,7 @@ def group_images(args): args = parse_args() path = args.dataset groups, sequence_groups = group_images(args) - image_manager = ImageManager(groups, path, preload_images=not args.no_preload, skip_frames=args.skip_frames) + image_manager = ImageManager(groups, path, preload_images=not args.no_preload) gcp_manager = GroundControlPointManager(path) root = tk.Tk() root.resizable(True, True) From 6d0c7cf27fba7922c0a380385cce1a7e1f7b5085 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Mon, 22 Feb 2021 08:51:21 +0200 Subject: [PATCH 59/64] skip_frames moved to json --- annotation_gui_gcp/main.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index d57d6fab0..c8b5cee6b 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -23,12 +23,6 @@ def parse_args(): help="If set, the UI will show one window per reconstruction, " "otherwise, it will use sequences as specified by 'sequence-file'", ) - parser.add_argument( - "--skip_frames", - type=int, - default=1, - help="Skip frames in sequences", - ) parser.add_argument( "--strict-missing", action="store_true", From e96a3d0fe07f70ac5d91a7316e8615109b779215 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Wed, 24 Feb 2021 00:30:31 +0200 Subject: [PATCH 60/64] networkx 1.11 won't work with python3.9 --- bin/opensfm_run_all | 6 +++--- requirements.txt | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/bin/opensfm_run_all b/bin/opensfm_run_all index b2c6cb22e..4eeb9ca0b 100755 --- a/bin/opensfm_run_all +++ b/bin/opensfm_run_all @@ -9,6 +9,6 @@ DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) "$DIR"/opensfm match_features "$1" "$DIR"/opensfm create_tracks "$1" "$DIR"/opensfm reconstruct "$1" -"$DIR"/opensfm mesh "$1" -"$DIR"/opensfm undistort "$1" -"$DIR"/opensfm compute_depthmaps "$1" +# "$DIR"/opensfm mesh "$1" +# "$DIR"/opensfm undistort "$1" +# "$DIR"/opensfm compute_depthmaps "$1" diff --git a/requirements.txt b/requirements.txt index 18c01485d..b7f8c0a47 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,7 +3,7 @@ exifread==2.1.2 fpdf2==2.1.0 joblib==0.14.1 matplotlib -networkx==1.11 +networkx==2.5.0 numpy Pillow pyproj>=1.9.5.1 From 9d4a6cd5acba001a5a322a47c3639a08347440b4 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Thu, 25 Feb 2021 18:46:39 +0200 Subject: [PATCH 61/64] Adding sequence graph --- annotation_gui_gcp/image_manager.py | 18 ++------ annotation_gui_gcp/main.py | 66 +++++++++++++-------------- bin/opensfm | 1 + bin/opensfm_run_all | 14 +++--- opensfm/actions/compute_statistics.py | 1 + opensfm/dataset.py | 4 +- opensfm/reconstruction.py | 1 + opensfm/stats.py | 54 +++++++++++++++++++++- 8 files changed, 101 insertions(+), 58 deletions(-) diff --git a/annotation_gui_gcp/image_manager.py b/annotation_gui_gcp/image_manager.py index 8bd77259f..5012e9e1b 100644 --- a/annotation_gui_gcp/image_manager.py +++ b/annotation_gui_gcp/image_manager.py @@ -1,6 +1,5 @@ import multiprocessing -import os import numpy as np from matplotlib.image import _rgb_to_rgba from opensfm import dataset @@ -10,8 +9,6 @@ def load_image(path): - if '.json' in path: - print(path) rgb = Image.open(path) # Reduce to some reasonable maximum size @@ -37,8 +34,7 @@ def __init__(self, seqs, path, preload_images=True): self.preload_images() def image_path(self, image_name): - # print(image_name) - return f"{self.path}/{image_name}" + return f"{self.path}/images/{image_name}" def get_image(self, image_name): if image_name not in self.image_cache: @@ -70,16 +66,8 @@ def preload_images(self): image_names = [] for keys in self.seqs.values(): for k in keys: - image_path = os.path.join(self.path, k) - if os.path.isfile(image_path): - # image_filename = image_path.replace(str(self.path) + '/', '') - image_filename = os.path.basename(image_path) - image_names.append(image_filename) - paths.append(image_path) - else: - image_names.append(k) - paths.append(self.image_path(k)) - + image_names.append(k) + paths.append(self.image_path(k)) pool = multiprocessing.Pool(processes=n_cpu) images = pool.map(load_image, paths) for image_name, im in zip(image_names, images): diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index c8b5cee6b..e9bd60d9d 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -1,9 +1,7 @@ import argparse -import os import tkinter as tk from collections import OrderedDict, defaultdict from pathlib import Path -from glob import glob from opensfm import dataset, io import GUI @@ -68,26 +66,26 @@ def parse_args(): def file_sanity_check(root, seq_dict, fname): # Images available under ./images for a sanity check - is_path_glob_expression = False - for seq_keys in seq_dict.values(): - if '*' in seq_keys or isinstance(seq_keys, dict): - is_path_glob_expression = True - - if is_path_glob_expression: - available_images = set() - for seq_keys in seq_dict.values(): - if isinstance(seq_keys, dict): - folder_glob_expression = seq_keys["folder"] - skip_frames = max(seq_keys["skip_frames"], 1) - seq_images = sorted(glob(str(root / folder_glob_expression))) - seq_images = seq_images[::skip_frames] - elif '*' in seq_keys: - seq_images = sorted(glob(str(root / seq_keys))) - else: - raise Exception(f"Input format not supported: {seq_dict}") - available_images.update(seq_images) - available_images = {x.replace(str(root) + '/', '') for x in available_images} - return available_images + # is_path_glob_expression = False + # for seq_keys in seq_dict.values(): + # if '*' in seq_keys or isinstance(seq_keys, dict): + # is_path_glob_expression = True + # + # if is_path_glob_expression: + # available_images = set() + # for seq_keys in seq_dict.values(): + # if isinstance(seq_keys, dict): + # folder_glob_expression = seq_keys["folder"] + # skip_frames = max(seq_keys["skip_frames"], 1) + # seq_images = sorted(glob(str(root / folder_glob_expression))) + # seq_images = seq_images[::skip_frames] + # elif '*' in seq_keys: + # seq_images = sorted(glob(str(root / seq_keys))) + # else: + # raise Exception(f"Input format not supported: {seq_dict}") + # available_images.update(seq_images) + # available_images = {x.replace(str(root) + '/', '') for x in available_images} + # return available_images available_images = {p.name for p in (root / "images").iterdir()} keys_in_seq_dict = {im_key for seq_keys in seq_dict.values() for im_key in seq_keys} @@ -121,18 +119,18 @@ def load_sequence_database_from_file( for skey in seq_dict: available_image_keys = [] image_list = seq_dict[skey] - if '*' in seq_dict[skey]: - image_list = sorted(glob(str(root / seq_dict[skey]))) - seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) - continue - elif isinstance(seq_dict[skey], dict): - folder_glob_expression = seq_dict[skey]["folder"] - skip_frames = max(seq_dict[skey]["skip_frames"], 1) - image_list = sorted(glob(str(root / folder_glob_expression))) - image_list = image_list[::skip_frames] - assert len(image_list) > 0, f"No valid images found for: {folder_glob_expression}" - seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) - continue + # if '*' in seq_dict[skey]: + # image_list = sorted(glob(str(root / seq_dict[skey]))) + # seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) + # continue + # elif isinstance(seq_dict[skey], dict): + # folder_glob_expression = seq_dict[skey]["folder"] + # skip_frames = max(seq_dict[skey]["skip_frames"], 1) + # image_list = sorted(glob(str(root / folder_glob_expression))) + # image_list = image_list[::skip_frames] + # assert len(image_list) > 0, f"No valid images found for: {folder_glob_expression}" + # seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) + # continue for k in image_list: if k in available_images: available_image_keys.append(k) diff --git a/bin/opensfm b/bin/opensfm index b5ee4b15d..62a50af95 100755 --- a/bin/opensfm +++ b/bin/opensfm @@ -9,4 +9,5 @@ else PYTHON=python fi +echo "${PYTHON} ${DIR}/opensfm_main.py $@" "$PYTHON" "$DIR"/opensfm_main.py "$@" diff --git a/bin/opensfm_run_all b/bin/opensfm_run_all index 4eeb9ca0b..7c17707d6 100755 --- a/bin/opensfm_run_all +++ b/bin/opensfm_run_all @@ -3,12 +3,14 @@ set -e DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) - -"$DIR"/opensfm extract_metadata "$1" -"$DIR"/opensfm detect_features "$1" -"$DIR"/opensfm match_features "$1" -"$DIR"/opensfm create_tracks "$1" -"$DIR"/opensfm reconstruct "$1" +echo ${DIR} +./opensfm extract_metadata "$1" +./opensfm detect_features "$1" +./opensfm match_features "$1" +./opensfm create_tracks "$1" +./opensfm reconstruct "$1" # "$DIR"/opensfm mesh "$1" # "$DIR"/opensfm undistort "$1" # "$DIR"/opensfm compute_depthmaps "$1" +./opensfm compute_statistics "$1" +./opensfm export_report "$1" diff --git a/opensfm/actions/compute_statistics.py b/opensfm/actions/compute_statistics.py index d2c4c3e24..f4b906f84 100644 --- a/opensfm/actions/compute_statistics.py +++ b/opensfm/actions/compute_statistics.py @@ -22,6 +22,7 @@ def run_dataset(data): stats_dict = stats.compute_all_statistics(data, tracks_manager, reconstructions) + stats.save_sequencegraph(data, reconstructions, output_path) stats.save_residual_grids(data, tracks_manager, reconstructions, output_path) stats.save_matchgraph(data, tracks_manager, reconstructions, output_path) stats.save_heatmap(data, tracks_manager, reconstructions, output_path) diff --git a/opensfm/dataset.py b/opensfm/dataset.py index f5a0a2fc8..797b78651 100644 --- a/opensfm/dataset.py +++ b/opensfm/dataset.py @@ -488,8 +488,8 @@ def invent_reference_lla(self, images=None): if not wlat and not wlon: for gcp in self.load_ground_control_points_impl(None): - lat += gcp.lla["latitude"] - lon += gcp.lla["longitude"] + lat += gcp.lla.get("latitude", 0.) + lon += gcp.lla.get("longitude", 0.) wlat += 1 wlon += 1 diff --git a/opensfm/reconstruction.py b/opensfm/reconstruction.py index 8fe66a51b..dc375f7c2 100644 --- a/opensfm/reconstruction.py +++ b/opensfm/reconstruction.py @@ -282,6 +282,7 @@ def get_image_metadata(data, image): metadata.compass_accuracy.value = exif["compass"]["accuracy"] if "capture_time" in exif: + print(exif) metadata.capture_time.value = exif["capture_time"] if "skey" in exif: diff --git a/opensfm/stats.py b/opensfm/stats.py index da6d6a1fe..91bb7fdba 100644 --- a/opensfm/stats.py +++ b/opensfm/stats.py @@ -2,7 +2,7 @@ import math import os import statistics -from collections import defaultdict +from collections import defaultdict, OrderedDict from functools import lru_cache import matplotlib.cm as cm @@ -380,6 +380,53 @@ def save_matchgraph(data, tracks_manager, reconstructions, output_path): ) +def load_sequence_database_from_file(data_path, fname="sequence_database.json"): + """ + Simply loads a sequence file and returns it. + This doesn't require an existing SfM reconstruction + """ + p_json = os.path.join(data_path, fname) + if not os.path.isfile(p_json): + return None + seq_dict = OrderedDict(io.json_load(open(p_json, "r"))) + + return seq_dict + + +def save_sequencegraph(data, reconstructions, output_path): + shot_sequences = {} + data_path = data.data_path + seq_dict = load_sequence_database_from_file(data_path) + sequences = list(seq_dict.keys()) + image_xyz_per_sequences = {seq: [] for seq in sequences} + + for i, rec in enumerate(reconstructions): + for shot_name, shot in rec.shots.items(): + shot_sequence = None + shot_sequences[shot_name] = None + for seq, shot_list in seq_dict.items(): + if shot_name in shot_list: + shot_sequence = seq + break + xyz = shot.pose.get_origin() + image_xyz_per_sequences[shot_sequence].append(xyz) + + plt.clf() + cmap = cm.get_cmap("rainbow") + + for seq, image_xyz in image_xyz_per_sequences.items(): + xyz = np.array(image_xyz) + c = sequences.index(seq) / len(sequences) + plt.scatter(xyz[:, 0], xyz[:, 2], color=cmap(c), label=seq) + plt.legend() + + plt.savefig( + os.path.join(output_path, "sequencegraph.png"), + dpi=300, + bbox_inches="tight", + ) + + def save_topview(data, tracks_manager, reconstructions, output_path): points = [] colors = [] @@ -410,6 +457,8 @@ def save_topview(data, tracks_manager, reconstructions, output_path): if not shot.metadata.gps_position.has_value: continue gps = shot.metadata.gps_position.value + if gps[0] == gps[1] == gps[2] == 0.: + continue all_x.append(gps[0]) all_y.append(gps[1]) @@ -506,6 +555,9 @@ def save_topview(data, tracks_manager, reconstructions, output_path): if not shot.metadata.gps_position.has_value: continue gps = shot.metadata.gps_position.value + if gps[0] == gps[1] == gps[2] == 0.: + continue + gps_x, gps_y = int((gps[0] - low_x) / size_x * im_size_x), int( (gps[1] - low_y) / size_y * im_size_y ) From 339bfaca1ef1a8e3629c32e61dcb88d7fa6d4525 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Thu, 25 Feb 2021 18:54:08 +0200 Subject: [PATCH 62/64] Code clean up --- annotation_gui_gcp/main.py | 37 +------ bin/opensfm_run_all | 16 +-- data/berlin/ground_control_points.json | 131 +++---------------------- 3 files changed, 26 insertions(+), 158 deletions(-) diff --git a/annotation_gui_gcp/main.py b/annotation_gui_gcp/main.py index e9bd60d9d..5be9cd9c4 100644 --- a/annotation_gui_gcp/main.py +++ b/annotation_gui_gcp/main.py @@ -2,6 +2,7 @@ import tkinter as tk from collections import OrderedDict, defaultdict from pathlib import Path + from opensfm import dataset, io import GUI @@ -66,27 +67,6 @@ def parse_args(): def file_sanity_check(root, seq_dict, fname): # Images available under ./images for a sanity check - # is_path_glob_expression = False - # for seq_keys in seq_dict.values(): - # if '*' in seq_keys or isinstance(seq_keys, dict): - # is_path_glob_expression = True - # - # if is_path_glob_expression: - # available_images = set() - # for seq_keys in seq_dict.values(): - # if isinstance(seq_keys, dict): - # folder_glob_expression = seq_keys["folder"] - # skip_frames = max(seq_keys["skip_frames"], 1) - # seq_images = sorted(glob(str(root / folder_glob_expression))) - # seq_images = seq_images[::skip_frames] - # elif '*' in seq_keys: - # seq_images = sorted(glob(str(root / seq_keys))) - # else: - # raise Exception(f"Input format not supported: {seq_dict}") - # available_images.update(seq_images) - # available_images = {x.replace(str(root) + '/', '') for x in available_images} - # return available_images - available_images = {p.name for p in (root / "images").iterdir()} keys_in_seq_dict = {im_key for seq_keys in seq_dict.values() for im_key in seq_keys} @@ -118,20 +98,7 @@ def load_sequence_database_from_file( for skey in seq_dict: available_image_keys = [] - image_list = seq_dict[skey] - # if '*' in seq_dict[skey]: - # image_list = sorted(glob(str(root / seq_dict[skey]))) - # seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) - # continue - # elif isinstance(seq_dict[skey], dict): - # folder_glob_expression = seq_dict[skey]["folder"] - # skip_frames = max(seq_dict[skey]["skip_frames"], 1) - # image_list = sorted(glob(str(root / folder_glob_expression))) - # image_list = image_list[::skip_frames] - # assert len(image_list) > 0, f"No valid images found for: {folder_glob_expression}" - # seq_dict[skey] = sorted([x.replace(str(root) + '/', '') for x in image_list]) - # continue - for k in image_list: + for k in seq_dict[skey]: if k in available_images: available_image_keys.append(k) elif not skip_missing: diff --git a/bin/opensfm_run_all b/bin/opensfm_run_all index 7c17707d6..52508c65b 100755 --- a/bin/opensfm_run_all +++ b/bin/opensfm_run_all @@ -3,14 +3,14 @@ set -e DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -echo ${DIR} -./opensfm extract_metadata "$1" -./opensfm detect_features "$1" -./opensfm match_features "$1" -./opensfm create_tracks "$1" -./opensfm reconstruct "$1" + +"$DIR"/opensfm extract_metadata "$1" +"$DIR"/opensfm detect_features "$1" +"$DIR"/opensfm match_features "$1" +"$DIR"/opensfm create_tracks "$1" +"$DIR"/opensfm reconstruct "$1" # "$DIR"/opensfm mesh "$1" # "$DIR"/opensfm undistort "$1" # "$DIR"/opensfm compute_depthmaps "$1" -./opensfm compute_statistics "$1" -./opensfm export_report "$1" +"$DIR"/opensfm compute_statistics "$1" +"$DIR"/opensfm export_report "$1" \ No newline at end of file diff --git a/data/berlin/ground_control_points.json b/data/berlin/ground_control_points.json index 0d9d86f91..f5c080696 100644 --- a/data/berlin/ground_control_points.json +++ b/data/berlin/ground_control_points.json @@ -1,35 +1,28 @@ { "points": [ { + "position": { + "latitude": 52.51926834404209, + "altitude": 14.946108041331172, + "longitude": 13.400703631118825 + }, "id": "0", "observations": [ { "projection": [ - -0.004333333333333258, - -0.035 - ], - "shot_id": "01.jpg" - }, - { - "projection": [ - 0.028222222222222284, - -0.171 + -0.015689, + -0.0625362 ], "shot_id": "02.jpg" }, { "projection": [ - -0.0062222222222221716, - -0.08377777777777777 + -0.0624397, + 0.0440716 ], "shot_id": "03.jpg" } - ], - "position": { - "altitude": 14.946108041331172, - "latitude": 52.51926834404209, - "longitude": 13.400703631118825 - } + ] }, { "id": "1", @@ -51,113 +44,21 @@ ] }, { - "id": "2", - "observations": [ - { - "projection": [ - -0.2716666666666667, - -0.2441111111111111 - ], - "shot_id": "03.jpg" - }, - { - "projection": [ - -0.16166666666666663, - -0.25666666666666677 - ], - "shot_id": "02.jpg" - }, - { - "projection": [ - -0.16077777777777771, - -0.08133333333333331 - ], - "shot_id": "01.jpg" - } - ] - }, - { - "id": "3", - "observations": [ - { - "projection": [ - 0.20333333333333337, - 0.26333333333333325 - ], - "shot_id": "01.jpg" - }, - { - "projection": [ - 0.2955555555555557, - 0.16088888888888891 - ], - "shot_id": "02.jpg" - }, - { - "projection": [ - 0.44955555555555565, - 0.32411111111111107 - ], - "shot_id": "03.jpg" - } - ], "position": { - "altitude": 12.859175567515194, "latitude": 52.5192651808067, + "altitude": 12.859175567515194, "longitude": 13.400764257288497 - } - }, - { - "id": "4", - "observations": [ - { - "projection": [ - -0.22377777777777783, - 0.1567777777777777 - ], - "shot_id": "03.jpg" - }, - { - "projection": [ - -0.13666666666666663, - 0.01733333333333337 - ], - "shot_id": "02.jpg" - }, - { - "projection": [ - -0.15211111111111109, - 0.1323333333333333 - ], - "shot_id": "01.jpg" - } - ] - }, - { - "id": "5", + }, + "id": "3", "observations": [ { "projection": [ - 0.16333333333333336, - -0.048333333333333374 - ], - "shot_id": "01.jpg" - }, - { - "projection": [ - 0.2450000000000001, - -0.21666666666666673 + 0.0608681, + -0.00730461 ], "shot_id": "02.jpg" - }, - { - "projection": [ - 0.2771111111111112, - -0.19377777777777777 - ], - "shot_id": "03.jpg" } ] } ] -} \ No newline at end of file +} From 39f71e0fad1e5987affec33247f023de3cc029d7 Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Thu, 25 Feb 2021 19:05:46 +0200 Subject: [PATCH 63/64] Removing print --- opensfm/reconstruction.py | 1 - 1 file changed, 1 deletion(-) diff --git a/opensfm/reconstruction.py b/opensfm/reconstruction.py index dc375f7c2..8fe66a51b 100644 --- a/opensfm/reconstruction.py +++ b/opensfm/reconstruction.py @@ -282,7 +282,6 @@ def get_image_metadata(data, image): metadata.compass_accuracy.value = exif["compass"]["accuracy"] if "capture_time" in exif: - print(exif) metadata.capture_time.value = exif["capture_time"] if "skey" in exif: From f23d3ae12d6af600953a0592fb8f9559839cd6ed Mon Sep 17 00:00:00 2001 From: Yonatan Simson Date: Sun, 28 Feb 2021 13:26:32 +0200 Subject: [PATCH 64/64] Improving the plot --- opensfm/stats.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/opensfm/stats.py b/opensfm/stats.py index 91bb7fdba..32392160e 100644 --- a/opensfm/stats.py +++ b/opensfm/stats.py @@ -417,8 +417,12 @@ def save_sequencegraph(data, reconstructions, output_path): for seq, image_xyz in image_xyz_per_sequences.items(): xyz = np.array(image_xyz) c = sequences.index(seq) / len(sequences) - plt.scatter(xyz[:, 0], xyz[:, 2], color=cmap(c), label=seq) + plt.scatter(xyz[:, 1], xyz[:, 2], color=cmap(c), label=seq, s=2) plt.legend() + plt.grid() + plt.xlabel('Y[m]') + plt.ylabel('Z[m]') + plt.axis('equal') plt.savefig( os.path.join(output_path, "sequencegraph.png"),