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 test_bundle_projection_fixed_internals(scene_synthetic): reference = scene_synthetic.reconstruction camera_priors = {c.id: c for c in reference.cameras.values()} graph = tracking.as_graph(scene_synthetic.tracks_manager) # 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 = pymap.Observation( pt[0], pt[1], g_obs["feature_scale"], color[0], color[1], color[2], g_obs["feature_id"], g_obs["feature_segmentation"], g_obs["feature_instance"], ) 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 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 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 test_clean_landmarks_with_min_observations() -> None: m = pymap.Map() n_cams = 2 n_shots = 2 n_landmarks = 10 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) m.create_rig_camera(pymap.RigCamera(pygeometry.Pose(), cam.id)) for shot_id in range(n_shots): cam_id = "cam" + str(int(np.random.rand(1) * 10 % n_cams)) m.create_rig_instance(str(shot_id)) m.create_shot(str(shot_id), cam_id, cam_id, str(shot_id), pygeometry.Pose()) for point_id in range(n_landmarks): m.create_landmark(str(point_id), np.random.rand(3)) for point_id in range(int(n_landmarks / 2)): for shot in m.get_shots().values(): # create a new observation obs = pymap.Observation(100, 200, 0.5, 255, 0, 0, point_id) m.add_observation(shot, m.get_landmark(str(point_id)), obs) for point_id in range(int(n_landmarks / 2), n_landmarks): shot = m.get_shot("0") # create a new observation obs = pymap.Observation(100, 200, 0.5, 255, 0, 0, point_id) m.add_observation(shot, m.get_landmark(str(point_id)), obs) m.clean_landmarks_below_min_observations(n_shots) assert len(m.get_landmarks()) == int(n_landmarks / 2) m.clean_landmarks_below_min_observations(n_shots + 1) assert len(m.get_landmarks()) == 0
def test_single_observation_delete() -> None: # Given a 1-camera, 1-point reconstruction and corresponding observation rec = _create_reconstruction(1, n_shots_cam={"0": 1}, n_points=1) obs = pymap.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() -> None: # 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 = pymap.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: 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_many_observations_delete() -> None: # 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) m.create_rig_camera(pymap.RigCamera(pygeometry.Pose(), cam.id)) for shot_id in range(n_shots): cam_id = "cam" + str(int(np.random.rand(1) * 10 % n_cams)) shot_id = str(shot_id) m.create_rig_instance(shot_id) m.create_shot(shot_id, cam_id, cam_id, shot_id, pygeometry.Pose()) 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 = pymap.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 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
def _add_point(rec, point_id, observations) -> None: rec.create_point(point_id) for shot_id in observations: obs = pymap.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: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 1, 1, 1)} t2 = {1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 2, 2, 2)} correspondences = reconstruction.corresponding_tracks(t1, t2) assert len(correspondences) == 0 t1 = {1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3, 3, 3)} t2 = {2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3, 3, 3)} correspondences = reconstruction.corresponding_tracks(t1, t2) assert len(correspondences) == 1 assert correspondences[0] == (1, 2) t1 = { 1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3, 3, 3), 2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 4, 4, 4), } t2 = { 1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 4, 4, 4), 2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5, 5, 5), } correspondences = reconstruction.corresponding_tracks(t1, t2) assert len(correspondences) == 1 assert correspondences[0] == (2, 1) t1 = { 1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5, 5, 5), 2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 6, 6, 6), } t2 = { 3: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5, 5, 5), 4: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 6, 6, 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 test_corresponding_tracks() -> None: t1 = {1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 1, 1, 1)} t2 = {1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 2, 2, 2)} # pyre-fixme[6]: For 1st param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. # pyre-fixme[6]: For 2nd param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. correspondences = reconstruction.corresponding_tracks(t1, t2) assert len(correspondences) == 0 t1 = {1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3, 3, 3)} t2 = {2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3, 3, 3)} # pyre-fixme[6]: For 1st param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. # pyre-fixme[6]: For 2nd param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. correspondences = reconstruction.corresponding_tracks(t1, t2) assert len(correspondences) == 1 assert correspondences[0] == (1, 2) t1 = { 1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 3, 3, 3), 2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 4, 4, 4), } t2 = { 1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 4, 4, 4), 2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5, 5, 5), } # pyre-fixme[6]: For 1st param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. # pyre-fixme[6]: For 2nd param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. correspondences = reconstruction.corresponding_tracks(t1, t2) assert len(correspondences) == 1 assert correspondences[0] == (2, 1) t1 = { 1: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5, 5, 5), 2: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 6, 6, 6), } t2 = { 3: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 5, 5, 5), 4: pymap.Observation(1.0, 1.0, 1.0, 0, 0, 0, 6, 6, 6), } # pyre-fixme[6]: For 1st param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. # pyre-fixme[6]: For 2nd param expected `Dict[str, Observation]` but got # `Dict[int, Observation]`. 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 _create_reconstruction( n_cameras: int = 0, n_shots_cam=None, n_pano_shots_cam=None, n_points: int = 0, dist_to_shots: bool = False, dist_to_pano_shots: bool = 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] """ if n_shots_cam is None: n_shots_cam = {} if n_pano_shots_cam is None: n_pano_shots_cam = {} 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 = pymap.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