def add_pano_subshot_tracks(
tracks_manager: pymap.TracksManager,
utracks_manager: pymap.TracksManager,
panoshot: pymap.Shot,
perspectiveshot: pymap.Shot,
) -> None:
"""Add edges between subshots and visible tracks."""
for track_id, obs in tracks_manager.get_shot_observations(panoshot.id).items():
bearing = panoshot.camera.pixel_bearing(obs.point)
rotation = np.dot(
perspectiveshot.pose.get_rotation_matrix(),
panoshot.pose.get_rotation_matrix().T,
)
rotated_bearing = np.dot(bearing, rotation.T)
if rotated_bearing[2] <= 0:
continue
perspective_feature = perspectiveshot.camera.project(rotated_bearing)
if (
perspective_feature[0] < -0.5
or perspective_feature[0] > 0.5
or perspective_feature[1] < -0.5
or perspective_feature[1] > 0.5
):
continue
obs.point = perspective_feature
utracks_manager.add_observation(perspectiveshot.id, track_id, obs)
def all_common_tracks(
tracks_manager: pymap.TracksManager,
include_features: bool = True,
min_common: int = 50,
) -> t.Dict[t.Tuple[str, str], t.Union[TPairTracks, t.List[str]]]:
"""List of tracks observed by each image pair.
Args:
tracks_manager: tracks manager
include_features: whether to include the features from the images
min_common: the minimum number of tracks the two images need to have
in common
Returns:
tuple: im1, im2 -> tuple: tracks, features from first image, features
from second image
"""
common_tracks = {}
for (im1,
im2), size in tracks_manager.get_all_pairs_connectivity().items():
if size < min_common:
continue
tuples = tracks_manager.get_all_common_observations(im1, im2)
if include_features:
common_tracks[im1, im2] = (
[v for v, _, _ in tuples],
np.array([p.point for _, p, _ in tuples]),
np.array([p.point for _, _, p in tuples]),
)
else:
common_tracks[im1, im2] = [v for v, _, _ in tuples]
return common_tracks
def retriangulate(
tracks_manager: pymap.TracksManager,
reconstruction: types.Reconstruction,
config: Dict[str, Any],
) -> Dict[str, Any]:
"""Retrianguate all points"""
chrono = Chronometer()
report = {}
report["num_points_before"] = len(reconstruction.points)
threshold = config["triangulation_threshold"]
min_ray_angle = config["triangulation_min_ray_angle"]
reconstruction.points = {}
all_shots_ids = set(tracks_manager.get_shot_ids())
triangulator = TrackTriangulator(tracks_manager, reconstruction)
tracks = set()
for image in reconstruction.shots.keys():
if image in all_shots_ids:
tracks.update(tracks_manager.get_shot_observations(image).keys())
for track in tracks:
if config["triangulation_type"] == "ROBUST":
triangulator.triangulate_robust(track, threshold, min_ray_angle)
elif config["triangulation_type"] == "FULL":
triangulator.triangulate(track, threshold, min_ray_angle)
report["num_points_after"] = len(reconstruction.points)
chrono.lap("retriangulate")
report["wall_time"] = chrono.total_time()
return report
def add_correspondences_from_tracks_manager(
self, tracks_manager: pymap.TracksManager) -> None:
for track_id in tracks_manager.get_track_ids():
if track_id not in self.points:
continue
track_obs = tracks_manager.get_track_observations(track_id)
for shot_id in track_obs.keys():
if shot_id in self.shots:
observation = tracks_manager.get_observation(
shot_id, track_id)
self.add_observation(shot_id, track_id, observation)
def as_weighted_graph(tracks_manager: pymap.TracksManager) -> nx.Graph:
"""Return the tracks manager as a weighted graph
having shots a snodes and weighted by the # of
common tracks between two nodes.
"""
images = tracks_manager.get_shot_ids()
image_graph = nx.Graph()
for im in images:
image_graph.add_node(im)
for k, v in tracks_manager.get_all_pairs_connectivity().items():
image_graph.add_edge(k[0], k[1], weight=v)
return image_graph
def add_subshot_tracks(
tracks_manager: pymap.TracksManager,
utracks_manager: pymap.TracksManager,
shot: pymap.Shot,
subshot: pymap.Shot,
) -> None:
"""Add shot tracks to the undistorted tracks_manager."""
if shot.id not in tracks_manager.get_shot_ids():
return
if pygeometry.Camera.is_panorama(shot.camera.projection_type):
add_pano_subshot_tracks(tracks_manager, utracks_manager, shot, subshot)
else:
for track_id, obs in tracks_manager.get_shot_observations(shot.id).items():
utracks_manager.add_observation(subshot.id, track_id, obs)
def features_statistics(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
reconstructions: List[types.Reconstruction],
) -> Dict[str, Any]:
stats = {}
detected = []
images = {s for r in reconstructions for s in r.shots}
for im in images:
features_data = feature_loader.instance.load_all_data(
data, im, False, False)
if not features_data:
continue
detected.append(len(features_data.points))
if len(detected) > 0:
stats["detected_features"] = {
"min": min(detected),
"max": max(detected),
"mean": int(np.mean(detected)),
"median": int(np.median(detected)),
}
else:
stats["detected_features"] = {
"min": -1,
"max": -1,
"mean": -1,
"median": -1
}
per_shots = defaultdict(int)
for rec in reconstructions:
all_points_keys = set(rec.points.keys())
for shot_id in rec.shots:
if shot_id not in tracks_manager.get_shot_ids():
continue
for point_id in tracks_manager.get_shot_observations(shot_id):
if point_id not in all_points_keys:
continue
per_shots[shot_id] += 1
per_shots = list(per_shots.values())
stats["reconstructed_features"] = {
"min": int(min(per_shots)) if len(per_shots) > 0 else -1,
"max": int(max(per_shots)) if len(per_shots) > 0 else -1,
"mean": int(np.mean(per_shots)) if len(per_shots) > 0 else -1,
"median": int(np.median(per_shots)) if len(per_shots) > 0 else -1,
}
return stats
def triangle_mesh_spherical(
shot_id: str, r: types.Reconstruction,
tracks_manager: pymap.TracksManager) -> Tuple[List[Any], List[Any]]:
shot = r.shots[shot_id]
bearings = []
vertices = []
# Add vertices to ensure that the camera is inside the convex hull
# of the points
for point in itertools.product([-1, 1], repeat=3): # vertices of a cube
bearing = 0.3 * np.array(point) / np.linalg.norm(point)
bearings.append(bearing)
point = shot.pose.transform_inverse(bearing)
vertices.append(point.tolist())
for track_id in tracks_manager.get_shot_observations(shot_id):
if track_id in r.points:
point = r.points[track_id].coordinates
direction = shot.pose.transform(point)
pixel = direction / np.linalg.norm(direction)
if not np.isnan(pixel).any():
vertices.append(point)
bearings.append(pixel.tolist())
tri = scipy.spatial.ConvexHull(bearings)
faces = tri.simplices.tolist()
return vertices, faces
def triangle_mesh(shot_id: str, r: types.Reconstruction,
tracks_manager: pymap.TracksManager):
"""
Create triangle meshes in a list
"""
if shot_id not in r.shots or shot_id not in tracks_manager.get_shot_ids():
return [], []
shot = r.shots[shot_id]
if shot.camera.projection_type in [
"perspective",
"brown",
"radial",
"simple_radial",
]:
return triangle_mesh_perspective(shot_id, r, tracks_manager)
elif shot.camera.projection_type in [
"fisheye",
"fisheye_opencv",
"fisheye62",
"fisheye624",
"dual",
]:
return triangle_mesh_fisheye(shot_id, r, tracks_manager)
elif pygeometry.Camera.is_panorama(shot.camera.projection_type):
return triangle_mesh_spherical(shot_id, r, tracks_manager)
else:
raise NotImplementedError(
f"triangle_mesh not implemented for projection type {shot.camera.projection_type}"
)
def reconstruct_from_prior(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
rec_prior: types.Reconstruction,
) -> Tuple[Dict[str, Any], types.Reconstruction]:
"""Retriangulate a new reconstruction from the rec_prior"""
reconstruction = types.Reconstruction()
report = {}
rec_report = {}
report["retriangulate"] = [rec_report]
images = tracks_manager.get_shot_ids()
# copy prior poses, cameras
reconstruction.cameras = rec_prior.cameras
for shot in rec_prior.shots.values():
reconstruction.add_shot(shot)
prior_images = set(rec_prior.shots)
remaining_images = set(images) - prior_images
rec_report["num_prior_images"] = len(prior_images)
rec_report["num_remaining_images"] = len(remaining_images)
# Start with the known poses
triangulate_shot_features(tracks_manager, reconstruction, prior_images,
data.config)
paint_reconstruction(data, tracks_manager, reconstruction)
report["not_reconstructed_images"] = list(remaining_images)
return report, reconstruction
def add_observation_to_reconstruction(
tracks_manager: pymap.TracksManager,
reconstruction: types.Reconstruction,
shot_id: str,
track_id: str,
) -> None:
observation = tracks_manager.get_observation(shot_id, track_id)
reconstruction.add_observation(shot_id, track_id, observation)
def _length_histogram(
tracks_manager: pymap.TracksManager,
points: Dict[str, pymap.Landmark]) -> Tuple[List[str], List[int]]:
hist = defaultdict(int)
for point in points.values():
obs_count = point.number_of_observations()
if not obs_count:
obs_count = len(tracks_manager.get_track_observations(point.id))
hist[obs_count] += 1
return list(hist.keys()), list(hist.values())
def compute_common_tracks(
reconstruction1: types.Reconstruction,
reconstruction2: types.Reconstruction,
tracks_manager1: pymap.TracksManager,
tracks_manager2: pymap.TracksManager,
) -> List[Tuple[str, str]]:
common_tracks = set()
common_images = set(reconstruction1.shots.keys()).intersection(
reconstruction2.shots.keys())
all_shot_ids1 = set(tracks_manager1.get_shot_ids())
all_shot_ids2 = set(tracks_manager2.get_shot_ids())
for image in common_images:
if image not in all_shot_ids1 or image not in all_shot_ids2:
continue
at_shot1 = tracks_manager1.get_shot_observations(image)
at_shot2 = tracks_manager2.get_shot_observations(image)
for t1, t2 in corresponding_tracks(at_shot1, at_shot2):
if t1 in reconstruction1.points and t2 in reconstruction2.points:
common_tracks.add((t1, t2))
return list(common_tracks)
def triangulate_shot_features(
tracks_manager: pymap.TracksManager,
reconstruction: types.Reconstruction,
shot_ids: Set[str],
config: Dict[str, Any],
) -> None:
"""Reconstruct as many tracks seen in shot_id as possible."""
reproj_threshold = config["triangulation_threshold"]
min_ray_angle = config["triangulation_min_ray_angle"]
triangulator = TrackTriangulator(tracks_manager, reconstruction)
all_shots_ids = set(tracks_manager.get_shot_ids())
tracks_ids = {
t
for s in shot_ids if s in all_shots_ids
for t in tracks_manager.get_shot_observations(s)
}
for track in tracks_ids:
if track not in reconstruction.points:
triangulator.triangulate(track, reproj_threshold, min_ray_angle)
def incremental_reconstruction(
data: DataSetBase, tracks_manager: pymap.TracksManager
) -> Tuple[Dict[str, Any], List[types.Reconstruction]]:
"""Run the entire incremental reconstruction pipeline."""
logger.info("Starting incremental reconstruction")
report = {}
chrono = Chronometer()
images = tracks_manager.get_shot_ids()
if not data.reference_lla_exists():
data.invent_reference_lla(images)
remaining_images = set(images)
gcp = data.load_ground_control_points()
common_tracks = tracking.all_common_tracks_with_features(tracks_manager)
reconstructions = []
pairs = compute_image_pairs(common_tracks, data)
chrono.lap("compute_image_pairs")
report["num_candidate_image_pairs"] = len(pairs)
report["reconstructions"] = []
for im1, im2 in pairs:
if im1 in remaining_images and im2 in remaining_images:
rec_report = {}
report["reconstructions"].append(rec_report)
_, p1, p2 = common_tracks[im1, im2]
reconstruction, rec_report["bootstrap"] = bootstrap_reconstruction(
data, tracks_manager, im1, im2, p1, p2)
if reconstruction:
remaining_images -= set(reconstruction.shots)
reconstruction, rec_report["grow"] = grow_reconstruction(
data,
tracks_manager,
reconstruction,
remaining_images,
gcp,
)
reconstructions.append(reconstruction)
reconstructions = sorted(reconstructions,
key=lambda x: -len(x.shots))
for k, r in enumerate(reconstructions):
logger.info("Reconstruction {}: {} images, {} points".format(
k, len(r.shots), len(r.points)))
logger.info("{} partial reconstructions in total.".format(
len(reconstructions)))
chrono.lap("compute_reconstructions")
report["wall_times"] = dict(chrono.lap_times())
report["not_reconstructed_images"] = list(remaining_images)
return report, reconstructions
def as_graph(tracks_manager: pymap.TracksManager) -> nx.Graph:
"""Return the tracks manager as a bipartite graph (legacy)."""
tracks = tracks_manager.get_track_ids()
images = tracks_manager.get_shot_ids()
graph = nx.Graph()
for track_id in tracks:
graph.add_node(track_id, bipartite=1)
for shot_id in images:
graph.add_node(shot_id, bipartite=0)
for track_id in tracks:
for im, obs in tracks_manager.get_track_observations(track_id).items():
graph.add_edge(
im,
track_id,
feature=obs.point,
feature_scale=obs.scale,
feature_id=obs.id,
feature_color=obs.color,
feature_segmentation=obs.segmentation,
feature_instance=obs.instance,
)
return graph
def common_tracks_double_dict(
tracks_manager: pymap.TracksManager,
) -> t.Dict[str, t.Dict[str, t.List[str]]]:
"""List of track ids observed by each image pair.
Return a dict, ``res``, such that ``res[im1][im2]`` is the list of
common tracks between ``im1`` and ``im2``.
"""
common_tracks_per_pair = tracking.all_common_tracks_without_features(
tracks_manager)
res = {image: {} for image in tracks_manager.get_shot_ids()}
for (im1, im2), v in common_tracks_per_pair.items():
res[im1][im2] = v
res[im2][im1] = v
return res
def paint_reconstruction(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
reconstruction: types.Reconstruction,
) -> None:
"""Set the color of the points from the color of the tracks."""
for k, point in reconstruction.points.items():
point.color = list(
map(
float,
next(
iter(
tracks_manager.get_track_observations(
str(k)).values())).color,
))
def triangle_mesh_fisheye(
shot_id: str, r: types.Reconstruction,
tracks_manager: pymap.TracksManager) -> Tuple[List[Any], List[Any]]:
shot = r.shots[shot_id]
bearings = []
vertices = []
# Add boundary vertices
num_circle_points = 20
for i in range(num_circle_points):
a = 2 * np.pi * float(i) / num_circle_points
point = 30 * np.array([np.cos(a), np.sin(a), 0])
bearing = point / np.linalg.norm(point)
point = shot.pose.transform_inverse(point)
vertices.append(point.tolist())
bearings.append(bearing)
# Add a single vertex in front of the camera
point = 30 * np.array([0, 0, 1])
bearing = 0.3 * point / np.linalg.norm(point)
point = shot.pose.transform_inverse(point)
vertices.append(point.tolist())
bearings.append(bearing)
# Add reconstructed points
for track_id in tracks_manager.get_shot_observations(shot_id):
if track_id in r.points:
point = r.points[track_id].coordinates
direction = shot.pose.transform(point)
pixel = direction / np.linalg.norm(direction)
if not np.isnan(pixel).any():
vertices.append(point)
bearings.append(pixel.tolist())
# Triangulate
tri = scipy.spatial.ConvexHull(bearings)
faces = tri.simplices.tolist()
# Remove faces having only boundary vertices
def good_face(face):
return (face[0] >= num_circle_points or face[1] >= num_circle_points
or face[2] >= num_circle_points)
faces = list(filter(good_face, faces))
return vertices, faces
def triangle_mesh_perspective(
shot_id: str, r: types.Reconstruction,
tracks_manager: pymap.TracksManager) -> Tuple[List[Any], List[Any]]:
shot = r.shots[shot_id]
cam = shot.camera
dx = float(cam.width) / 2 / max(cam.width, cam.height)
dy = float(cam.height) / 2 / max(cam.width, cam.height)
pixels = [[-dx, -dy], [-dx, dy], [dx, dy], [dx, -dy]]
vertices = [None for i in range(4)]
for track_id in tracks_manager.get_shot_observations(shot_id):
if track_id in r.points:
point = r.points[track_id]
pixel = shot.project(point.coordinates)
nonans = not np.isnan(pixel).any()
if nonans and -dx <= pixel[0] <= dx and -dy <= pixel[1] <= dy:
vertices.append(point.coordinates)
pixels.append(pixel.tolist())
try:
tri = scipy.spatial.Delaunay(pixels)
except Exception as e:
logger.error("Delaunay triangulation failed for input: {}".format(
repr(pixels)))
raise e
sums = [0.0, 0.0, 0.0, 0.0]
depths = [0.0, 0.0, 0.0, 0.0]
for t in tri.simplices:
for i in range(4):
if i in t:
for j in t:
if j >= 4:
depths[i] += shot.pose.transform(vertices[j])[2]
sums[i] += 1
for i in range(4):
if sums[i] > 0:
d = depths[i] / sums[i]
else:
d = 50.0
vertices[i] = back_project_no_distortion(shot, pixels[i], d).tolist()
faces = tri.simplices.tolist()
return vertices, faces
def common_tracks(
tracks_manager: pymap.TracksManager, im1: str, im2: str
) -> t.Tuple[t.List[str], np.ndarray, np.ndarray]:
"""List of tracks observed in both images.
Args:
tracks_manager: tracks manager
im1: name of the first image
im2: name of the second image
Returns:
tuple: tracks, feature from first image, feature from second image
"""
t1 = tracks_manager.get_shot_observations(im1)
t2 = tracks_manager.get_shot_observations(im2)
tracks, p1, p2 = [], [], []
for track, obs in t1.items():
if track in t2:
p1.append(obs.point)
p2.append(t2[track].point)
tracks.append(track)
p1 = np.array(p1)
p2 = np.array(p2)
return tracks, p1, p2
def reconstruction_statistics(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
reconstructions: List[types.Reconstruction],
) -> Dict[str, Any]:
stats = {}
stats["components"] = len(reconstructions)
gps_count = 0
for rec in reconstructions:
for shot in rec.shots.values():
gps_count += shot.metadata.gps_position.has_value
stats["has_gps"] = gps_count > 2
stats["has_gcp"] = True if data.load_ground_control_points() else False
stats["initial_points_count"] = tracks_manager.num_tracks()
stats["initial_shots_count"] = len(data.images())
stats["reconstructed_points_count"] = 0
stats["reconstructed_shots_count"] = 0
stats["observations_count"] = 0
hist_agg = defaultdict(int)
for rec in reconstructions:
if len(rec.points) > 0:
stats["reconstructed_points_count"] += len(rec.points)
stats["reconstructed_shots_count"] += len(rec.shots)
# get tracks length distrbution for current reconstruction
hist, values = _length_histogram(tracks_manager, rec.points)
# update aggregrated histogram
for length, count_tracks in zip(hist, values):
hist_agg[length] += count_tracks
# observations total and average tracks lengths
hist_agg = sorted(hist_agg.items(), key=lambda x: x[0])
lengths, counts = np.array([int(x[0]) for x in hist_agg
]), np.array([x[1] for x in hist_agg])
points_count = stats["reconstructed_points_count"]
points_count_over_two = sum(counts[1:])
stats["observations_count"] = int(sum(lengths * counts))
stats["average_track_length"] = ((stats["observations_count"] /
points_count)
if points_count > 0 else -1)
stats["average_track_length_over_two"] = (
(int(sum(lengths[1:] * counts[1:])) /
points_count_over_two) if points_count_over_two > 0 else -1)
stats["histogram_track_length"] = {k: v for k, v in hist_agg}
(
avg_normalized,
avg_pixels,
avg_angular,
(hist_normalized, bins_normalized),
(hist_pixels, bins_pixels),
(hist_angular, bins_angular),
) = _projection_error(tracks_manager, reconstructions)
stats["reprojection_error_normalized"] = avg_normalized
stats["reprojection_error_pixels"] = avg_pixels
stats["reprojection_error_angular"] = avg_angular
stats["reprojection_histogram_normalized"] = (
list(map(float, hist_normalized)),
list(map(float, bins_normalized)),
)
stats["reprojection_histogram_pixels"] = (
list(map(float, hist_pixels)),
list(map(float, bins_pixels)),
)
stats["reprojection_histogram_angular"] = (
list(map(float, hist_angular)),
list(map(float, bins_angular)),
)
return stats
def save_matchgraph(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
reconstructions: List[types.Reconstruction],
output_path: str,
io_handler: io.IoFilesystemBase,
) -> None:
all_shots = []
all_points = []
shot_component = {}
for i, rec in enumerate(reconstructions):
all_points += rec.points
all_shots += rec.shots
for shot in rec.shots:
shot_component[shot] = i
connectivity = tracks_manager.get_all_pairs_connectivity(
all_shots, all_points)
all_values = connectivity.values()
lowest = np.percentile(list(all_values), 5)
highest = np.percentile(list(all_values), 95)
plt.clf()
cmap = cm.get_cmap("viridis")
for (node1, node2), edge in sorted(connectivity.items(),
key=lambda x: x[1]):
if edge < 2 * data.config["resection_min_inliers"]:
continue
comp1 = shot_component[node1]
comp2 = shot_component[node2]
if comp1 != comp2:
continue
o1 = reconstructions[comp1].shots[node1].pose.get_origin()
o2 = reconstructions[comp2].shots[node2].pose.get_origin()
c = max(0, min(1.0, 1 - (edge - lowest) / (highest - lowest)))
plt.plot([o1[0], o2[0]], [o1[1], o2[1]], linestyle="-", color=cmap(c))
for i, rec in enumerate(reconstructions):
for shot in rec.shots.values():
o = shot.pose.get_origin()
c = i / len(reconstructions)
plt.plot(o[0], o[1], linestyle="", marker="o", color=cmap(c))
plt.xticks([])
plt.yticks([])
ax = plt.gca()
for b in ["top", "bottom", "left", "right"]:
ax.spines[b].set_visible(False)
norm = colors.Normalize(vmin=lowest, vmax=highest)
sm = cm.ScalarMappable(norm=norm, cmap=cmap.reversed())
sm.set_array([])
plt.colorbar(
sm,
orientation="horizontal",
label="Number of matches between images",
pad=0.0,
)
with io_handler.open(os.path.join(output_path, "matchgraph.png"),
"wb") as fwb:
plt.savefig(
fwb,
dpi=300,
bbox_inches="tight",
)
def resect(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
reconstruction: types.Reconstruction,
shot_id: str,
threshold: float,
min_inliers: int,
) -> Tuple[bool, Set[str], Dict[str, Any]]:
"""Try resecting and adding a shot to the reconstruction.
Return:
True on success.
"""
rig_assignments = data.load_rig_assignments_per_image()
camera = reconstruction.cameras[data.load_exif(shot_id)["camera"]]
bs, Xs, ids = [], [], []
for track, obs in tracks_manager.get_shot_observations(shot_id).items():
if track in reconstruction.points:
b = camera.pixel_bearing(obs.point)
bs.append(b)
Xs.append(reconstruction.points[track].coordinates)
ids.append(track)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False, set(), {"num_common_points": len(bs)}
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("{} resection inliers: {} / {}".format(shot_id, ninliers,
len(bs)))
report = {
"num_common_points": len(bs),
"num_inliers": ninliers,
}
if ninliers >= min_inliers:
R = T[:, :3].T
t = -R.dot(T[:, 3])
assert shot_id not in reconstruction.shots
new_shots = add_shot(data, reconstruction, rig_assignments, shot_id,
pygeometry.Pose(R, t))
if shot_id in rig_assignments:
triangulate_shot_features(tracks_manager, reconstruction,
new_shots, data.config)
for i, succeed in enumerate(inliers):
if succeed:
add_observation_to_reconstruction(tracks_manager,
reconstruction, shot_id,
ids[i])
# pyre-fixme [6]: Expected `int` for 2nd positional
report["shots"] = list(new_shots)
return True, new_shots, report
else:
return False, set(), report
def save_topview(
data: DataSetBase,
tracks_manager: pymap.TracksManager,
reconstructions: List[types.Reconstruction],
output_path: str,
io_handler: io.IoFilesystemBase,
) -> None:
points = []
colors = []
for rec in reconstructions:
for point in rec.points.values():
track = tracks_manager.get_track_observations(point.id)
if len(track) < 2:
continue
coords = point.coordinates
points.append(coords)
r, g, b = [], [], []
for obs in track.values():
r.append(obs.color[0])
g.append(obs.color[1])
b.append(obs.color[2])
colors.append((statistics.median(r), statistics.median(g),
statistics.median(b)))
all_x = []
all_y = []
for rec in reconstructions:
for shot in rec.shots.values():
o = shot.pose.get_origin()
all_x.append(o[0])
all_y.append(o[1])
if not shot.metadata.gps_position.has_value:
continue
gps = shot.metadata.gps_position.value
all_x.append(gps[0])
all_y.append(gps[1])
# compute camera's XY bounding box
low_x, high_x = np.min(all_x), np.max(all_x)
low_y, high_y = np.min(all_y), np.max(all_y)
# get its size
size_x = high_x - low_x
size_y = high_y - low_y
# expand bounding box by some margin
margin = 0.05
low_x -= size_x * margin
high_x += size_y * margin
low_y -= size_x * margin
high_y += size_y * margin
# update size
size_x = high_x - low_x
size_y = high_y - low_y
im_size_x = 2000
im_size_y = int(im_size_x * size_y / size_x)
topview = np.zeros((im_size_y, im_size_x, 3))
# splat points using gaussian + max-pool
splatting = 15
size = 2 * splatting + 1
kernel = _get_gaussian_kernel(splatting, 2)
kernel /= kernel[splatting, splatting]
for point, color in zip(points, colors):
x, y = int((point[0] - low_x) / size_x * im_size_x), int(
(point[1] - low_y) / size_y * im_size_y)
if not ((0 < x < (im_size_x - 1)) and (0 < y < (im_size_y - 1))):
continue
k_low_x, k_low_y = -min(x - splatting, 0), -min(y - splatting, 0)
k_high_x, k_high_y = (
size - max(x + splatting - (im_size_x - 2), 0),
size - max(y + splatting - (im_size_y - 2), 0),
)
h_low_x, h_low_y = max(x - splatting, 0), max(y - splatting, 0)
h_high_x, h_high_y = min(x + splatting + 1,
im_size_x - 1), min(y + splatting + 1,
im_size_y - 1)
for i in range(3):
current = topview[h_low_y:h_high_y, h_low_x:h_high_x, i]
splat = kernel[k_low_y:k_high_y, k_low_x:k_high_x]
topview[h_low_y:h_high_y, h_low_x:h_high_x,
i] = np.maximum(splat * (color[i] / 255.0), current)
plt.clf()
plt.imshow(topview)
# display computed camera's XY
linewidth = 1
markersize = 4
for rec in reconstructions:
sorted_shots = sorted(rec.shots.values(),
key=lambda x: x.metadata.capture_time.value)
c_camera = cm.get_cmap("cool")(0 / len(reconstructions))
c_gps = cm.get_cmap("autumn")(0 / len(reconstructions))
for j, shot in enumerate(sorted_shots):
o = shot.pose.get_origin()
x, y = int((o[0] - low_x) / size_x * im_size_x), int(
(o[1] - low_y) / size_y * im_size_y)
plt.plot(
x,
y,
linestyle="",
marker="o",
color=c_camera,
markersize=markersize,
linewidth=1,
)
# also display camera path using capture time
if j < len(sorted_shots) - 1:
n = sorted_shots[j + 1].pose.get_origin()
nx, ny = int((n[0] - low_x) / size_x * im_size_x), int(
(n[1] - low_y) / size_y * im_size_y)
plt.plot([x, nx], [y, ny],
linestyle="-",
color=c_camera,
linewidth=linewidth)
# display GPS error
if not shot.metadata.gps_position.has_value:
continue
gps = shot.metadata.gps_position.value
gps_x, gps_y = int((gps[0] - low_x) / size_x * im_size_x), int(
(gps[1] - low_y) / size_y * im_size_y)
plt.plot(
gps_x,
gps_y,
linestyle="",
marker="v",
color=c_gps,
markersize=markersize,
linewidth=1,
)
plt.plot([x, gps_x], [y, gps_y],
linestyle="-",
color=c_gps,
linewidth=linewidth)
plt.xticks(
[0, im_size_x / 2, im_size_x],
[0, f"{int(size_x / 2):.0f}", f"{size_x:.0f} meters"],
fontsize="small",
)
plt.yticks(
[im_size_y, im_size_y / 2, 0],
[0, f"{int(size_y / 2):.0f}", f"{size_y:.0f} meters"],
fontsize="small",
)
with io_handler.open(os.path.join(output_path, "topview.png"),
"wb") as fwb:
plt.savefig(
fwb,
dpi=300,
bbox_inches="tight",
)
def save_undistorted_tracks_manager(
self, tracks_manager: pymap.TracksManager
) -> None:
filename = os.path.join(self.data_path, "tracks.csv")
with self.io_handler.open(filename, "w") as fw:
fw.write(tracks_manager.as_string())
def save_tracks_manager(
self, tracks_manager: pymap.TracksManager, filename: Optional[str] = None
) -> None:
with self.io_handler.open(self._tracks_manager_file(filename), "w") as fw:
fw.write(tracks_manager.as_string())