def test_add_correspondences_from_tracks_manager() -> None:
n_shots = 3
rec = _create_reconstruction(
n_cameras=1,
n_shots_cam={"0": n_shots},
n_points=10,
)
# create tracks manager
tm = pymap.TracksManager()
# add observations for 3 tracks
# One shot and one landmark are not in the reconstruction
for track_id in ["0", "1", "100"]:
for shot_id in range(n_shots + 1):
obs = pymap.Observation(100, 200, 0.5, 255, 0, 0, 100)
tm.add_observation(str(shot_id), track_id, obs)
# add a shot that is NOT in the tracks manager
rec.create_shot(str(n_shots + 5), next(iter(rec.cameras)))
rec.add_correspondences_from_tracks_manager(tm)
# make sure to have the observations for []
assert "100" not in rec.points
for track_id in ["0", "1"]:
pt = rec.points[track_id]
observations = pt.get_observations()
assert len(observations) == n_shots
def __init__(
self,
reconstruction: types.Reconstruction,
reference: geo.TopocentricConverter,
projection_max_depth: float,
projection_noise: float,
gps_noise: Union[Dict[str, float], float],
causal_gps_noise: bool,
on_disk_features_filename: Optional[str] = None,
generate_projections: bool = True,
):
self.reconstruction = reconstruction
self.exifs = sg.generate_exifs(reconstruction,
reference,
gps_noise,
causal_gps_noise=causal_gps_noise)
if generate_projections:
(self.features, self.tracks_manager) = sg.generate_track_data(
reconstruction,
projection_max_depth,
projection_noise,
on_disk_features_filename,
)
else:
self.features = sd.SyntheticFeatures(None)
self.tracks_manager = pymap.TracksManager()
def undistort_reconstruction(
tracks_manager: Optional[pymap.TracksManager],
reconstruction: types.Reconstruction,
data: DataSetBase,
udata: UndistortedDataSet,
) -> Dict[pymap.Shot, List[pymap.Shot]]:
all_images = set(data.images())
image_format = data.config["undistorted_image_format"]
urec = types.Reconstruction()
urec.points = reconstruction.points
urec.reference = reconstruction.reference
rig_instance_count = itertools.count()
utracks_manager = pymap.TracksManager()
logger.debug("Undistorting the reconstruction")
undistorted_shots = {}
for shot in reconstruction.shots.values():
if shot.id not in all_images:
logger.warning(
f"Not undistorting {shot.id} as it is missing from the dataset's input images."
)
continue
if shot.camera.projection_type == "perspective":
urec.add_camera(perspective_camera_from_perspective(shot.camera))
subshots = [get_shot_with_different_camera(urec, shot, image_format)]
elif shot.camera.projection_type == "brown":
urec.add_camera(perspective_camera_from_brown(shot.camera))
subshots = [get_shot_with_different_camera(urec, shot, image_format)]
elif shot.camera.projection_type in ["fisheye", "fisheye_opencv"]:
urec.add_camera(perspective_camera_from_fisheye(shot.camera))
subshots = [get_shot_with_different_camera(urec, shot, image_format)]
elif pygeometry.Camera.is_panorama(shot.camera.projection_type):
subshot_width = int(data.config["depthmap_resolution"])
subshots = perspective_views_of_a_panorama(
shot, subshot_width, urec, image_format, rig_instance_count
)
else:
logger.warning(
f"Not undistorting {shot.id} with unknown camera type."
)
continue
for subshot in subshots:
if tracks_manager:
add_subshot_tracks(tracks_manager, utracks_manager, shot, subshot)
undistorted_shots[shot.id] = subshots
udata.save_undistorted_reconstruction([urec])
if tracks_manager:
udata.save_undistorted_tracks_manager(utracks_manager)
udata.save_undistorted_shot_ids(
{
shot_id: [ushot.id for ushot in ushots]
for shot_id, ushots in undistorted_shots.items()
}
)
return undistorted_shots
def undistort_reconstruction(
tracks_manager, reconstruction, data: DataSetBase, udata: UndistortedDataSet
):
image_format = data.config["undistorted_image_format"]
urec = types.Reconstruction()
urec.points = reconstruction.points
urec.reference = reconstruction.reference
rig_instance_count = itertools.count()
utracks_manager = pymap.TracksManager()
logger.debug("Undistorting the reconstruction")
undistorted_shots = {}
for shot in reconstruction.shots.values():
if shot.camera.projection_type == "perspective":
camera = perspective_camera_from_perspective(shot.camera)
urec.add_camera(camera)
subshots = [
get_shot_with_different_camera(urec, shot, camera, image_format)
]
elif shot.camera.projection_type == "brown":
camera = perspective_camera_from_brown(shot.camera)
urec.add_camera(camera)
subshots = [
get_shot_with_different_camera(urec, shot, camera, image_format)
]
elif shot.camera.projection_type in ["fisheye", "fisheye_opencv"]:
camera = perspective_camera_from_fisheye(shot.camera)
urec.add_camera(camera)
subshots = [
get_shot_with_different_camera(urec, shot, camera, image_format)
]
elif pygeometry.Camera.is_panorama(shot.camera.projection_type):
subshot_width = int(data.config["depthmap_resolution"])
subshots = perspective_views_of_a_panorama(
shot, subshot_width, urec, image_format, rig_instance_count
)
for subshot in subshots:
if tracks_manager:
add_subshot_tracks(tracks_manager, utracks_manager, shot, subshot)
# pyre-fixme[61]: `subshots` may not be initialized here.
undistorted_shots[shot.id] = subshots
udata.save_undistorted_reconstruction([urec])
if tracks_manager:
udata.save_undistorted_tracks_manager(utracks_manager)
udata.save_undistorted_shot_ids(
{
shot_id: [ushot.id for ushot in ushots]
for shot_id, ushots in undistorted_shots.items()
}
)
return undistorted_shots
def import_images_reconstruction(path_images, keypoints, rec):
"""
Read images.bin, building shots and tracks graph
"""
logger.info("Importing images from {}".format(path_images))
tracks_manager = pymap.TracksManager()
image_ix_to_shot_id = {}
with open(path_images, "rb") as f:
n_ims = unpack("<Q", f.read(8))[0]
for image_ix in range(n_ims):
image_id = unpack("<I", f.read(4))[0]
q0 = unpack("<d", f.read(8))[0]
q1 = unpack("<d", f.read(8))[0]
q2 = unpack("<d", f.read(8))[0]
q3 = unpack("<d", f.read(8))[0]
t0 = unpack("<d", f.read(8))[0]
t1 = unpack("<d", f.read(8))[0]
t2 = unpack("<d", f.read(8))[0]
camera_id = unpack("<I", f.read(4))[0]
filename = ""
while True:
c = f.read(1).decode()
if c == "\0":
break
filename += c
q = np.array([q0, q1, q2, q3])
q /= np.linalg.norm(q)
t = np.array([t0, t1, t2])
pose = pygeometry.Pose(rotation=quaternion_to_angle_axis(q), translation=t)
shot = rec.create_shot(filename, str(camera_id), pose)
image_ix_to_shot_id[image_ix] = shot.id
n_points_2d = unpack("<Q", f.read(8))[0]
for point2d_ix in range(n_points_2d):
x = unpack("<d", f.read(8))[0]
y = unpack("<d", f.read(8))[0]
point3d_id = unpack("<Q", f.read(8))[0]
if point3d_id != np.iinfo(np.uint64).max:
kp = keypoints[image_id][point2d_ix]
r, g, b = rec.points[str(point3d_id)].color
obs = pymap.Observation(
x,
y,
kp[2],
int(r),
int(g),
int(b),
point2d_ix,
)
tracks_manager.add_observation(shot.id, str(point3d_id), obs)
return tracks_manager, image_ix_to_shot_id
def create_tracks_manager(
features: t.Dict[str, np.ndarray],
colors: t.Dict[str, np.ndarray],
segmentations: t.Dict[str, np.ndarray],
instances: t.Dict[str, np.ndarray],
matches: t.Dict[t.Tuple[str, str], t.List[t.Tuple[int, int]]],
min_length: int,
):
"""Link matches into tracks."""
logger.debug("Merging features onto tracks")
uf = UnionFind()
for im1, im2 in matches:
for f1, f2 in matches[im1, im2]:
uf.union((im1, f1), (im2, f2))
sets = {}
for i in uf:
p = uf[i]
if p in sets:
sets[p].append(i)
else:
sets[p] = [i]
tracks = [t for t in sets.values() if _good_track(t, min_length)]
logger.debug("Good tracks: {}".format(len(tracks)))
NO_VALUE = pymap.Observation.NO_SEMANTIC_VALUE
tracks_manager = pymap.TracksManager()
for track_id, track in enumerate(tracks):
for image, featureid in track:
if image not in features:
continue
x, y, s = features[image][featureid]
r, g, b = colors[image][featureid]
segmentation, instance = (
segmentations[image][featureid] if image in segmentations else NO_VALUE,
instances[image][featureid] if image in instances else NO_VALUE,
)
obs = pymap.Observation(
x, y, s, int(r), int(g), int(b), featureid, segmentation, instance
)
tracks_manager.add_observation(image, str(track_id), obs)
return tracks_manager
def test_track_triangulator_spherical() -> None:
"""Test triangulating tracks of spherical images."""
tracks_manager = pymap.TracksManager()
tracks_manager.add_observation("im1", "1",
pymap.Observation(0, 0, 1.0, 0, 0, 0, 0))
tracks_manager.add_observation("im2", "1",
pymap.Observation(-0.1, 0, 1.0, 0, 0, 0, 1))
rec = io.reconstruction_from_json({
"cameras": {
"theta": {
"projection_type": "spherical",
"width": 800,
"height": 400,
}
},
"shots": {
"im1": {
"camera": "theta",
"rotation": [0.0, 0.0, 0.0],
"translation": [0.0, 0.0, 0.0],
},
"im2": {
"camera": "theta",
"rotation": [0, 0, 0.0],
"translation": [-1, 0, 0.0],
},
},
"points": {},
})
triangulator = reconstruction.TrackTriangulator(
rec, reconstruction.TrackHandlerTrackManager(tracks_manager, rec))
triangulator.triangulate("1", 0.01, 2.0, 10)
assert "1" in rec.points
p = rec.points["1"].coordinates
assert np.allclose(p, [0, 0, 1.3763819204711])
assert len(rec.points["1"].get_observations()) == 2
def compute_and_save_undistorted_reconstruction(
reconstruction, tracks_manager, data, udata
):
image_format = data.config["undistorted_image_format"]
urec = types.Reconstruction()
utracks_manager = pymap.TracksManager()
undistorted_shots = []
for shot in reconstruction.shots.values():
if shot.camera.projection_type == "perspective":
ucamera = osfm_u.perspective_camera_from_perspective(shot.camera)
elif shot.camera.projection_type == "brown":
ucamera = osfm_u.perspective_camera_from_brown(shot.camera)
elif shot.camera.projection_type == "fisheye":
ucamera = osfm_u.perspective_camera_from_fisheye(shot.camera)
else:
raise ValueError
urec.add_camera(ucamera)
ushot = osfm_u.get_shot_with_different_camera(urec, shot, image_format)
if tracks_manager:
osfm_u.add_subshot_tracks(tracks_manager, utracks_manager, shot, ushot)
undistorted_shots.append(ushot)
image = data.load_image(shot.id, unchanged=True, anydepth=True)
if image is not None:
max_size = data.config["undistorted_image_max_size"]
undistorted = osfm_u.undistort_image(
shot, undistorted_shots, image, cv2.INTER_AREA, max_size
)
for k, v in undistorted.items():
udata.save_undistorted_image(k, v)
udata.save_undistorted_reconstruction([urec])
if tracks_manager:
udata.save_undistorted_tracks_manager(utracks_manager)
return urec
def generate_track_data(
reconstruction: types.Reconstruction,
maximum_depth: float,
projection_noise: float,
gcp_noise: Tuple[float, float],
gcps_count: Optional[int],
gcp_shift: Optional[np.ndarray],
on_disk_features_filename: Optional[str],
) -> Tuple[
sd.SyntheticFeatures, pymap.TracksManager, Dict[str, pymap.GroundControlPoint]
]:
"""Generate projection data from a reconstruction, considering a maximum
viewing depth and gaussian noise added to the ideal projections.
Returns feature/descriptor/color data per shot and a tracks manager object.
"""
tracks_manager = pymap.TracksManager()
feature_data_type = np.float32
desc_size = 128
non_zeroes = 5
points_ids = list(reconstruction.points)
points_coordinates = [p.coordinates for p in reconstruction.points.values()]
points_colors = [p.color for p in reconstruction.points.values()]
# generate random descriptors per point
track_descriptors = []
for _ in points_coordinates:
descriptor = np.zeros(desc_size)
for _ in range(non_zeroes):
index = np.random.randint(0, desc_size)
descriptor[index] = np.random.random() * 255
track_descriptors.append(descriptor.round().astype(feature_data_type))
# should speed-up projection queries
points_tree = spatial.cKDTree(points_coordinates)
start = time.time()
features = sd.SyntheticFeatures(on_disk_features_filename)
default_scale = 0.004
for index, (shot_index, shot) in enumerate(reconstruction.shots.items()):
# query all closest points
neighbors = list(
sorted(points_tree.query_ball_point(shot.pose.get_origin(), maximum_depth))
)
# project them
projections = shot.project_many(
np.array([points_coordinates[c] for c in neighbors])
)
# shot constants
center = shot.pose.get_origin()
z_axis = shot.pose.get_rotation_matrix()[2]
is_panorama = pygeometry.Camera.is_panorama(shot.camera.projection_type)
perturbation = float(projection_noise) / float(
max(shot.camera.width, shot.camera.height)
)
sigmas = np.array([perturbation, perturbation])
# pre-generate random perturbations
perturbations = np.random.normal(0.0, sigmas, (len(projections), 2))
# run and check valid projections
projections_inside = []
descriptors_inside = []
colors_inside = []
for i, (p_id, projection) in enumerate(zip(neighbors, projections)):
if not _is_inside_camera(projection, shot.camera):
continue
point = points_coordinates[p_id]
if not is_panorama and not _is_in_front(point, center, z_axis):
continue
# add perturbation
projection += perturbations[i]
# push data
color = points_colors[p_id]
original_id = points_ids[p_id]
projections_inside.append([projection[0], projection[1], default_scale])
descriptors_inside.append(track_descriptors[p_id])
colors_inside.append(color)
obs = pymap.Observation(
projection[0],
projection[1],
default_scale,
color[0],
color[1],
color[2],
len(projections_inside) - 1,
)
tracks_manager.add_observation(str(shot_index), str(original_id), obs)
features[shot_index] = oft.FeaturesData(
np.array(projections_inside),
np.array(descriptors_inside),
np.array(colors_inside),
None,
)
if index % 100 == 0:
logger.info(
f"Flushing images # {index} ({(time.time() - start)/(index+1)} sec. per image"
)
features.sync()
gcps = {}
if gcps_count is not None and gcp_shift is not None:
all_track_ids = list(tracks_manager.get_track_ids())
gcps_ids = [
all_track_ids[i]
for i in np.random.randint(len(all_track_ids) - 1, size=gcps_count)
]
sigmas_gcp = np.random.normal(
0.0,
np.array([gcp_noise[0], gcp_noise[0], gcp_noise[1]]),
(len(gcps_ids), 3),
)
for i, gcp_id in enumerate(gcps_ids):
point = reconstruction.points[gcp_id]
gcp = pymap.GroundControlPoint()
gcp.id = f"gcp-{gcp_id}"
enu = point.coordinates + gcp_shift + sigmas_gcp[i]
lat, lon, alt = reconstruction.reference.to_lla(*enu)
gcp.lla = {"latitude": lat, "longitude": lon, "altitude": alt}
gcp.has_altitude = True
for shot_id, obs in tracks_manager.get_track_observations(gcp_id).items():
o = pymap.GroundControlPointObservation()
o.shot_id = shot_id
o.projection = obs.point
gcp.add_observation(o)
gcps[gcp.id] = gcp
return features, tracks_manager, gcps
