def triangulate_robust(self, track, reproj_threshold, min_ray_angle_degrees):
"""Triangulate track in a RANSAC way and add point to reconstruction."""
os, bs, ids = [], [], []
for shot_id, obs in self.tracks_manager.get_track_observations(track).items():
if shot_id in self.reconstruction.shots:
shot = self.reconstruction.shots[shot_id]
os.append(self._shot_origin(shot))
b = shot.camera.pixel_bearing(np.array(obs.point))
r = self._shot_rotation_inverse(shot)
bs.append(r.dot(b))
ids.append(shot_id)
if len(ids) < 2:
return
best_inliers = []
best_point = types.Point()
best_point.id = track
combinatiom_tried = set()
ransac_tries = 11 # 0.99 proba, 60% inliers
all_combinations = list(combinations(range(len(ids)), 2))
thresholds = len(os) * [reproj_threshold]
for i in range(ransac_tries):
random_id = int(np.random.rand()*(len(all_combinations)-1))
if random_id in combinatiom_tried:
continue
i, j = all_combinations[random_id]
combinatiom_tried.add(random_id)
os_t = [os[i], os[j]]
bs_t = [bs[i], bs[j]]
e, X = pygeometry.triangulate_bearings_midpoint(
os_t, bs_t, thresholds, np.radians(min_ray_angle_degrees))
if X is not None:
reprojected_bs = X-os
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < reproj_threshold
if sum(inliers) > sum(best_inliers):
best_inliers = inliers
best_point.coordinates = X.tolist()
pout = 0.99
inliers_ratio = float(sum(best_inliers))/len(ids)
if inliers_ratio == 1.0:
break
optimal_iter = math.log(1.0-pout)/math.log(1.0-inliers_ratio*inliers_ratio)
if optimal_iter <= ransac_tries:
break
if len(best_inliers) > 1:
self.reconstruction.add_point(best_point)
for i, succeed in enumerate(best_inliers):
if succeed:
self._add_track_to_graph_inlier(track, ids[i])
def triangulate_gcp(
point: pymap.GroundControlPoint,
shots: Dict[str, pymap.Shot],
reproj_threshold: float,
min_ray_angle_degrees: float,
) -> Optional[np.ndarray]:
"""Compute the reconstructed position of a GCP from observations."""
os, bs, ids = [], [], []
for observation in point.observations:
shot_id = observation.shot_id
if shot_id in shots:
shot = shots[shot_id]
os.append(shot.pose.get_origin())
x = observation.projection
b = shot.camera.pixel_bearing(np.array(x))
r = shot.pose.get_rotation_matrix().T
bs.append(r.dot(b))
ids.append(shot_id)
if len(os) >= 2:
thresholds = len(os) * [reproj_threshold]
valid_triangulation, X = pygeometry.triangulate_bearings_midpoint(
np.asarray(os),
np.asarray(bs),
thresholds,
np.radians(min_ray_angle_degrees),
)
if valid_triangulation:
return X
return None
def test_triangulate_bearings_midpoint():
o1 = np.array([0.0, 0, 0])
b1 = unit_vector([0.0, 0, 1])
o2 = np.array([1.0, 0, 0])
b2 = unit_vector([-1.0, 0, 1])
max_reprojection = 0.01
min_ray_angle = np.radians(2.0)
valid_triangulation, X = pygeometry.triangulate_bearings_midpoint(
[o1, o2], [b1, b2], 2 * [max_reprojection], min_ray_angle)
assert np.allclose(X, [0, 0, 1.0])
assert valid_triangulation is True
def test_triangulate_bearings_midpoint() -> None:
o1 = np.array([0.0, 0, 0])
b1 = unit_vector([0.0, 0, 1])
o2 = np.array([1.0, 0, 0])
b2 = unit_vector([-1.0, 0, 1])
max_reprojection = 0.01
min_ray_angle = np.radians(2.0)
valid_triangulation, X = pygeometry.triangulate_bearings_midpoint(
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[ndarray]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[typing.Any]`.
[o1, o2],
[b1, b2],
2 * [max_reprojection],
min_ray_angle)
assert np.allclose(X, [0, 0, 1.0])
assert valid_triangulation is True
def triangulate_single_gcp(reconstruction, observations):
"""Triangulate one Ground Control Point."""
reproj_threshold = 0.004
min_ray_angle_degrees = 2.0
os, bs = [], []
for o in observations:
if o.shot_id in reconstruction.shots:
shot = reconstruction.shots[o.shot_id]
os.append(shot.pose.get_origin())
b = shot.camera.pixel_bearing(np.asarray(o.projection))
r = shot.pose.get_rotation_matrix().T
bs.append(r.dot(b))
if len(os) >= 2:
thresholds = len(os) * [reproj_threshold]
angle = np.radians(min_ray_angle_degrees)
e, X = pygeometry.triangulate_bearings_midpoint(os, bs, thresholds, angle)
return X
def triangulate(self, track, reproj_threshold, min_ray_angle_degrees):
"""Triangulate track and add point to reconstruction."""
os, bs, ids = [], [], []
for shot_id, obs in self.tracks_manager.get_track_observations(track).items():
if shot_id in self.reconstruction.shots:
shot = self.reconstruction.shots[shot_id]
os.append(self._shot_origin(shot))
b = shot.camera.pixel_bearing(np.array(obs.point))
r = self._shot_rotation_inverse(shot)
bs.append(r.dot(b))
ids.append(shot_id)
if len(os) >= 2:
thresholds = len(os) * [reproj_threshold]
e, X = pygeometry.triangulate_bearings_midpoint(
os, bs, thresholds, np.radians(min_ray_angle_degrees))
if e:
self.reconstruction.create_point(track, X.tolist())
for shot_id in ids:
self._add_track_to_reconstruction(track, shot_id)
def triangulate_gcp(point, shots):
"""Compute the reconstructed position of a GCP from observations."""
reproj_threshold = 1.0
min_ray_angle = np.radians(0.1)
os, bs, ids = [], [], []
for observation in point.observations:
shot_id = observation.shot_id
if shot_id in shots:
shot = shots[shot_id]
os.append(shot.pose.get_origin())
x = observation.projection
b = shot.camera.pixel_bearing(np.array(x))
r = shot.pose.get_rotation_matrix().T
bs.append(r.dot(b))
ids.append(shot_id)
if len(os) >= 2:
thresholds = len(os) * [reproj_threshold]
e, X = pygeometry.triangulate_bearings_midpoint(
os, bs, thresholds, min_ray_angle)
return X
def td_errors(data: DataSetBase, tracks_manager, reconstructions):
errors = []
reproj_threshold = data.config["triangulation_threshold"]
min_ray_angle_degrees = data.config["triangulation_min_ray_angle"]
for rec in reconstructions:
reproj_errors = rec.map.compute_reprojection_errors(
tracks_manager, pymap.ErrorType.Pixel)
# For each point (cap to 1000 samples)
# get the first (up to) 3 cameras that
# triangulate a point and sample around
# the projection error radius (4 points)
# by computing all triangulation permutations
for p in list(rec.points.values())[:1000]:
track_obs = tracks_manager.get_track_observations(p.id)
err_perms = []
# Add error projection permutations
for shot_id, obs in track_obs.items():
if not shot_id in reproj_errors:
continue
rerr = reproj_errors[shot_id][p.id]
err_perms.append([
rerr * np.array([1, 1]), rerr * np.array([-1, 1]),
rerr * np.array([1, -1]), rerr * np.array([-1, -1])
])
if len(err_perms) >= 3:
break
# Calculate the cartesian product (try all possibilities)
err_products = np.array(list(product(*err_perms)))
# Triangulate
ray_errors = []
for err_prod in err_products:
os, bs = [], []
i = 0
for shot_id, obs in track_obs.items():
if not shot_id in reproj_errors:
continue
shot = rec.shots[shot_id]
os.append(shot.pose.get_origin())
reprojected_obs = obs.point + err_prod[i]
b = shot.camera.pixel_bearing(np.array(reprojected_obs))
r = shot.pose.get_rotation_matrix().T
bs.append(r.dot(b))
i += 1
if i >= 3:
break
if len(os) >= 2:
thresholds = len(os) * [reproj_threshold]
valid_triangulation, X = pygeometry.triangulate_bearings_midpoint(
os, bs, thresholds, np.radians(min_ray_angle_degrees),
np.radians(180.0 - min_ray_angle_degrees))
if valid_triangulation:
ray_errors.append(X - p.coordinates)
# Take the max. This is the maximum 3D error estimate
# for this point
if len(ray_errors) > 0:
errors.append((np.max(np.array(ray_errors), axis=0)))
return _gps_gcp_errors_stats(errors)
