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 = orec.triangulate_gcp(point, shots)
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)
for observation in point.observations:
if observation.shot_id in shots:
ba.add_point_projection_observation(
observation.shot_id,
point_id,
observation.projection[0],
observation.projection[1],
gcp_std,
)
def gcp_to_ply(gcps: List[pymap.GroundControlPoint],
reconstruction: types.Reconstruction):
"""Export GCP position as a PLY string."""
vertices = []
for gcp in gcps:
if gcp.lla:
p = reconstruction.reference.to_topocentric(*gcp.lla_vec)
else:
p = orec.triangulate_gcp(gcp, reconstruction.shots)
if p is None:
logger.warning(
"Could not compute the 3D position of GCP '{}'".format(gcp.id))
continue
c = 255, 0, 0
s = "{} {} {} {} {} {}".format(p.value[0], p.value[1], p.value[2],
int(c[0]), int(c[1]), int(c[2]))
vertices.append(s)
header = [
"ply",
"format ascii 1.0",
"element vertex {}".format(len(vertices)),
"property float x",
"property float y",
"property float z",
"property uchar diffuse_red",
"property uchar diffuse_green",
"property uchar diffuse_blue",
"end_header",
]
return '\n'.join(header + vertices + [''])
def gcp_to_ply(gcps, reconstruction):
"""Export GCP position as a PLY string."""
vertices = []
for gcp in gcps:
if gcp.coordinates is not None:
p = gcp.coordinates
else:
p = orec.triangulate_gcp(gcp, reconstruction.shots)
if p is None:
logger.warning("Could not compute the 3D position of GCP '{}'"
.format(gcp.id))
continue
c = 255, 0, 0
s = "{} {} {} {} {} {}".format(
p[0], p[1], p[2], int(c[0]), int(c[1]), int(c[2]))
vertices.append(s)
header = [
"ply",
"format ascii 1.0",
"element vertex {}".format(len(vertices)),
"property float x",
"property float y",
"property float z",
"property uchar diffuse_red",
"property uchar diffuse_green",
"property uchar diffuse_blue",
"end_header",
]
return '\n'.join(header + vertices + [''])
def main():
args = parse_args()
logging.basicConfig(
format="%(asctime)s %(levelname)s %(name)s: %(message)s",
level=logging.DEBUG)
data = dataset.DataSet(args.dataset)
reconstruction = data.load_reconstruction()[0]
gcps = data.load_ground_control_points()
with io.open_wt(data.data_path + '/gcp.ply') as fout:
fout.write(gcp_to_ply(gcps, reconstruction))
for gcp in gcps:
plt.suptitle("GCP '{}'".format(gcp.id))
if gcp.lla:
coordinates = reconstruction.reference.to_topocentric(*gcp.lla_vec)
else:
coordinates = orec.triangulate_gcp(gcp, reconstruction.shots)
if coordinates is None:
logger.warning(
"Could not compute the 3D position of GCP '{}'".format(gcp.id))
continue
for i, observation in enumerate(gcp.observations):
image = data.load_image(observation.shot_id)
shot = reconstruction.shots[observation.shot_id]
reprojected = shot.project(coordinates.value)
annotated = observation.projection
rpixel = pix_coords(reprojected, image)
apixel = pix_coords(annotated, image)
n = (len(gcp.observations) + 3) / 4
ax = plt.subplot(n, min(len(gcp.observations), 4), i + 1)
plt.imshow(image)
ax.title.set_text("{}".format(observation.shot_id))
plt.scatter(rpixel[0], rpixel[1])
plt.scatter(apixel[0], apixel[1])
plt.show()
def reproject_gcps(gcps, reconstruction):
output = {}
for gcp in gcps:
point = orec.triangulate_gcp(gcp, reconstruction.shots)
output[gcp.id] = {}
n_obs = len(gcp.observations)
if point is None:
print(f"Could not triangulate {gcp.id} with {n_obs} annotations")
continue
for observation in gcp.observations:
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] = {
"error": error,
"reprojection": [reproj[0], reproj[1]],
}
return output
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 = orec.triangulate_gcp(gcp, rec.shots)
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 triangulate_gcps(gcps, reconstruction):
coords = []
for gcp in gcps:
res = orec.triangulate_gcp(gcp, reconstruction.shots)
coords.append(res)
return coords
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 = orec.triangulate_gcp(gcp, reconstruction.shots)
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:
print(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 = orec.get_image_metadata(data, im)
return shot
else:
print(f"Not enough inliers to resect image {im}")
return None