def triangulate(self, track, reproj_threshold, min_ray_angle_degrees, return_reason=False):
"""Triangulate a track and add point to reconstruction."""
os, bs = [], []
for shot_id in self.graph[track]:
# This will not add in new image, it will only triangulate the shots that are included
if shot_id in self.reconstruction.shots:
# The formed track
# one track, and the subset of the images in reconstruction right now
shot = self.reconstruction.shots[shot_id]
os.append(self._shot_origin(shot))
x = self.graph[track][shot_id]['feature']
b = shot.camera.pixel_bearing(np.array(x))
r = self._shot_rotation_inverse(shot)
bs.append(r.dot(b))
if len(os) >= 2:
# error and triangulated 3D point
e, X = csfm.triangulate_bearings_midpoint(
os, bs, reproj_threshold, np.radians(min_ray_angle_degrees))
if X is not None:
point = types.Point()
point.id = track
point.coordinates = X.tolist()
self.reconstruction.add_point(point)
else:
e = 4
if return_reason:
return e
'''
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)
res, X = csfm.triangulate_bearings_midpoint([o1, o2], [b1, b2],
2 * [max_reprojection],
min_ray_angle)
assert np.allclose(X, [0, 0, 1.0])
assert res == 0
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.shot_coordinates))
r = shot.pose.get_rotation_matrix().T
bs.append(r.dot(b))
if len(os) >= 2:
e, X = csfm.triangulate_bearings_midpoint(
os, bs, reproj_threshold, np.radians(min_ray_angle_degrees))
return X
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.shot_coordinates))
r = shot.pose.get_rotation_matrix().T
bs.append(r.dot(b))
if len(os) >= 2:
e, X = csfm.triangulate_bearings_midpoint(
os, bs, reproj_threshold, np.radians(min_ray_angle_degrees))
return X
def triangulate(self, track, reproj_threshold, min_ray_angle_degrees):
"""Triangulate track and add point to reconstruction."""
os, bs = [], []
for shot_id in self.graph[track]:
if shot_id in self.reconstruction.shots:
shot = self.reconstruction.shots[shot_id]
os.append(self._shot_origin(shot))
x = self.graph[track][shot_id]['feature']
b = shot.camera.pixel_bearing(np.array(x))
r = self._shot_rotation_inverse(shot)
bs.append(r.dot(b))
if len(os) >= 2:
e, X = csfm.triangulate_bearings_midpoint(
os, bs, reproj_threshold, np.radians(min_ray_angle_degrees))
if X is not None:
point = types.Point()
point.id = track
point.coordinates = X.tolist()
self.reconstruction.add_point(point)
def triangulate(self, track, reproj_threshold, min_ray_angle_degrees):
"""Triangulate track and add point to reconstruction."""
os, bs = [], []
for shot_id in self.graph[track]:
if shot_id in self.reconstruction.shots:
shot = self.reconstruction.shots[shot_id]
os.append(self._shot_origin(shot))
x = self.graph[track][shot_id]['feature']
b = shot.camera.pixel_bearing(np.array(x))
r = self._shot_rotation_inverse(shot)
bs.append(r.dot(b))
if len(os) >= 2:
e, X = csfm.triangulate_bearings_midpoint(
os, bs, reproj_threshold, np.radians(min_ray_angle_degrees))
if X is not None:
point = types.Point()
point.id = track
point.coordinates = X.tolist()
self.reconstruction.add_point(point)
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 = csfm.triangulate_bearings_midpoint(
os, bs, thresholds, min_ray_angle)
return X
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 in self.graph[track]:
if shot_id in self.reconstruction.shots:
shot = self.reconstruction.shots[shot_id]
os.append(self._shot_origin(shot))
x = self.graph[track][shot_id]['feature']
b = shot.camera.pixel_bearing(np.array(x))
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 = csfm.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])
