def reproject_gcps(gcps: List[pymap.GroundControlPoint], reconstruction: types.Reconstruction, reproj_threshold):
output = {}
for gcp in gcps:
point = multiview.triangulate_gcp(
gcp,
reconstruction.shots,
reproj_threshold=reproj_threshold,
min_ray_angle_degrees=0.1,
)
output[gcp.id] = {}
n_obs = len(gcp.observations)
if point is None:
logger.info(f"Could not triangulate {gcp.id} with {n_obs} annotations")
continue
for observation in gcp.observations:
lat, lon, alt = reconstruction.reference.to_lla(*point)
output[gcp.id][observation.shot_id] = {"lla": [lat, lon, alt], "error": 0}
if observation.shot_id not in reconstruction.shots:
continue
shot = reconstruction.shots[observation.shot_id]
reproj = shot.project(point)
error = np.linalg.norm(reproj - observation.projection)
output[gcp.id][observation.shot_id].update(
{
"error": error,
"reprojection": [reproj[0], reproj[1]],
}
)
return output
def gcp_errors(data: DataSetBase, reconstructions):
all_errors = []
gcp = data.load_ground_control_points()
if not gcp:
return {}
all_errors = []
for gcp in gcp:
if not gcp.coordinates.has_value:
continue
for rec in reconstructions:
triangulated = multiview.triangulate_gcp(gcp, rec.shots, 1.0, 0.1)
if triangulated is None:
continue
else:
break
# pyre-fixme[61]: `triangulated` may not be initialized here.
if triangulated is None:
continue
all_errors.append(triangulated - gcp.coordinates.value)
return _gps_gcp_errors_stats(all_errors)
def gcp_errors(data: DataSetBase,
reconstructions: List[types.Reconstruction]) -> Dict[str, Any]:
all_errors = []
reference = data.load_reference()
gcps = data.load_ground_control_points()
if not gcps:
return {}
all_errors = []
for gcp in gcps:
if not gcp.lla:
continue
triangulated = None
for rec in reconstructions:
triangulated = multiview.triangulate_gcp(gcp, rec.shots, 1.0, 0.1)
if triangulated is None:
continue
else:
break
if triangulated is None:
continue
gcp_enu = reference.to_topocentric(*gcp.lla_vec)
all_errors.append(triangulated - gcp_enu)
return _gps_gcp_errors_stats(np.array(all_errors))
def add_gcp_to_bundle(ba, gcp, gcp_std, shots):
"""Add Ground Control Points constraints to the bundle problem."""
for point in gcp:
point_id = "gcp-" + point.id
coordinates = multiview.triangulate_gcp(
point,
shots,
reproj_threshold=1,
min_ray_angle_degrees=0.1,
)
if coordinates is None:
if point.coordinates.has_value:
logger.warning(
f"Could not triangulate GCP '{point.id}'."
f"Using {point.coordinates.value} (derived from lat,lon)"
)
coordinates = point.coordinates.value
else:
logger.warning(
"Cannot initialize GCP '{}'." " Ignoring it".format(point.id)
)
continue
ba.add_point(point_id, coordinates, False)
for observation in point.observations:
if observation.shot_id in shots:
ba.add_point_projection_observation(
observation.shot_id,
point_id,
observation.projection,
gcp_std,
)
def triangulate_gcps(gcps: List[pymap.GroundControlPoint], reconstruction: types.Reconstruction):
coords = []
for gcp in gcps:
res = multiview.triangulate_gcp(
gcp,
reconstruction.shots,
reproj_threshold=1,
min_ray_angle_degrees=0.1,
)
coords.append(res)
return coords
def triangulate_gcps(gcps, reconstruction):
coords = []
for gcp in gcps:
res = multiview.triangulate_gcp(
gcp,
reconstruction.shots,
reproj_threshold=1,
min_ray_angle_degrees=0.1,
)
coords.append(res)
return coords
def triangulate_all_gcp(reconstruction, gcp):
"""Group and triangulate Ground Control Points seen in 2+ images."""
triangulated, measured = [], []
for point in gcp:
x = multiview.triangulate_gcp(
point,
reconstruction.shots,
reproj_threshold=0.004,
min_ray_angle_degrees=2.0,
)
if x is not None:
triangulated.append(x)
measured.append(point.coordinates.value)
return triangulated, measured
def gcp_errors(
candidate: types.Reconstruction, gcps: Dict[str, pymap.GroundControlPoint]
) -> np.ndarray:
errors = []
for gcp in gcps.values():
if not gcp.lla:
continue
triangulated = multiview.triangulate_gcp(gcp, candidate.shots, 1.0, 0.1)
if triangulated is None:
continue
gcp_enu = candidate.reference.to_topocentric(*gcp.lla_vec)
errors.append(triangulated - gcp_enu)
return np.array(errors)
def triangulate_all_gcp(
reconstruction: types.Reconstruction, gcp: List[pymap.GroundControlPoint]
) -> Tuple[List[np.ndarray], List[np.ndarray]]:
"""Group and triangulate Ground Control Points seen in 2+ images."""
triangulated, measured = [], []
for point in gcp:
x = multiview.triangulate_gcp(
point,
reconstruction.shots,
reproj_threshold=0.004,
min_ray_angle_degrees=2.0,
)
if x is not None:
triangulated.append(x)
point_enu = reconstruction.reference.to_topocentric(*point.lla_vec)
measured.append(point_enu)
return triangulated, measured
def add_gcp_to_bundle(
ba: orec.pybundle.BundleAdjuster,
reference: types.TopocentricConverter,
gcp: List[pymap.GroundControlPoint],
gcp_std,
shots,
):
"""Add Ground Control Points constraints to the bundle problem."""
for point in gcp:
point_id = "gcp-" + point.id
coordinates = multiview.triangulate_gcp(
point,
shots,
reproj_threshold=1,
min_ray_angle_degrees=0.1,
)
if coordinates is None:
if point.lla:
enu = reference.to_topocentric(*point.lla_vec)
logger.warning(
f"Could not triangulate GCP '{point.id}'."
f"Using {enu} (derived from lat,lon)"
)
coordinates = enu
else:
logger.warning(
"Cannot initialize GCP '{}'." " Ignoring it".format(point.id)
)
continue
ba.add_point(point_id, coordinates, False)
for observation in point.observations:
if observation.shot_id in shots:
ba.add_point_projection_observation(
observation.shot_id,
point_id,
observation.projection,
gcp_std,
)
def _add_gcp_to_bundle(
ba: pybundle.BundleAdjuster, gcp: List[pymap.GroundControlPoint], shots: List[str]
):
"""Add Ground Control Points constraints to the bundle problem."""
for point in gcp:
point_id = "gcp-" + point.id
coordinates = multiview.triangulate_gcp(
point,
shots,
reproj_threshold=1,
min_ray_angle_degrees=0.1,
)
if coordinates is None:
if point.coordinates.has_value:
coordinates = point.coordinates.value
else:
logger.warning(
"Cannot initialize GCP '{}'." " Ignoring it".format(point.id)
)
continue
ba.add_point(point_id, coordinates, False)
if point.coordinates.has_value:
point_type = pybundle.XYZ if point.has_altitude else pybundle.XY
ba.add_point_position_world(
point_id, point.coordinates.value, 0.1, point_type
)
for observation in point.observations:
if observation.shot_id in shots:
# TODO(pau): move this to a config or per point parameter.
scale = 0.0001
ba.add_point_projection_observation(
observation.shot_id,
point_id,
observation.projection,
scale,
)
def gcp_errors(data, reconstructions):
all_errors = []
gcp = data.load_ground_control_points()
if not gcp:
return {}
all_errors = []
for gcp in gcp:
if not gcp.coordinates.has_value:
continue
for rec in reconstructions:
triangulated = multiview.triangulate_gcp(gcp, rec.shots, 1.0, 0.1)
if triangulated is None:
continue
else:
break
if triangulated is None:
continue
all_errors.append(triangulated - gcp.coordinates.value)
return _gps_gcp_errors_stats(all_errors)
def resect_image(im, camera, gcps, reconstruction, data, dst_reconstruction=None):
"""
Resect an image into a reconstruction based only on GCPs annotations.
Pass another reconstruction to dst_reconstruction
if you want the resected points to be added there instead
Returns:
The resected shot.
"""
threshold = 0.01
min_inliers = 3
bs, Xs = [], []
for gcp in gcps:
obs = _gcp_image_observation(gcp, im)
if not obs:
continue
gcp_3d_coords = multiview.triangulate_gcp(
gcp,
reconstruction.shots,
reproj_threshold=1,
min_ray_angle_degrees=0.1,
)
if gcp_3d_coords is None:
continue
b = camera.pixel_bearing(obs.projection)
bs.append(b)
Xs.append(gcp_3d_coords)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < min_inliers:
logger.info(f"Not enough annotations to resect image {im}")
return None
T = multiview.absolute_pose_ransac(bs, Xs, threshold, 1000, 0.999)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = int(sum(inliers))
logger.info(f"{im} resection inliers: {ninliers} / {len(bs)}")
if dst_reconstruction is None:
dst_reconstruction = reconstruction
if ninliers >= min_inliers:
R = T[:, :3].T
t = -R.dot(T[:, 3])
dst_reconstruction.add_camera(camera)
shot = dst_reconstruction.create_shot(im, camera.id, pygeometry.Pose(R, t))
shot.metadata = helpers.get_image_metadata(data, im)
return shot
else:
logger.info(f"Not enough inliers to resect image {im}")
return None
def gcp_errors(data: DataSetBase,
reconstructions: List[types.Reconstruction]) -> Dict[str, Any]:
all_errors = []
reference = data.load_reference()
gcps = data.load_ground_control_points()
if not gcps:
return {}
all_errors = []
gcp_stats = []
for gcp in gcps:
if not gcp.lla:
continue
triangulated = None
for rec in reconstructions:
triangulated = multiview.triangulate_gcp(gcp, rec.shots, 1.0, 0.1)
if triangulated is None:
continue
else:
break
if triangulated is None:
continue
gcp_enu = reference.to_topocentric(*gcp.lla_vec)
e = triangulated - gcp_enu
all_errors.append(e)
# Begin computation of GCP stats
observations = []
for i, obs in enumerate(gcp.observations):
if not obs.shot_id in rec.shots:
continue
shot = rec.shots[obs.shot_id]
reprojected = shot.project(gcp_enu)
annotated = obs.projection
r_pixel = features.denormalized_image_coordinates(
np.array([[reprojected[0], reprojected[1]]]),
shot.camera.width, shot.camera.height)[0]
r_pixel[0] /= shot.camera.width
r_pixel[1] /= shot.camera.height
a_pixel = features.denormalized_image_coordinates(
np.array([[annotated[0], annotated[1]]]), shot.camera.width,
shot.camera.height)[0]
a_pixel[0] /= shot.camera.width
a_pixel[1] /= shot.camera.height
observations.append({
'shot_id': obs.shot_id,
'annotated': list(a_pixel),
'reprojected': list(r_pixel)
})
gcp_stats.append({
'id': gcp.id,
'coordinates': list(gcp_enu),
'observations': observations,
'error': list(e)
})
# End computation of GCP stats
with open(
os.path.join(data.data_path, "stats",
"ground_control_points.json"), 'w') as f:
f.write(json.dumps(gcp_stats, indent=4))
return _gps_gcp_errors_stats(np.array(all_errors))
