def test_bundle_projection_fixed_internals(scene_synthetic):
reference = scene_synthetic[0].get_reconstruction()
camera_priors = {c.id: c for c in scene_synthetic[0].cameras}
graph = tracking.as_graph(scene_synthetic[5])
# Create the connnections in the reference
for point_id in reference.points.keys():
if point_id in graph:
for shot_id, g_obs in graph[point_id].items():
color = g_obs["feature_color"]
pt = g_obs["feature"]
obs = pysfm.Observation(
pt[0],
pt[1],
g_obs["feature_scale"],
g_obs["feature_id"],
color[0],
color[1],
color[2],
)
reference.map.add_observation(shot_id, point_id, obs)
orig_camera = copy.deepcopy(reference.cameras["1"])
custom_config = config.default_config()
custom_config["bundle_use_gps"] = False
custom_config["optimize_camera_parameters"] = False
reconstruction.bundle(reference, camera_priors, [], custom_config)
assert _projection_errors_std(reference.points) < 5e-3
assert reference.cameras["1"].focal == orig_camera.focal
assert reference.cameras["1"].k1 == orig_camera.k1
assert reference.cameras["1"].k2 == orig_camera.k2
def create_tracks_manager(features, colors, matches, config):
"""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]
min_length = config['min_track_length']
tracks = [t for t in sets.values() if _good_track(t, min_length)]
logger.debug('Good tracks: {}'.format(len(tracks)))
tracks_manager = pysfm.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]
obs = pysfm.Observation(x, y, s, int(r), int(g), int(b), featureid)
tracks_manager.add_observation(image, str(track_id), obs)
return tracks_manager
def test_many_observations_delete():
# Given a map with 10 shots, 1000 landmarks ...
m = pymap.Map()
n_cams = 2
n_shots = 10
n_landmarks = 1000
for cam_id in range(n_cams):
cam = pygeometry.Camera.create_perspective(0.5, 0, 0)
cam.id = "cam" + str(cam_id)
m.create_camera(cam)
for shot_id in range(n_shots):
m.create_shot(str(shot_id), "cam" + str(int(np.random.rand(1) * 10 % n_cams)))
for point_id in range(n_landmarks):
m.create_landmark(str(point_id), np.random.rand(3))
# ... and random connections (observations) between shots and points
n_total_obs = 0
for lm in m.get_landmarks().values():
n_obs = 0
for shot in m.get_shots().values():
# create a new observation
obs = pysfm.Observation(100, 200, 0.5, 255, 0, 0, int(lm.id))
m.add_observation(shot, lm, obs)
n_obs += 1
n_total_obs += 1
# (we expect it to be created correctly)
for lm in m.get_landmarks().values():
n_total_obs -= lm.number_of_observations()
assert n_total_obs == 0
# and when we clear all the observations
m.clear_observations_and_landmarks()
def _create_reconstruction(n_cameras=0,
n_shots_cam={},
n_pano_shots_cam={},
n_points=0,
dist_to_shots=False,
dist_to_pano_shots=False):
""" Creates a reconstruction with n_cameras random cameras and
shots, where n_shots_cam is a dictionary, containing the
camera_id and the number of shots.
Example:
shot_cams = {"0": 50, "1": 30}
_create_reconstruction(2, shot_cams)
Will create a reconstruction with two cameras and 80 shots,
50 are associated with cam "0" and 30 with cam "1".
n_points_in_shots is the number of points to create.
If dist_to_shots, then observations are created and randomly
distributed to all shots. We pick with the repeat option, thus
if we have three shots the distribution could be
something like: [1,2,2], [0,1,2]. We avoid things like [3,3,3]
"""
rec = types.Reconstruction()
if n_cameras > 0:
for i in range(n_cameras):
focal, k1, k2 = np.random.rand(3)
cam = pygeometry.Camera.create_perspective(focal, k1, k2)
cam.id = str(i)
rec.add_camera(cam)
shot_id = 0
for cam_id, n_shots in n_shots_cam.items():
for _ in range(n_shots):
rec.create_shot(str(shot_id), cam_id)
shot_id += 1
shot_id = 0
for cam_id, n_shots in n_pano_shots_cam.items():
for _ in range(n_shots):
rec.create_pano_shot(str(shot_id), cam_id)
shot_id += 1
if n_points > 0:
for i in range(n_points):
rec.create_point(str(i), np.random.rand(3))
if dist_to_shots:
n_shots = len(rec.shots)
for pt in rec.points.values():
choice = set(np.random.choice(n_shots, n_shots))
if len(choice) > 1:
for ch in choice:
# create a new observation
obs = pysfm.Observation(100, 200, 0.5, 255, 0, 0,
int(pt.id))
shot = rec.shots[str(ch)]
rec.add_observation(shot, pt, obs)
# TODO: If required, we have to do the same for pano shots
return rec
def generate_track_data(reconstruction, maximum_depth, noise):
tracks_manager = pysfm.TracksManager()
feature_data_type = np.float32
desc_size = 128
non_zeroes = 5
track_descriptors = {}
for track_index in reconstruction.points:
descriptor = np.zeros(desc_size)
for i in range(non_zeroes):
index = np.random.randint(0, desc_size)
descriptor[index] = np.random.random()*255
track_descriptors[track_index] = descriptor.round().\
astype(feature_data_type)
colors = {}
features = {}
descriptors = {}
default_scale = 0.004
for shot_index, shot in reconstruction.shots.items():
# need to have these as we lost track of keys
all_keys = list(reconstruction.points.keys())
all_values = list(reconstruction.points.values())
# temporary work on numpy array
all_coordinates = [p.coordinates for p in all_values]
projections = shot.project_many(np.array(all_coordinates))
projections_inside = []
descriptors_inside = []
colors_inside = []
for i, projection in enumerate(projections):
if not _is_inside_camera(projection, shot.camera):
continue
original_key = all_keys[i]
original_point = all_values[i]
if not _is_in_front(original_point, shot):
continue
if not _check_depth(original_point, shot, maximum_depth):
continue
# add perturbation
perturbation = float(noise)/float(max(shot.camera.width,
shot.camera.height))
perturb_points([projection], np.array([perturbation, perturbation]))
projections_inside.append(np.hstack((projection, [default_scale])))
descriptors_inside.append(track_descriptors[original_key])
colors_inside.append(original_point.color)
obs = pysfm.Observation(
projection[0], projection[1], default_scale,
original_point.color[0], original_point.color[1],
original_point.color[2], len(projections_inside) - 1)
tracks_manager.add_observation(str(shot_index), str(original_key), obs)
features[shot_index] = np.array(projections_inside)
colors[shot_index] = np.array(colors_inside)
descriptors[shot_index] = np.array(descriptors_inside)
return features, descriptors, colors, tracks_manager
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 = pysfm.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 = pysfm.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 test_many_observations_delete():
# Given a map with 10 shots, 1000 landmarks ...
m = pymap.Map()
n_cams = 2
n_shots = 10
n_landmarks = 1000
for cam_id in range(n_cams):
cam = pygeometry.Camera.create_perspective(0.5, 0, 0)
cam.id = "cam" + str(cam_id)
m.create_camera(cam)
for shot_id in range(n_shots):
m.create_shot(str(shot_id),
"cam" + str(int(np.random.rand(1) * 10 % n_cams)))
for point_id in range(n_landmarks):
m.create_landmark(str(point_id), np.random.rand(3))
# ... and random connections (observations) between shots and points
n_total_obs = 0
for lm in m.get_landmarks().values():
n_obs = 0
for shot in m.get_shots().values():
# create a new observation
obs = pysfm.Observation(100, 200, 0.5, 255, 0, 0, int(lm.id))
m.add_observation(shot, lm, obs)
n_obs += 1
n_total_obs += 1
# (we expect it to be created correctly)
for lm in m.get_landmarks().values():
n_total_obs -= lm.number_of_observations()
assert n_total_obs == 0
# When we remove the observations for half of the shots
for lm in m.get_landmarks().values():
for shot_id in range(int(n_shots / 2)):
m.remove_observation(str(shot_id), lm.id)
# Then each each landmark has N/2 observations
assert lm.number_of_observations() == int(n_shots / 2)
# Then there #shots x #points observations remaning
n_total_obs = 0
for shot in m.get_shots().values():
n_total_obs += shot.compute_num_valid_pts(1)
assert n_total_obs == int((n_shots * n_landmarks) / 2)
# and when we clear all the observations
m.clear_observations_and_landmarks()
# Then there's none
assert m.number_of_landmarks() == 0
def test_track_triangulator_equirectangular():
"""Test triangulating tracks of spherical images."""
tracks_manager = pysfm.TracksManager()
tracks_manager.add_observation('im1', '1',
pysfm.Observation(0, 0, 1.0, 0, 0, 0, 0))
tracks_manager.add_observation('im2', '1',
pysfm.Observation(-0.1, 0, 1.0, 0, 0, 0, 1))
rec = io.reconstruction_from_json({
"cameras": {
"theta": {
"projection_type": "equirectangular",
"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": {},
})
graph_inliers = nx.Graph()
triangulator = reconstruction.TrackTriangulator(tracks_manager,
graph_inliers, rec)
triangulator.triangulate('1', 0.01, 2.0)
assert '1' in rec.points
p = rec.points['1'].coordinates
assert np.allclose(p, [0, 0, 1.3763819204711])
assert len(graph_inliers.edges()) == 2
def test_track_triangulator_spherical():
"""Test triangulating tracks of spherical images."""
tracks_manager = pysfm.TracksManager()
tracks_manager.add_observation("im1", "1", pysfm.Observation(0, 0, 1.0, 0, 0, 0, 0))
tracks_manager.add_observation(
"im2", "1", pysfm.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(tracks_manager, rec)
triangulator.triangulate("1", 0.01, 2.0)
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 test_single_observation_delete():
# Given a 1-camera, 1-point reconstruction and corresponding observation
rec = _create_reconstruction(1, n_shots_cam={"0": 1}, n_points=1)
obs = pysfm.Observation(100, 200, 0.5, 255, 0, 0, 100)
rec.add_observation("0", "0", obs)
shot = rec.shots["0"]
pt = rec.points["0"]
# When we remove it
rec.remove_observation(shot.id, pt.id)
# Then there's none
observations = pt.get_observations()
assert len(observations) == 0
assert pt.number_of_observations() == 0
def test_single_observation():
# Given a 1-camera, 1-point reconstruction
rec = _create_reconstruction(1, n_shots_cam={"0": 1}, n_points=1)
# When we add an observation to it
obs = pysfm.Observation(100, 200, 0.5, 255, 0, 0, 100, 2, 5)
rec.add_observation("0", "0", obs)
shot = rec.shots["0"]
pt = rec.points["0"]
# Then it has one observation ...
observations = pt.get_observations()
assert len(observations) == 1
assert pt.number_of_observations() == 1
# ... and the corresponding observation object
obs = shot.get_landmark_observation(pt)
assert obs is not None
def create_tracks_manager(features, colors, segmentations, instances, matches,
config):
"""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]
min_length = config["min_track_length"]
tracks = [t for t in sets.values() if _good_track(t, min_length)]
logger.debug("Good tracks: {}".format(len(tracks)))
NO_VALUE = pysfm.Observation.NO_SEMANTIC_VALUE
tracks_manager = pysfm.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 = pysfm.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 _add_point(rec, point_id, observations):
rec.create_point(point_id)
for shot_id in observations:
obs = pysfm.Observation(100, 200, 0.5, 255, 0, 0, int(point_id))
rec.add_observation(shot_id, point_id, obs)
def test_corresponding_tracks():
t1 = {1: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 1)}
t2 = {1: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 2)}
correspondences = reconstruction.corresponding_tracks(t1, t2)
assert len(correspondences) == 0
t1 = {1: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3)}
t2 = {2: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3)}
correspondences = reconstruction.corresponding_tracks(t1, t2)
assert len(correspondences) == 1
assert correspondences[0] == (1, 2)
t1 = {1: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3),
2: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 4)}
t2 = {1: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 4),
2: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5)}
correspondences = reconstruction.corresponding_tracks(t1, t2)
assert len(correspondences) == 1
assert correspondences[0] == (2, 1)
t1 = {1: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5),
2: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 6)}
t2 = {3: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5),
4: pysfm.Observation(1.0, 1.0, 1.0, 0, 0, 0, 6)}
correspondences = reconstruction.corresponding_tracks(t1, t2)
correspondences.sort(key=lambda c: c[0] + c[1])
assert len(correspondences) == 2
assert correspondences[0] == (1, 3)
assert correspondences[1] == (2, 4)
def generate_track_data(
reconstruction: types.Reconstruction, maximum_depth: float, noise: float
) -> Tuple[Dict[str, np.ndarray], Dict[str, np.ndarray], Dict[str, np.ndarray],
pysfm.TracksManager, ]:
"""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 = pysfm.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)
colors = {}
features = {}
descriptors = {}
default_scale = 0.004
for shot_index, shot in 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(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 = pysfm.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] = np.array(projections_inside)
colors[shot_index] = np.array(colors_inside)
descriptors[shot_index] = np.array(descriptors_inside)
return features, descriptors, colors, tracks_manager
