Source code for opensfm.reconstruction

# -*- coding: utf-8 -*-
"""Incremental reconstruction pipeline"""

import datetime
import logging
from collections import defaultdict
from itertools import combinations

import cv2
import numpy as np
import pyopengv
import six
from timeit import default_timer as timer
from six import iteritems

from opensfm import csfm
from opensfm import log
from opensfm import tracking
from opensfm import multiview
from opensfm import types
from opensfm.align import align_reconstruction, apply_similarity
from opensfm.context import parallel_map, current_memory_usage


logger = logging.getLogger(__name__)


def _add_camera_to_bundle(ba, camera, constant):
    """Add camera to a bundle adjustment problem."""
    if camera.projection_type == 'perspective':
        ba.add_perspective_camera(
            camera.id, camera.focal, camera.k1, camera.k2,
            camera.focal_prior, camera.k1_prior, camera.k2_prior,
            constant)
    elif camera.projection_type == 'brown':
        c = csfm.BABrownPerspectiveCamera()
        c.id = camera.id
        c.focal_x = camera.focal_x
        c.focal_y = camera.focal_y
        c.c_x = camera.c_x
        c.c_y = camera.c_y
        c.k1 = camera.k1
        c.k2 = camera.k2
        c.p1 = camera.p1
        c.p2 = camera.p2
        c.k3 = camera.k3
        c.focal_x_prior = camera.focal_x_prior
        c.focal_y_prior = camera.focal_y_prior
        c.c_x_prior = camera.c_x_prior
        c.c_y_prior = camera.c_y_prior
        c.k1_prior = camera.k1_prior
        c.k2_prior = camera.k2_prior
        c.p1_prior = camera.p1_prior
        c.p2_prior = camera.p2_prior
        c.k3_prior = camera.k3_prior
        c.constant = constant
        ba.add_brown_perspective_camera(c)
    elif camera.projection_type == 'fisheye':
        ba.add_fisheye_camera(
            camera.id, camera.focal, camera.k1, camera.k2,
            camera.focal_prior, camera.k1_prior, camera.k2_prior,
            constant)
    elif camera.projection_type in ['equirectangular', 'spherical']:
        ba.add_equirectangular_camera(camera.id)


def _get_camera_from_bundle(ba, camera):
    """Read camera parameters from a bundle adjustment problem."""
    if camera.projection_type == 'perspective':
        c = ba.get_perspective_camera(camera.id)
        camera.focal = c.focal
        camera.k1 = c.k1
        camera.k2 = c.k2
    elif camera.projection_type == 'brown':
        c = ba.get_brown_perspective_camera(camera.id)
        camera.focal_x = c.focal_x
        camera.focal_y = c.focal_y
        camera.c_x = c.c_x
        camera.c_y = c.c_y
        camera.k1 = c.k1
        camera.k2 = c.k2
        camera.p1 = c.p1
        camera.p2 = c.p2
        camera.k3 = c.k3
    elif camera.projection_type == 'fisheye':
        c = ba.get_fisheye_camera(camera.id)
        camera.focal = c.focal
        camera.k1 = c.k1
        camera.k2 = c.k2


