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