[docs]def bundle(graph, reconstruction, gcp, config): """Bundle adjust a reconstruction.""" fix_cameras = not config['optimize_camera_parameters'] chrono = Chronometer() ba = csfm.BundleAdjuster() for camera in reconstruction.cameras.values(): _add_camera_to_bundle(ba, camera, fix_cameras) for shot in reconstruction.shots.values(): r = shot.pose.rotation t = shot.pose.translation ba.add_shot( shot.id, shot.camera.id, r[0], r[1], r[2], t[0], t[1], t[2], False ) for point in reconstruction.points.values(): x = point.coordinates ba.add_point(point.id, x[0], x[1], x[2], False) for shot_id in reconstruction.shots: if shot_id in graph: for track in graph[shot_id]: if track in reconstruction.points: ba.add_observation(shot_id, track, *graph[shot_id][track]['feature']) if config['bundle_use_gps']: for shot in reconstruction.shots.values(): g = shot.metadata.gps_position ba.add_position_prior(shot.id, g[0], g[1], g[2], shot.metadata.gps_dop) if config['bundle_use_gcp'] and gcp: for observation in gcp: if observation.shot_id in reconstruction.shots: ba.add_ground_control_point_observation( observation.shot_id, observation.coordinates[0], observation.coordinates[1], observation.coordinates[2], observation.shot_coordinates[0], observation.shot_coordinates[1]) ba.set_loss_function(config['loss_function'], config['loss_function_threshold']) ba.set_reprojection_error_sd(config['reprojection_error_sd']) ba.set_internal_parameters_prior_sd( config['exif_focal_sd'], config['principal_point_sd'], config['radial_distorsion_k1_sd'], config['radial_distorsion_k2_sd'], config['radial_distorsion_p1_sd'], config['radial_distorsion_p2_sd'], config['radial_distorsion_k3_sd']) ba.set_num_threads(config['processes']) ba.set_max_num_iterations(50) ba.set_linear_solver_type("SPARSE_SCHUR") chrono.lap('setup') ba.run() chrono.lap('run') for camera in reconstruction.cameras.values(): _get_camera_from_bundle(ba, camera) for shot in reconstruction.shots.values(): s = ba.get_shot(shot.id) shot.pose.rotation = [s.rx, s.ry, s.rz] shot.pose.translation = [s.tx, s.ty, s.tz] for point in reconstruction.points.values(): p = ba.get_point(point.id) point.coordinates = [p.x, p.y, p.z] point.reprojection_error = p.reprojection_error chrono.lap('teardown') logger.debug(ba.brief_report()) report = { 'wall_times': dict(chrono.lap_times()), 'brief_report': ba.brief_report(), } return report
[docs]def bundle_single_view(graph, reconstruction, shot_id, config): """Bundle adjust a single camera.""" ba = csfm.BundleAdjuster() shot = reconstruction.shots[shot_id] camera = shot.camera _add_camera_to_bundle(ba, camera, constant=True) r = shot.pose.rotation t = shot.pose.translation ba.add_shot( shot.id, camera.id, r[0], r[1], r[2], t[0], t[1], t[2], False ) for track_id in graph[shot_id]: if track_id in reconstruction.points: track = reconstruction.points[track_id] x = track.coordinates ba.add_point(track_id, x[0], x[1], x[2], True) ba.add_observation(shot_id, track_id, *graph[shot_id][track_id]['feature']) if config['bundle_use_gps']: g = shot.metadata.gps_position ba.add_position_prior(shot.id, g[0], g[1], g[2], shot.metadata.gps_dop) ba.set_loss_function(config['loss_function'], config['loss_function_threshold']) ba.set_reprojection_error_sd(config['reprojection_error_sd']) ba.set_internal_parameters_prior_sd( config['exif_focal_sd'], config['principal_point_sd'], config['radial_distorsion_k1_sd'], config['radial_distorsion_k2_sd'], config['radial_distorsion_p1_sd'], config['radial_distorsion_p2_sd'], config['radial_distorsion_k3_sd']) ba.set_num_threads(config['processes']) ba.set_max_num_iterations(10) ba.set_linear_solver_type("DENSE_QR") ba.run() logger.debug(ba.brief_report()) s = ba.get_shot(shot_id) shot.pose.rotation = [s.rx, s.ry, s.rz] shot.pose.translation = [s.tx, s.ty, s.tz]
[docs]def bundle_local(graph, reconstruction, gcp, central_shot_id, config): """Bundle adjust the local neighborhood of a shot.""" chrono = Chronometer() interior, boundary = shot_neighborhood( graph, reconstruction, central_shot_id, config['local_bundle_radius'], config['local_bundle_min_common_points'], config['local_bundle_max_shots']) logger.debug( 'Local bundle sets: interior {} boundary {} other {}'.format( len(interior), len(boundary), len(reconstruction.shots) - len(interior) - len(boundary))) point_ids = set() for shot_id in interior: if shot_id in graph: for track in graph[shot_id]: if track in reconstruction.points: point_ids.add(track) ba = csfm.BundleAdjuster() for camera in reconstruction.cameras.values(): _add_camera_to_bundle(ba, camera, constant=True) for shot_id in interior | boundary: shot = reconstruction.shots[shot_id] r = shot.pose.rotation t = shot.pose.translation ba.add_shot( shot.id, shot.camera.id, r[0], r[1], r[2], t[0], t[1], t[2], shot.id in boundary ) for point_id in point_ids: point = reconstruction.points[point_id] x = point.coordinates ba.add_point(point.id, x[0], x[1], x[2], False) for shot_id in interior | boundary: if shot_id in graph: for track in graph[shot_id]: if track in point_ids: ba.add_observation(shot_id, track, *graph[shot_id][track]['feature']) if config['bundle_use_gps']: for shot_id in interior: shot = reconstruction.shots[shot_id] g = shot.metadata.gps_position ba.add_position_prior(shot.id, g[0], g[1], g[2], shot.metadata.gps_dop) if config['bundle_use_gcp'] and gcp: for observation in gcp: if observation.shot_id in interior: ba.add_ground_control_point_observation( observation.shot_id, observation.coordinates[0], observation.coordinates[1], observation.coordinates[2], observation.shot_coordinates[0], observation.shot_coordinates[1]) ba.set_loss_function(config['loss_function'], config['loss_function_threshold']) ba.set_reprojection_error_sd(config['reprojection_error_sd']) ba.set_internal_parameters_prior_sd( config['exif_focal_sd'], config['principal_point_sd'], config['radial_distorsion_k1_sd'], config['radial_distorsion_k2_sd'], config['radial_distorsion_p1_sd'], config['radial_distorsion_p2_sd'], config['radial_distorsion_k3_sd']) ba.set_num_threads(config['processes']) ba.set_max_num_iterations(10) ba.set_linear_solver_type("DENSE_SCHUR") chrono.lap('setup') ba.run() chrono.lap('run') for shot_id in interior: shot = reconstruction.shots[shot_id] s = ba.get_shot(shot.id) shot.pose.rotation = [s.rx, s.ry, s.rz] shot.pose.translation = [s.tx, s.ty, s.tz] for point in point_ids: point = reconstruction.points[point] p = ba.get_point(point.id) point.coordinates = [p.x, p.y, p.z] point.reprojection_error = p.reprojection_error chrono.lap('teardown') logger.debug(ba.brief_report()) report = { 'wall_times': dict(chrono.lap_times()), 'brief_report': ba.brief_report(), 'num_interior_images': len(interior), 'num_boundary_images': len(boundary), 'num_other_images': (len(reconstruction.shots) - len(interior) - len(boundary)), } return report
[docs]def shot_neighborhood(graph, reconstruction, central_shot_id, radius, min_common_points, max_interior_size): """Reconstructed shots near a given shot. Returns: a tuple with interior and boundary: - interior: the list of shots at distance smaller than radius - boundary: shots sharing at least on point with the interior Central shot is at distance 0. Shots at distance n + 1 share at least min_common_points points with shots at distance n. """ max_boundary_size = 1000000 interior = set([central_shot_id]) for distance in range(1, radius): remaining = max_interior_size - len(interior) if remaining <= 0: break neighbors = direct_shot_neighbors( graph, reconstruction, interior, min_common_points, remaining) interior.update(neighbors) boundary = direct_shot_neighbors( graph, reconstruction, interior, 1, max_boundary_size) return interior, boundary
[docs]def direct_shot_neighbors(graph, reconstruction, shot_ids, min_common_points, max_neighbors): """Reconstructed shots sharing reconstructed points with a shot set.""" points = set() for shot_id in shot_ids: for track_id in graph[shot_id]: if track_id in reconstruction.points: points.add(track_id) candidate_shots = set(reconstruction.shots) - set(shot_ids) common_points = defaultdict(int) for track_id in points: for neighbor in graph[track_id]: if neighbor in candidate_shots: common_points[neighbor] += 1 pairs = sorted(common_points.items(), key=lambda x: -x[1]) neighbors = set() for neighbor, num_points in pairs[:max_neighbors]: if num_points >= min_common_points: neighbors.add(neighbor) else: break return neighbors
[docs]def pairwise_reconstructability(common_tracks, rotation_inliers): """Likeliness of an image pair giving a good initial reconstruction.""" outliers = common_tracks - rotation_inliers outlier_ratio = float(outliers) / common_tracks if outlier_ratio >= 0.3: return outliers else: return 0
[docs]def compute_image_pairs(track_dict, data): """All matched image pairs sorted by reconstructability.""" args = _pair_reconstructability_arguments(track_dict, data) processes = data.config['processes'] result = parallel_map(_compute_pair_reconstructability, args, processes) result = list(result) pairs = [(im1, im2) for im1, im2, r in result if r > 0] score = [r for im1, im2, r in result if r > 0] order = np.argsort(-np.array(score)) return [pairs[o] for o in order]
def _pair_reconstructability_arguments(track_dict, data): threshold = 4 * data.config['five_point_algo_threshold'] cameras = data.load_camera_models() args = [] for (im1, im2), (tracks, p1, p2) in iteritems(track_dict): camera1 = cameras[data.load_exif(im1)['camera']] camera2 = cameras[data.load_exif(im2)['camera']] args.append((im1, im2, p1, p2, camera1, camera2, threshold)) return args def _compute_pair_reconstructability(args): log.setup() im1, im2, p1, p2, camera1, camera2, threshold = args R, inliers = two_view_reconstruction_rotation_only( p1, p2, camera1, camera2, threshold) r = pairwise_reconstructability(len(p1), len(inliers)) return (im1, im2, r)
[docs]def get_image_metadata(data, image): """Get image metadata as a ShotMetadata object.""" metadata = types.ShotMetadata() exif = data.load_exif(image) reference = data.load_reference() if ('gps' in exif and 'latitude' in exif['gps'] and 'longitude' in exif['gps']): lat = exif['gps']['latitude'] lon = exif['gps']['longitude'] if data.config['use_altitude_tag']: alt = exif['gps'].get('altitude', 2.0) else: alt = 2.0 # Arbitrary value used to align the reconstruction x, y, z = reference.to_topocentric(lat, lon, alt) metadata.gps_position = [x, y, z] metadata.gps_dop = exif['gps'].get('dop', 15.0) else: metadata.gps_position = [0.0, 0.0, 0.0] metadata.gps_dop = 999999.0 metadata.orientation = exif.get('orientation', 1) if 'accelerometer' in exif: metadata.accelerometer = exif['accelerometer'] if 'compass' in exif: metadata.compass = exif['compass'] if 'capture_time' in exif: metadata.capture_time = exif['capture_time'] if 'skey' in exif: metadata.skey = exif['skey'] return metadata
def _two_view_reconstruction_inliers(b1, b2, R, t, threshold): """Compute number of points that can be triangulated. Args: b1, b2: Bearings in the two images. R, t: Rotation and translation from the second image to the first. That is the opengv's convention and the opposite of many functions in this module. threshold: max reprojection error in radians. Returns: array: Inlier indices. """ p = pyopengv.triangulation_triangulate(b1, b2, t, R) br1 = p.copy() br1 /= np.linalg.norm(br1, axis=1)[:, np.newaxis] br2 = R.T.dot((p - t).T).T br2 /= np.linalg.norm(br2, axis=1)[:, np.newaxis] ok1 = np.linalg.norm(br1 - b1, axis=1) < threshold ok2 = np.linalg.norm(br2 - b2, axis=1) < threshold return np.nonzero(ok1 * ok2)[0]
[docs]def two_view_reconstruction_plane_based(p1, p2, camera1, camera2, threshold): """Reconstruct two views from point correspondences lying on a plane. Args: p1, p2: lists points in the images camera1, camera2: Camera models threshold: reprojection error threshold Returns: rotation, translation and inlier list """ b1 = camera1.pixel_bearing_many(p1) b2 = camera2.pixel_bearing_many(p2) x1 = multiview.euclidean(b1) x2 = multiview.euclidean(b2) H, inliers = cv2.findHomography(x1, x2, cv2.RANSAC, threshold) motions = multiview.motion_from_plane_homography(H) motion_inliers = [] for R, t, n, d in motions: inliers = _two_view_reconstruction_inliers( b1, b2, R.T, -R.T.dot(t), threshold) motion_inliers.append(inliers) best = np.argmax(map(len, motion_inliers)) R, t, n, d = motions[best] inliers = motion_inliers[best] return cv2.Rodrigues(R)[0].ravel(), t, inliers
[docs]def two_view_reconstruction(p1, p2, camera1, camera2, threshold): """Reconstruct two views using the 5-point method. Args: p1, p2: lists points in the images camera1, camera2: Camera models threshold: reprojection error threshold Returns: rotation, translation and inlier list """ b1 = camera1.pixel_bearing_many(p1) b2 = camera2.pixel_bearing_many(p2) # Note on threshold: # See opengv doc on thresholds here: # http://laurentkneip.github.io/opengv/page_how_to_use.html # Here we arbitrarily assume that the threshold is given for a camera of # focal length 1. Also, arctan(threshold) \approx threshold since # threshold is small T = multiview.relative_pose_ransac( b1, b2, b"STEWENIUS", 1 - np.cos(threshold), 1000, 0.999) R = T[:, :3] t = T[:, 3] inliers = _two_view_reconstruction_inliers(b1, b2, R, t, threshold) if inliers.sum() > 5: T = multiview.relative_pose_optimize_nonlinear(b1[inliers], b2[inliers], t, R) R = T[:, :3] t = T[:, 3] inliers = _two_view_reconstruction_inliers(b1, b2, R, t, threshold) return cv2.Rodrigues(R.T)[0].ravel(), -R.T.dot(t), inliers
def _two_view_rotation_inliers(b1, b2, R, threshold): br2 = R.dot(b2.T).T ok = np.linalg.norm(br2 - b1, axis=1) < threshold return np.nonzero(ok)[0]
[docs]def two_view_reconstruction_rotation_only(p1, p2, camera1, camera2, threshold): """Find rotation between two views from point correspondences. Args: p1, p2: lists points in the images camera1, camera2: Camera models threshold: reprojection error threshold Returns: rotation and inlier list """ b1 = camera1.pixel_bearing_many(p1) b2 = camera2.pixel_bearing_many(p2) R = multiview.relative_pose_ransac_rotation_only( b1, b2, 1 - np.cos(threshold), 1000, 0.999) inliers = _two_view_rotation_inliers(b1, b2, R, threshold) return cv2.Rodrigues(R.T)[0].ravel(), inliers
[docs]def two_view_reconstruction_general(p1, p2, camera1, camera2, threshold): """Reconstruct two views from point correspondences. These will try different reconstruction methods and return the results of the one with most inliers. Args: p1, p2: lists points in the images camera1, camera2: Camera models threshold: reprojection error threshold Returns: rotation, translation and inlier list """ R_5p, t_5p, inliers_5p = two_view_reconstruction( p1, p2, camera1, camera2, threshold) R_plane, t_plane, inliers_plane = two_view_reconstruction_plane_based( p1, p2, camera1, camera2, threshold) report = { '5_point_inliers': len(inliers_5p), 'plane_based_inliers': len(inliers_plane), } if len(inliers_5p) > len(inliers_plane): report['method'] = '5_point' return R_5p, t_5p, inliers_5p, report else: report['method'] = 'plane_based' return R_plane, t_plane, inliers_plane, report
[docs]def bootstrap_reconstruction(data, graph, im1, im2, p1, p2): """Start a reconstruction using two shots.""" logger.info("Starting reconstruction with {} and {}".format(im1, im2)) report = { 'image_pair': (im1, im2), 'common_tracks': len(p1), } cameras = data.load_camera_models() camera1 = cameras[data.load_exif(im1)['camera']] camera2 = cameras[data.load_exif(im2)['camera']] threshold = data.config['five_point_algo_threshold'] min_inliers = data.config['five_point_algo_min_inliers'] R, t, inliers, report['two_view_reconstruction'] = \ two_view_reconstruction_general(p1, p2, camera1, camera2, threshold) logger.info("Two-view reconstruction inliers: {} / {}".format( len(inliers), len(p1))) if len(inliers) <= 5: report['decision'] = "Could not find initial motion" logger.info(report['decision']) return None, report reconstruction = types.Reconstruction() reconstruction.cameras = cameras shot1 = types.Shot() shot1.id = im1 shot1.camera = camera1 shot1.pose = types.Pose() shot1.metadata = get_image_metadata(data, im1) reconstruction.add_shot(shot1) shot2 = types.Shot() shot2.id = im2 shot2.camera = camera2 shot2.pose = types.Pose(R, t) shot2.metadata = get_image_metadata(data, im2) reconstruction.add_shot(shot2) triangulate_shot_features(graph, reconstruction, im1, data.config) logger.info("Triangulated: {}".format(len(reconstruction.points))) report['triangulated_points'] = len(reconstruction.points) if len(reconstruction.points) < min_inliers: report['decision'] = "Initial motion did not generate enough points" logger.info(report['decision']) return None, report bundle_single_view(graph, reconstruction, im2, data.config) retriangulate(graph, reconstruction, data.config) bundle_single_view(graph, reconstruction, im2, data.config) report['decision'] = 'Success' report['memory_usage'] = current_memory_usage() return reconstruction, report
[docs]def reconstructed_points_for_images(graph, reconstruction, images): """Number of reconstructed points visible on each image. Returns: A list of (image, num_point) pairs sorted by decreasing number of points. """ res = [] for image in images: if image not in reconstruction.shots: common_tracks = 0 for track in graph[image]: if track in reconstruction.points: common_tracks += 1 res.append((image, common_tracks)) return sorted(res, key=lambda x: -x[1])
[docs]def resect(graph, reconstruction, shot_id, camera, metadata, threshold, min_inliers): """Try resecting and adding a shot to the reconstruction. Return: True on success. """ bs = [] Xs = [] for track in graph[shot_id]: if track in reconstruction.points: x = graph[track][shot_id]['feature'] b = camera.pixel_bearing(x) bs.append(b) Xs.append(reconstruction.points[track].coordinates) bs = np.array(bs) Xs = np.array(Xs) if len(bs) < 5: return False, {'num_common_points': len(bs)} T = multiview.absolute_pose_ransac( bs, Xs, b"KNEIP", 1 - np.cos(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("{} resection inliers: {} / {}".format( shot_id, ninliers, len(bs))) report = { 'num_common_points': len(bs), 'num_inliers': ninliers, } if ninliers >= min_inliers: R = T[:, :3].T t = -R.dot(T[:, 3]) shot = types.Shot() shot.id = shot_id shot.camera = camera shot.pose = types.Pose() shot.pose.set_rotation_matrix(R) shot.pose.translation = t shot.metadata = metadata reconstruction.add_shot(shot) return True, report else: return False, report
def corresponding_tracks(tracks1, tracks2): features1 = {tracks1[t1]["feature_id"]: t1 for t1 in tracks1} corresponding_tracks = [] for t2 in tracks2: feature_id = tracks2[t2]["feature_id"] if feature_id in features1: corresponding_tracks.append((features1[feature_id], t2)) return corresponding_tracks def compute_common_tracks(reconstruction1, reconstruction2, graph1, graph2): common_tracks = set() common_images = set(reconstruction1.shots.keys()).intersection( reconstruction2.shots.keys()) for image in common_images: for t1, t2 in corresponding_tracks(graph1[image], graph2[image]): if t1 in reconstruction1.points and t2 in reconstruction2.points: common_tracks.add((t1, t2)) return list(common_tracks) def resect_reconstruction(reconstruction1, reconstruction2, graph1, graph2, threshold, min_inliers): common_tracks = compute_common_tracks( reconstruction1, reconstruction2, graph1, graph2) worked, similarity, inliers = align_two_reconstruction( reconstruction1, reconstruction2, common_tracks, threshold) if not worked: return False, [], [] inliers = [common_tracks[inliers[i]] for i in range(len(inliers))] return True, similarity, inliers
[docs]class TrackTriangulator: """Triangulate tracks in a reconstruction. Caches shot origin and rotation matrix """ def __init__(self, graph, reconstruction): """Build a triangulator for a specific reconstruction.""" self.graph = graph self.reconstruction = reconstruction self.origins = {} self.rotation_inverses = {} self.Rts = {}
[docs] 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: thresholds = len(os) * [reproj_threshold] e, X = csfm.triangulate_bearings_midpoint( os, bs, thresholds, 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)
[docs] def triangulate_dlt(self, track, reproj_threshold, min_ray_angle_degrees): """Triangulate track using DLT and add point to reconstruction.""" Rts, bs = [], [] for shot_id in self.graph[track]: if shot_id in self.reconstruction.shots: shot = self.reconstruction.shots[shot_id] Rts.append(self._shot_Rt(shot)) x = self.graph[track][shot_id]['feature'] b = shot.camera.pixel_bearing(np.array(x)) bs.append(b) if len(Rts) >= 2: e, X = csfm.triangulate_bearings_dlt( Rts, 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 _shot_origin(self, shot): if shot.id in self.origins: return self.origins[shot.id] else: o = shot.pose.get_origin() self.origins[shot.id] = o return o def _shot_rotation_inverse(self, shot): if shot.id in self.rotation_inverses: return self.rotation_inverses[shot.id] else: r = shot.pose.get_rotation_matrix().T self.rotation_inverses[shot.id] = r return r def _shot_Rt(self, shot): if shot.id in self.Rts: return self.Rts[shot.id] else: r = shot.pose.get_Rt() self.Rts[shot.id] = r return r
[docs]def triangulate_shot_features(graph, reconstruction, shot_id, config): """Reconstruct as many tracks seen in shot_id as possible.""" reproj_threshold = config['triangulation_threshold'] min_ray_angle = config['triangulation_min_ray_angle'] triangulator = TrackTriangulator(graph, reconstruction) for track in graph[shot_id]: if track not in reconstruction.points: triangulator.triangulate(track, reproj_threshold, min_ray_angle)
[docs]def retriangulate(graph, reconstruction, config): """Retrianguate all points""" chrono = Chronometer() report = {} report['num_points_before'] = len(reconstruction.points) threshold = config['triangulation_threshold'] min_ray_angle = config['triangulation_min_ray_angle'] triangulator = TrackTriangulator(graph, reconstruction) tracks = set() for image in reconstruction.shots.keys(): if image in graph: tracks.update(graph[image].keys()) for track in tracks: triangulator.triangulate(track, threshold, min_ray_angle) report['num_points_after'] = len(reconstruction.points) chrono.lap('retriangulate') report['wall_time'] = chrono.total_time() return report
[docs]def remove_outliers(graph, reconstruction, config): """Remove points with large reprojection error.""" threshold = config['bundle_outlier_threshold'] if threshold > 0: outliers = [] for track in reconstruction.points: error = reconstruction.points[track].reprojection_error if error > threshold: outliers.append(track) for track in outliers: del reconstruction.points[track] logger.info("Removed outliers: {}".format(len(outliers)))
[docs]def shot_lla_and_compass(shot, reference): """Lat, lon, alt and compass of the reconstructed shot position.""" topo = shot.pose.get_origin() lat, lon, alt = reference.to_lla(*topo) dz = shot.viewing_direction() angle = np.rad2deg(np.arctan2(dz[0], dz[1])) angle = (angle + 360) % 360 return lat, lon, alt, angle
[docs]def align_two_reconstruction(r1, r2, common_tracks, threshold): """Estimate similarity transform between two reconstructions.""" t1, t2 = r1.points, r2.points if len(common_tracks) > 6: p1 = np.array([t1[t[0]].coordinates for t in common_tracks]) p2 = np.array([t2[t[1]].coordinates for t in common_tracks]) # 3 samples / 100 trials / 50% outliers = 0.99 probability # with probability = 1-(1-(1-outlier)^model)^trial T, inliers = multiview.fit_similarity_transform( p1, p2, max_iterations=100, threshold=threshold) if len(inliers) > 0: return True, T, inliers return False, None, None
[docs]def merge_two_reconstructions(r1, r2, config, threshold=1): """Merge two reconstructions with common tracks IDs.""" common_tracks = list(set(r1.points) & set(r2.points)) worked, T, inliers = align_two_reconstruction( r1, r2, common_tracks, threshold) if worked and len(inliers) >= 10: s, A, b = multiview.decompose_similarity_transform(T) r1p = r1 apply_similarity(r1p, s, A, b) r = r2 r.shots.update(r1p.shots) r.points.update(r1p.points) align_reconstruction(r, None, config) return [r] else: return [r1, r2]
[docs]def merge_reconstructions(reconstructions, config): """Greedily merge reconstructions with common tracks.""" num_reconstruction = len(reconstructions) ids_reconstructions = np.arange(num_reconstruction) remaining_reconstruction = ids_reconstructions reconstructions_merged = [] num_merge = 0 for (i, j) in combinations(ids_reconstructions, 2): if (i in remaining_reconstruction) and (j in remaining_reconstruction): r = merge_two_reconstructions( reconstructions[i], reconstructions[j], config) if len(r) == 1: remaining_reconstruction = list(set( remaining_reconstruction) - set([i, j])) for k in remaining_reconstruction: rr = merge_two_reconstructions(r[0], reconstructions[k], config) if len(r) == 2: break else: r = rr remaining_reconstruction = list(set( remaining_reconstruction) - set([k])) reconstructions_merged.append(r[0]) num_merge += 1 for k in remaining_reconstruction: reconstructions_merged.append(reconstructions[k]) logger.info("Merged {0} reconstructions".format(num_merge)) return reconstructions_merged
[docs]def paint_reconstruction(data, graph, reconstruction): """Set the color of the points from the color of the tracks.""" for k, point in iteritems(reconstruction.points): point.color = six.next(six.itervalues(graph[k]))['feature_color']
[docs]class ShouldBundle: """Helper to keep track of when to run bundle.""" def __init__(self, data, reconstruction): self.interval = data.config['bundle_interval'] self.new_points_ratio = data.config['bundle_new_points_ratio'] self.reconstruction = reconstruction self.done() def should(self): max_points = self.num_points_last * self.new_points_ratio max_shots = self.num_shots_last + self.interval return (len(self.reconstruction.points) >= max_points or len(self.reconstruction.shots) >= max_shots) def done(self): self.num_points_last = len(self.reconstruction.points) self.num_shots_last = len(self.reconstruction.shots)
[docs]class ShouldRetriangulate: """Helper to keep track of when to re-triangulate.""" def __init__(self, data, reconstruction): self.active = data.config['retriangulation'] self.ratio = data.config['retriangulation_ratio'] self.reconstruction = reconstruction self.done() def should(self): max_points = self.num_points_last * self.ratio return self.active and len(self.reconstruction.points) > max_points def done(self): self.num_points_last = len(self.reconstruction.points)
[docs]def grow_reconstruction(data, graph, reconstruction, images, gcp): """Incrementally add shots to an initial reconstruction.""" config = data.config report = {'steps': []} bundle(graph, reconstruction, None, config) remove_outliers(graph, reconstruction, config) align_reconstruction(reconstruction, gcp, config) should_bundle = ShouldBundle(data, reconstruction) should_retriangulate = ShouldRetriangulate(data, reconstruction) while True: if config['save_partial_reconstructions']: paint_reconstruction(data, graph, reconstruction) data.save_reconstruction( [reconstruction], 'reconstruction.{}.json'.format( datetime.datetime.now().isoformat().replace(':', '_'))) candidates = reconstructed_points_for_images( graph, reconstruction, images) if not candidates: break logger.info("-------------------------------------------------------") threshold = data.config['resection_threshold'] min_inliers = data.config['resection_min_inliers'] for image, num_tracks in candidates: camera = reconstruction.cameras[data.load_exif(image)['camera']] metadata = get_image_metadata(data, image) ok, resrep = resect(graph, reconstruction, image, camera, metadata, threshold, min_inliers) if not ok: continue bundle_single_view(graph, reconstruction, image, data.config) logger.info("Adding {0} to the reconstruction".format(image)) step = { 'image': image, 'resection': resrep, 'memory_usage': current_memory_usage() } report['steps'].append(step) images.remove(image) np_before = len(reconstruction.points) triangulate_shot_features(graph, reconstruction, image, config) np_after = len(reconstruction.points) step['triangulated_points'] = np_after - np_before if should_retriangulate.should(): logger.info("Re-triangulating") b1rep = bundle(graph, reconstruction, None, config) rrep = retriangulate(graph, reconstruction, config) b2rep = bundle(graph, reconstruction, None, config) remove_outliers(graph, reconstruction, config) align_reconstruction(reconstruction, gcp, config) step['bundle'] = b1rep step['retriangulation'] = rrep step['bundle_after_retriangulation'] = b2rep should_retriangulate.done() should_bundle.done() elif should_bundle.should(): brep = bundle(graph, reconstruction, None, config) remove_outliers(graph, reconstruction, config) align_reconstruction(reconstruction, gcp, config) step['bundle'] = brep should_bundle.done() elif config['local_bundle_radius'] > 0: brep = bundle_local(graph, reconstruction, None, image, config) remove_outliers(graph, reconstruction, config) step['local_bundle'] = brep break else: logger.info("Some images can not be added") break logger.info("-------------------------------------------------------") bundle(graph, reconstruction, gcp, config) remove_outliers(graph, reconstruction, config) align_reconstruction(reconstruction, gcp, config) paint_reconstruction(data, graph, reconstruction) return reconstruction, report
[docs]def incremental_reconstruction(data, graph): """Run the entire incremental reconstruction pipeline.""" logger.info("Starting incremental reconstruction") report = {} chrono = Chronometer() tracks, images = tracking.tracks_and_images(graph) chrono.lap('load_tracks_graph') if not data.reference_lla_exists(): data.invent_reference_lla(images) remaining_images = set(images) gcp = None if data.ground_control_points_exist(): gcp = data.load_ground_control_points() common_tracks = tracking.all_common_tracks(graph, tracks) reconstructions = [] pairs = compute_image_pairs(common_tracks, data) chrono.lap('compute_image_pairs') report['num_candidate_image_pairs'] = len(pairs) report['reconstructions'] = [] for im1, im2 in pairs: if im1 in remaining_images and im2 in remaining_images: rec_report = {} report['reconstructions'].append(rec_report) tracks, p1, p2 = common_tracks[im1, im2] reconstruction, rec_report['bootstrap'] = bootstrap_reconstruction( data, graph, im1, im2, p1, p2) if reconstruction: remaining_images.remove(im1) remaining_images.remove(im2) reconstruction, rec_report['grow'] = grow_reconstruction( data, graph, reconstruction, remaining_images, gcp) reconstructions.append(reconstruction) reconstructions = sorted(reconstructions, key=lambda x: -len(x.shots)) for k, r in enumerate(reconstructions): logger.info("Reconstruction {}: {} images, {} points".format( k, len(r.shots), len(r.points))) logger.info("{} partial reconstructions in total.".format( len(reconstructions))) chrono.lap('compute_reconstructions') report['wall_times'] = dict(chrono.lap_times()) report['not_reconstructed_images'] = list(remaining_images) return report, reconstructions
class Chronometer: def __init__(self): self.start() def start(self): t = timer() lap = ('start', 0, t) self.laps = [lap] self.laps_dict = {'start': lap} def lap(self, key): t = timer() dt = t - self.laps[-1][2] lap = (key, dt, t) self.laps.append(lap) self.laps_dict[key] = lap def lap_time(self, key): return self.laps_dict[key][1] def lap_times(self): return [(k, dt) for k, dt, t in self.laps[1:]] def total_time(self): return self.laps[-1][2] - self.laps[0][2]