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.reference = data.load_reference() 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
def test_shot_neighborhood_linear_graph(): graph = nx.Graph() reconstruction = types.Reconstruction() graph.add_node('im0', bipartite=0) shot = types.Shot() shot.id = 'im0' reconstruction.add_shot(shot) for i in range(1, 4): shot = types.Shot() shot.id = 'im' + str(i) point = types.Point() point.id = str(i) prev_shot_id = 'im' + str(i - 1) reconstruction.add_shot(shot) reconstruction.add_point(point) graph.add_node(shot.id, bipartite=0) graph.add_node(point.id, bipartite=1) graph.add_edge(shot.id, point.id) graph.add_edge(prev_shot_id, point.id) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 1) assert interior == set(['im2']) assert boundary == set(['im1', 'im3']) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 2) assert interior == set(['im1', 'im2', 'im3']) assert boundary == set(['im0']) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 3) assert interior == set(['im0', 'im1', 'im2', 'im3']) assert boundary == set()
def test_reconstruction_class_initialization(): # Instantiate Reconstruction reconstruction = types.Reconstruction() # Instantiate camera instrinsics camera = types.PerspectiveCamera() camera.id = 'apple iphone 4s back camera 4.28mm f/2.4' camera.focal = 0.9722222222222222 camera.k1 = 0.006094395128698237 camera.k2 = -0.0004952058188617129 camera.height = 2448 camera.width = 3264 # Instantiate GPS data metadata = types.ShotMetadata() metadata.orientation = 1 metadata.capture_time = 0.0 metadata.gps_dop = 5.0 metadata.gps_position = [ 1.0815875281451939, -0.96510451436708888, 1.2042133903991235 ] # Instantiate shots pose0 = types.Pose() pose0.rotation = [0.0, 0.0, 0.0] pose0.translation = [0.0, 0.0, 0.0] shot0 = types.Shot() shot0.id = 0 shot0.camera = camera shot0.pose = pose0 shot0.metadata = metadata pose1 = types.Pose() pose1.rotation = [0.0, 0.0, 0.0] pose1.translation = [-1.0, 0.0, 0.0] shot1 = types.Shot() shot1.id = 1 shot1.camera = camera shot1.pose = pose1 shot1.metadata = metadata # Add info to current reconstruction reconstruction.add_camera(camera) reconstruction.add_shot(shot0) reconstruction.add_shot(shot1) # TEST assert len(reconstruction.cameras) == 1 assert len(reconstruction.shots) == 2 assert len(reconstruction.points) == 0 assert reconstruction.get_camera(camera.id) == camera assert reconstruction.get_camera(1) is None assert reconstruction.get_shot(shot0.id) == shot0 assert reconstruction.get_shot(shot1.id) == shot1 assert reconstruction.get_shot(2) is None
def undistort_reconstruction(self, graph, reconstruction, data): urec = types.Reconstruction() urec.points = reconstruction.points logger.debug('Undistorting the reconstruction') undistorted_shots = {} for shot in reconstruction.shots.values(): if shot.camera.projection_type == 'perspective': urec.add_camera(shot.camera) urec.add_shot(shot) undistorted_shots[shot.id] = [shot] elif shot.camera.projection_type == 'brown': ushot = types.Shot() ushot.id = shot.id ushot.camera = perspective_camera_from_brown(shot.camera) ushot.pose = shot.pose ushot.metadata = shot.metadata urec.add_camera(ushot.camera) urec.add_shot(ushot) undistorted_shots[shot.id] = [ushot] elif shot.camera.projection_type == 'fisheye': ushot = types.Shot() ushot.id = shot.id ushot.camera = perspective_camera_from_fisheye(shot.camera) ushot.pose = shot.pose ushot.metadata = shot.metadata urec.add_camera(ushot.camera) urec.add_shot(ushot) undistorted_shots[shot.id] = [ushot] elif shot.camera.projection_type in [ 'equirectangular', 'spherical' ]: subshot_width = int(data.config['depthmap_resolution']) subshots = perspective_views_of_a_panorama(shot, subshot_width) for subshot in subshots: urec.add_camera(subshot.camera) urec.add_shot(subshot) add_subshot_tracks(graph, shot, subshot) undistorted_shots[shot.id] = subshots data.save_undistorted_reconstruction([urec]) arguments = [] for shot in reconstruction.shots.values(): arguments.append( (shot, undistorted_shots[shot.id], data, 'load_image', 'save_undistorted_image', cv2.INTER_AREA)) arguments.append( (shot, undistorted_shots[shot.id], data, 'load_mask', 'save_undistorted_mask', cv2.INTER_NEAREST)) arguments.append( (shot, undistorted_shots[shot.id], data, 'load_segmentation', 'save_undistorted_segmentation', cv2.INTER_NEAREST)) processes = data.config['processes'] parallel_map(undistort_image, arguments, processes)
def bootstrap_reconstruction(data, graph, im1, im2): '''Starts a reconstruction using two shots. ''' print 'Initial reconstruction with', im1, 'and', im2 d1 = data.load_exif(im1) d2 = data.load_exif(im2) cameras = data.load_camera_models() camera1 = cameras[d1['camera']] camera2 = cameras[d2['camera']] tracks, p1, p2 = matching.common_tracks(graph, im1, im2) print 'Number of common tracks', len(tracks) threshold = data.config.get('five_point_algo_threshold', 0.006) R, t, inliers = two_view_reconstruction(p1, p2, camera1, camera2, threshold) if len(inliers) > 5: print 'Number of inliers', len(inliers) reconstruction = types.Reconstruction() reconstruction.cameras = cameras shot1 = types.Shot() shot1.id = im1 shot1.camera = cameras[str(d1['camera'])] shot1.pose = types.Pose() shot1.metadata = get_image_metadata(data, im1) reconstruction.add_shot(shot1) shot2 = types.Shot() shot2.id = im2 shot2.camera = cameras[str(d2['camera'])] shot2.pose = types.Pose() shot2.pose.rotation = R shot2.pose.translation = t shot2.metadata = get_image_metadata(data, im2) reconstruction.add_shot(shot2) triangulate_shot_features( graph, reconstruction, im1, data.config.get('triangulation_threshold', 0.004), data.config.get('triangulation_min_ray_angle', 2.0)) print 'Number of reconstructed 3D points :{}'.format( len(reconstruction.points)) if len(reconstruction.points) > data.config.get( 'five_point_algo_min_inliers', 50): print 'Found initialize good pair', im1, 'and', im2 bundle_single_view(graph, reconstruction, im2, data.config) retriangulate(graph, reconstruction, data.config) bundle_single_view(graph, reconstruction, im2, data.config) return reconstruction print 'Pair', im1, ':', im2, 'fails' return None
def bootstrap_reconstruction(data, graph, im1, im2, p1, p2, my_init=False): """Start a reconstruction using two shots.""" logger.info("Starting reconstruction with {} and {}".format(im1, im2)) d1 = data.load_exif(im1) d2 = data.load_exif(im2) cameras = data.load_camera_models() camera1 = cameras[d1['camera']] camera2 = cameras[d2['camera']] logger.info("Common tracks: {}".format(len(p1))) # thresh = data.config.get('five_point_algo_threshold', 0.006) thresh = data.config.get('five_point_algo_threshold', 0.1) min_inliers = data.config.get('five_point_algo_min_inliers', 50) if (not my_init): R, t, inliers = two_view_reconstruction(p1, p2, camera1, camera2, thresh) else: R, t, inliers = two_view_reconstruction_my(p1, p2, thresh) if len(inliers) > 5: logger.info("Two-view reconstruction inliers {}".format(len(inliers))) reconstruction = types.Reconstruction() reconstruction.cameras = cameras shot1 = types.Shot() shot1.id = im1 shot1.camera = cameras[str(d1['camera'])] shot1.pose = types.Pose() shot1.metadata = get_image_metadata(data, im1) reconstruction.add_shot(shot1) shot2 = types.Shot() shot2.id = im2 shot2.camera = cameras[str(d2['camera'])] 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.get('triangulation_threshold', 0.004), data.config.get('triangulation_min_ray_angle', 2.0)) logger.info("Triangulated: {}".format(len(reconstruction.points))) if len(reconstruction.points) > min_inliers: bundle_single_view(graph, reconstruction, im2, data.config) retriangulate(graph, reconstruction, data.config) bundle_single_view(graph, reconstruction, im2, data.config) return reconstruction logger.info("Starting reconstruction with {} and {} failed")
def bootstrap_reconstruction(data, graph, im1, im2, p1, p2): """Start a reconstruction using two shots.""" logger.info("Starting reconstruction with {} and {}".format(im1, im2)) d1 = data.load_exif(im1) d2 = data.load_exif(im2) cameras = data.load_camera_models() camera1 = cameras[d1['camera']] camera2 = cameras[d2['camera']] logger.info("Common tracks: {}".format(len(p1))) thresh = data.config.get('five_point_algo_threshold', 0.006) min_inliers = data.config.get('five_point_algo_min_inliers', 50) # TODO: why we are using five point methods? there is still possibility that # TODO: input calibration from visualSFM # the camera info is not complete, the estimated camera models are 0.0 default. R, t, inliers = two_view_reconstruction(p1, p2, camera1, camera2, thresh) if len(inliers) > 5: logger.info("Two-view reconstruction inliers {}".format(len(inliers))) reconstruction = types.Reconstruction() reconstruction.cameras = cameras shot1 = types.Shot() shot1.id = im1 shot1.camera = cameras[str(d1['camera'])] shot1.pose = types.Pose() shot1.metadata = get_image_metadata(data, im1) reconstruction.add_shot(shot1) shot2 = types.Shot() shot2.id = im2 shot2.camera = cameras[str(d2['camera'])] shot2.pose = types.Pose(R, t) shot2.metadata = get_image_metadata(data, im2) reconstruction.add_shot(shot2) # triangulate the remaining keypoints (that is not included in two_view_reconstruction) in im1 triangulate_shot_features( graph, reconstruction, im1, data.config.get('triangulation_threshold', 0.004), data.config.get('triangulation_min_ray_angle', 2.0)) logger.info("Triangulated: {}".format(len(reconstruction.points))) if len(reconstruction.points) > min_inliers: # only bundle the second image bundle_single_view(graph, reconstruction, im2, data.config) # retriangulate all points in all images retriangulate(graph, reconstruction, data.config) # refine the second image again bundle_single_view(graph, reconstruction, im2, data.config) return reconstruction logger.info("Starting reconstruction with {} and {} failed")
def test_shot_project_back_project(): pixels = np.array([[0.1, 0.2], [-0.1, 0.2]], dtype=float) depths = np.array([1, 2], dtype=float) pose = types.Pose([1, 2, 3], [4, 5, 6]) cameras = [ _get_perspective_camera(), _get_brown_perspective_camera(), _get_spherical_camera(), ] if context.OPENCV3: cameras.append(_get_fisheye_camera()) shot = types.Shot() shot.pose = pose for pair in cameras: for cam in pair: shot.camera = cam bp_single = [ shot.back_project(p, d) for p, d in zip(pixels, depths) ] bp_many = shot.back_project_many(pixels, depths) assert np.allclose(bp_single, bp_many), cam.projection_type px_single = [shot.project(p) for p in bp_single] px_many = shot.project_many(bp_many) assert np.allclose(pixels, px_single), cam.projection_type assert np.allclose(pixels, px_many), cam.projection_type
def shot_from_json(key, obj, cameras): """ Read shot from a json object """ pose = types.Pose() pose.rotation = obj["rotation"] if "translation" in obj: pose.translation = obj["translation"] metadata = types.ShotMetadata() metadata.orientation = obj.get("orientation") metadata.capture_time = obj.get("capture_time") metadata.gps_dop = obj.get("gps_dop") metadata.gps_position = obj.get("gps_position") shot = types.Shot() shot.id = key shot.metadata = metadata shot.pose = pose shot.camera = cameras.get(obj["camera"]) if 'scale' in obj: shot.scale = obj['scale'] if 'covariance' in obj: shot.covariance = np.array(obj['covariance']) if 'merge_cc' in obj: shot.merge_cc = obj['merge_cc'] if 'vertices' in obj and 'faces' in obj: shot.mesh = types.ShotMesh() shot.mesh.vertices = obj['vertices'] shot.mesh.faces = obj['faces'] return shot
def resect(data, graph, reconstruction, shot_id): """Try resecting and adding a shot to the reconstruction. Return: True on success. """ exif = data.load_exif(shot_id) camera = reconstruction.cameras[exif['camera']] # 1. collect all tracks that is in the reconstruction and this image # pixel bearing and reconstructed 3D positions 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 # 2. estimate the pose of this camera using KNEIP method threshold = data.config.get('resection_threshold', 0.004) T = pyopengv.absolute_pose_ransac( bs, Xs, "KNEIP", 1 - np.cos(threshold), 1000) R = T[:, :3] t = T[:, 3] # 3. reproject all points and figure out which is inliers 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 = sum(inliers) # 4. output resecting inliners logger.info("{} resection inliers: {} / {}".format( shot_id, ninliers, len(bs))) if ninliers >= data.config.get('resection_min_inliers', 15): # 5. if inliers are enough, then add this shot to the reconstruction 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 = get_image_metadata(data, shot_id) reconstruction.add_shot(shot) # 6. and do single view bundle adjustment bundle_single_view(graph, reconstruction, shot_id, data.config) return True else: return False
def test_bundle_alignment_prior(): """Test that cameras are aligned to have the Y axis pointing down.""" camera = pygeometry.Camera.create_perspective(1.0, 0.0, 0.0) camera.id = 'camera1' shot = types.Shot() shot.id = '1' shot.camera = camera shot.pose = types.Pose(np.random.rand(3), np.random.rand(3)) shot.metadata = types.ShotMetadata() shot.metadata.gps_position = [0, 0, 0] shot.metadata.gps_dop = 1 r = types.Reconstruction() r.add_camera(camera) r.add_shot(shot) graph = nx.Graph() camera_priors = {camera.id: camera} gcp = [] myconfig = config.default_config() reconstruction.bundle(graph, r, camera_priors, gcp, myconfig) assert np.allclose(shot.pose.translation, np.zeros(3)) # up vector in camera coordinates is (0, -1, 0) assert np.allclose(shot.pose.transform([0, 0, 1]), [0, -1, 0])
def perspective_views_of_a_panorama(spherical_shot, width): """Create 6 perspective views of a panorama.""" camera = types.PerspectiveCamera() camera.id = 'perspective_panorama_camera' camera.width = width camera.height = width camera.focal = 0.5 camera.k1 = camera.k2 = 0.0 names = ['front', 'left', 'back', 'right', 'top', 'bottom'] rotations = [ tf.rotation_matrix(-0 * np.pi / 2, (0, 1, 0)), tf.rotation_matrix(-1 * np.pi / 2, (0, 1, 0)), tf.rotation_matrix(-2 * np.pi / 2, (0, 1, 0)), tf.rotation_matrix(-3 * np.pi / 2, (0, 1, 0)), tf.rotation_matrix(-np.pi / 2, (1, 0, 0)), tf.rotation_matrix(+np.pi / 2, (1, 0, 0)), ] shots = [] for name, rotation in zip(names, rotations): shot = types.Shot() shot.id = '{}_perspective_view_{}'.format(spherical_shot.id, name) shot.camera = camera R = np.dot(rotation[:3, :3], spherical_shot.pose.get_rotation_matrix()) o = spherical_shot.pose.get_origin() shot.pose = types.Pose() shot.pose.set_rotation_matrix(R) shot.pose.set_origin(o) shots.append(shot) return shots
def get_shot_with_different_camera(shot, camera): """Copy shot and replace camera.""" ushot = types.Shot() ushot.id = shot.id ushot.camera = camera ushot.pose = shot.pose ushot.metadata = shot.metadata return ushot
def resect(graph, graph_inliers, reconstruction, shot_id, camera, metadata, threshold, min_inliers): """Try resecting and adding a shot to the reconstruction. Return: True on success. """ bs, Xs, ids = [], [], [] 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) ids.append(track) 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) for i, succeed in enumerate(inliers): if succeed: copy_graph_data(graph, graph_inliers, shot_id, ids[i]) return True, report else: return False, report
def resect(data, graph, reconstruction, shot_id): """Try resecting and adding a shot to the reconstruction. Return: True on success. """ exif = data.load_exif(shot_id) camera = reconstruction.cameras[exif['camera']] 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)} threshold = data.config['resection_threshold'] T = run_absolute_pose_ransac(bs, Xs, "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 >= data.config['resection_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 = get_image_metadata(data, shot_id) reconstruction.add_shot(shot) bundle_single_view(graph, reconstruction, shot_id, data.config) return True, report else: return False, report
def add_shots_to_reconstruction(positions, rotations, camera, reconstruction): shift = len(reconstruction.shots) for i, item in enumerate(zip(positions, rotations)): shot = types.Shot() shot.id = 'shot' + str(shift + i) shot.camera = camera shot.pose = types.Pose() shot.pose.set_rotation_matrix(item[1]) shot.pose.set_origin(item[0]) reconstruction.add_shot(shot) reconstruction.add_camera(camera)
def reconstruction_from_nvm(data_path, nvm_file, reconstruction_file=None): if not nvm_file or not os.path.isfile(nvm_file): logger.info('\tNo NVM file found!') return None with open(nvm_file, 'r') as fin: lines = fin.readlines() # initialization reconstruction = types.Reconstruction() camera = types.PerspectiveCamera() camera.id = 'dummy_camera' camera.width = 640 camera.height = 480 camera.focal = 0.85 reconstruction.add_camera(camera) for d,datum in enumerate(lines): # Header # NVM_V3 # if d < 2: # Skip header continue if d == 2: num_cameras = int(datum) continue # Actual camera information # 000001.jpg 1156.19 0.530173 0.000750277 0.846232 -0.0529769 3.23925 0.140912 0.566615 0.0318637 0 # <File name> <focal length> <quaternion WXYZ> <camera center> <radial distortion> 0 shot_key, focal, qw, qx, qy, qz, ox, oy, oz, radial_distortion, _ = datum.split() q = Quaternion(np.array([float(qw), float(qx), float(qy), float(qz)])) shot = types.Shot() shot.id = shot_key shot.camera = camera shot.pose = types.Pose() shot.pose.set_rotation_matrix(q.rotation_matrix) shot.pose.set_origin([float(ox), float(oy), float(oz)]) reconstruction.add_shot(shot) if d == num_cameras + 2: break # save reconstruction if reconstruction_file is not None: with open(reconstruction_file, 'wb') as fout: obj = reconstructions_to_json([reconstruction]) json_dump(obj, fout) return reconstruction
def build_reconstruction(opensfm_path, log_file, dataset_path): if not opensfm_path in sys.path: sys.path.insert(1, opensfm_path) from opensfm import dataset, matching, reconstruction, types, io from opensfm.reconstruction import TrackTriangulator # from opensfm import learners from opensfm import log global types Rs, ts, subsampled_images, timestamps = parse_log_file(log_file, sample_rate=10) generate_dataset(dataset_path, subsampled_images) recon = types.Reconstruction() camera = build_camera() recon.add_camera(camera) for i, rotation in enumerate(Rs): pose = types.Pose() # pose.rotation = Rs[i] # pose.set_rotation_matrix(pose.get_rotation_matrix().T) pose.set_rotation_matrix(Rs[i]) # pose.set_rotation_matrix(-pose.get_rotation_matrix()) pose.set_rotation_matrix(pose.get_rotation_matrix().T) pose.set_origin(np.array(ts[i])) # pose.translation = 20.0 * pose.translation # pose.set_rotation_matrix(np.random.rand(3,3)) # pose.set_rotation_matrix(pose.get_rotation_matrix().T) # pose.set_origin(np.array(ts[i])) # pose.translation = np.array(ts[i]) # pose.translation = ts[i] # pose.translation = 20.0 * pose.translation shot = types.Shot() shot.camera = camera shot.pose = pose shot.id = '{}.png'.format(timestamps[i]) sm = types.ShotMetadata() sm.orientation = 1 sm.gps_position = [0.0, 0.0, 0.0] sm.gps_dop = 999999.0 shot.metadata = sm # add shot to reconstruction recon.add_shot(shot) data = dataset.DataSet(dataset_path) data.save_reconstruction([recon], 'reconstruction_gt.json')
def resect(data, graph, reconstruction, shot_id): '''Add a shot to the reconstruction. ''' exif = data.load_exif(shot_id) camera = reconstruction.cameras[exif['camera']] 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 threshold = data.config.get('resection_threshold', 0.004) T = pyopengv.absolute_pose_ransac(bs, Xs, "KNEIP", 1 - np.cos(threshold), 1000) 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 = sum(inliers) print 'Resection', shot_id, 'inliers:', ninliers, '/', len(bs) if ninliers >= data.config.get('resection_min_inliers', 15): 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 = get_image_metadata(data, shot_id) reconstruction.add_shot(shot) bundle_single_view(graph, reconstruction, shot_id, data.config) return True else: return False
def build_reconstruction(opensfm_path, log_file, trans_file, dataset_path): if not opensfm_path in sys.path: sys.path.insert(1, opensfm_path) from opensfm import dataset, matching, reconstruction, types, io from opensfm.reconstruction import TrackTriangulator from opensfm import log global types T = parse_log_file(log_file) T_g = parse_trans_file(trans_file) pose_g = types.Pose() pose_g.set_rotation_matrix(np.matrix(T_g[0:3, 0:3])) pose_g.set_rotation_matrix(np.matrix(T_g[0:3, 0:3]).T) pose_g.set_origin(T_g[0:3, 3]) recon = types.Reconstruction() camera = build_camera() recon.add_camera(camera) for i, transformation in enumerate(T): pose = types.Pose() pose.set_rotation_matrix(np.matrix(transformation[0:3, 0:3])) pose.set_rotation_matrix(np.matrix(transformation[0:3, 0:3]).T) pose.set_origin(transformation[0:3, 3]) pose = pose.compose(pose_g) shot = types.Shot() shot.camera = camera shot.pose = pose shot.id = '{}.jpg'.format(str(i + 1).zfill(6)) sm = types.ShotMetadata() sm.orientation = 1 sm.gps_position = [0.0, 0.0, 0.0] sm.gps_dop = 999999.0 shot.metadata = sm # add shot to reconstruction recon.add_shot(shot) data = dataset.DataSet(dataset_path) data.save_reconstruction([recon], 'reconstruction_gt.json')
def test_depthmap_to_ply(): height, width = 2, 3 camera = pygeometry.Camera.create_perspective(0.8, 0.0, 0.0) camera.id = 'cam1' camera.height = height camera.width = width shot = types.Shot() shot.id = 'shot1' shot.camera = camera shot.pose = types.Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]) image = np.zeros((height, width, 3)) depth = np.ones((height, width)) ply = dense.depthmap_to_ply(shot, depth, image) assert len(ply.splitlines()) == 16
def __init__(self, num_cameras, num_points, noise, outlier_fraction): self.cameras = {} for i in range(num_cameras): camera = types.PerspectiveCamera() camera.id = 'camera' + str(i) camera.focal = 0.9 camera.k1 = -0.1 camera.k2 = 0.01 camera.height = 600 camera.width = 800 self.cameras[camera.id] = camera self.shots = {} for i in range(num_cameras): alpha = float(i) / (num_cameras - 1) position = [alpha, -5.0, 0.5] lookat = [1.0 - alpha, alpha, alpha] up = [alpha * 0.2, alpha * 0.2, 1.0] shot = types.Shot() shot.id = 'shot' + str(i) shot.camera = self.cameras['camera' + str(i)] shot.pose = camera_pose(position, lookat, up) self.shots[shot.id] = shot points = np.random.rand(num_points, 3) self.points = {'point' + str(i): p for i, p in enumerate(points)} g = nx.Graph() for shot_id, shot in iteritems(self.shots): for point_id, point in iteritems(self.points): feature = shot.project(point) feature += np.random.rand(*feature.shape) * noise point_integer = int(point_id.split('point')[1]) g.add_node(shot_id, bipartite=0) g.add_node(point_id, bipartite=1) g.add_edge(shot_id, point_id, feature=feature, feature_id=point_integer, feature_color=(0, 0, 0), feature_scale=0.004) self.tracks = g
def test_shot_neighborhood_complete_graph(): graph = nx.Graph() reconstruction = types.Reconstruction() point = types.Point() point.id = '1' reconstruction.add_point(point) graph.add_node(point.id, bipartite=1) for i in range(4): shot = types.Shot() shot.id = 'im' + str(i) reconstruction.add_shot(shot) graph.add_node(shot.id, bipartite=0) graph.add_edge(shot.id, point.id) interior, boundary = opensfm.reconstruction.shot_neighborhood( graph, reconstruction, 'im2', 2) assert interior == set(['im0', 'im1', 'im2', 'im3']) assert boundary == set()
def test_depthmap_to_ply(): height, width = 2, 3 camera = types.PerspectiveCamera() camera.id = 'cam1' camera.focal = 0.8 camera.k1 = 0.0 camera.k2 = 0.0 camera.height = height camera.width = width shot = types.Shot() shot.id = 'shot1' shot.camera = camera shot.pose = types.Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]) image = np.zeros((height, width, 3)) depth = np.ones((height, width)) ply = dense.depthmap_to_ply(shot, depth, image) assert len(ply.splitlines()) == 16
def __init__(self, num_cameras, num_points, noise): self.cameras = {} for i in range(num_cameras): camera = types.PerspectiveCamera() camera.id = 'camera' + str(i) camera.focal = 0.9 camera.k1 = -0.1 camera.k2 = 0.01 camera.height = 600 camera.width = 800 self.cameras[camera.id] = camera self.shots = {} r = 2.0 for i in range(num_cameras): phi = np.random.rand() * math.pi theta = np.random.rand() * 2.0 * math.pi x = r * np.sin(theta) * np.cos(phi) y = r * np.sin(theta) * np.sin(phi) z = r * np.cos(theta) position = [x, y, z] alpha = np.random.rand() lookat = [0.0, 0, 0] up = [alpha * 0.2, alpha * 0.2, 1.0] shot = types.Shot() shot.id = 'shot' + str(i) shot.camera = self.cameras['camera' + str(i)] shot.pose = camera_pose(position, lookat, up) self.shots[shot.id] = shot points = np.random.rand(num_points, 3) - [0.5, 0.5, 0.5] self.points = {} for i, p in enumerate(points): pt = types.Point() pt.id = 'point' + str(i) pt.coordinates = p pt.color = [100, 100, 20] self.points[pt.id] = pt
def import_bundler(data_path, bundle_file, list_file, track_file, reconstruction_file=None): """ Reconstruction and tracks graph from Bundler's output """ # Init OpenSfM working folder. mkdir_p(data_path) # Copy image list. list_dir = os.path.dirname(list_file) with open(list_file, 'rb') as fin: lines = fin.read().splitlines() ordered_shots = [] image_list = [] for line in lines: image_path = os.path.join(list_dir, line.split()[0]) rel_to_data = os.path.relpath(image_path, data_path) image_list.append(rel_to_data) ordered_shots.append(os.path.basename(image_path)) with open(os.path.join(data_path, 'image_list.txt'), 'w') as fout: fout.write('\n'.join(image_list) + '\n') # Check for bundle_file if not bundle_file or not os.path.isfile(bundle_file): return None with open(bundle_file, 'rb') as fin: lines = fin.readlines() offset = 1 if '#' in lines[0] else 0 # header num_shot, num_point = map(int, lines[offset].split(' ')) offset += 1 # initialization reconstruction = types.Reconstruction() # cameras for i in xrange(num_shot): # Creating a model for each shot. shot_key = ordered_shots[i] focal, k1, k2 = map(float, lines[offset].rstrip('\n').split(' ')) if focal > 0: im = imread(os.path.join(data_path, image_list[i])) height, width = im.shape[0:2] camera = types.PerspectiveCamera() camera.id = 'camera_' + str(i) camera.width = width camera.height = height camera.focal = focal / max(width, height) camera.k1 = k1 camera.k2 = k2 reconstruction.add_camera(camera) # Shots rline = [] for k in xrange(3): rline += lines[offset + 1 + k].rstrip('\n').split(' ') R = ' '.join(rline) t = lines[offset + 4].rstrip('\n').split(' ') R = np.array(map(float, R.split())).reshape(3, 3) t = np.array(map(float, t)) R[1], R[2] = -R[1], -R[2] # Reverse y and z t[1], t[2] = -t[1], -t[2] shot = types.Shot() shot.id = shot_key shot.camera = camera shot.pose = types.Pose() shot.pose.set_rotation_matrix(R) shot.pose.translation = t reconstruction.add_shot(shot) else: print 'ignore failed image', shot_key offset += 5 # tracks track_lines = [] for i in xrange(num_point): coordinates = lines[offset].rstrip('\n').split(' ') color = lines[offset + 1].rstrip('\n').split(' ') point = types.Point() point.id = i point.coordinates = map(float, coordinates) point.color = map(int, color) reconstruction.add_point(point) view_line = lines[offset + 2].rstrip('\n').split(' ') num_view, view_list = int(view_line[0]), view_line[1:] for k in xrange(num_view): shot_key = ordered_shots[int(view_list[4 * k])] if shot_key in reconstruction.shots: camera = reconstruction.shots[shot_key].camera scale = max(camera.width, camera.height) v = '{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format( shot_key, i, view_list[4 * k + 1], float(view_list[4 * k + 2]) / scale, -float(view_list[4 * k + 3]) / scale, point.color[0], point.color[1], point.color[2] ) track_lines.append(v) offset += 3 # save track file with open(track_file, 'wb') as fout: fout.writelines('\n'.join(track_lines)) # save reconstruction if reconstruction_file is not None: with open(reconstruction_file, 'wb') as fout: obj = reconstructions_to_json([reconstruction]) json_dump(obj, fout) return reconstruction
def build_reconstruction(opensfm_path, log_file, dataset_path): if not opensfm_path in sys.path: sys.path.insert(1, opensfm_path) from opensfm import dataset, matching, reconstruction, types, io from opensfm.reconstruction import TrackTriangulator # from opensfm import learners # from opensfm import log global types Rs, ts, subsampled_images = parse_log_file(log_file) recon = types.Reconstruction() camera = build_camera() recon.add_camera(camera) offset = None pose0 = types.Pose() pose0_recon = types.Pose() for i, _ in enumerate(Rs): pose = types.Pose() pose.rotation = Rs[i] # pose.set_rotation_matrix(Rs[i]) # print pose.get_rotation_matrix() pose.set_rotation_matrix(pose.get_rotation_matrix().T) pose.set_origin(np.array(ts[i])) if False and i == 0: print subsampled_images[i] # pose0 = types.Pose() # pose0_recon = types.Pose() pose0.rotation = pose.rotation pose0.translation = pose.translation pose0_recon.rotation = [ 1.46327114203856, 0.6520934519442041, -0.7289951219890223 ] pose0_recon.translation = [ -151.62008675764042, 7.551077656334444, 32.03538718382186 ] if False: R_ = np.matrix(pose.get_rotation_matrix()) * np.matrix( pose0.get_rotation_matrix()).T * np.matrix( pose0_recon.get_rotation_matrix()) pose.set_rotation_matrix(R_) # Bad cases if subsampled_images[i] == '1476939075123622.jpg': print '-' * 100 print '{} : {}'.format(subsampled_images[i], pose.rotation) pose.rotation[2] = math.pi + pose.rotation[2] # Good cases if subsampled_images[i] == '1476939074934112.jpg': print '+' * 100 print '{} : {}'.format(subsampled_images[i], pose.rotation) # if offset is None: # offset = pose.get_origin() - pose0_recon.get_origin() # pose.set_origin(pose.get_origin() - offset) # pose.translation = pose.translation * 0.1 # pose.translation = np.array(ts[i]) # print pose.get_origin() # print pose.get_rotation_matrix() # print pose.get_rotation_matrix() # print pose.get_origin() # sys.exit(1) # t = pose.translation # t[0] = -t[0] # t[1] = -t[1] # t[2] = -t[2] # t[1],t[2] = t[2],t[1] # pose.translation = t # print pose.rotation # print pose.translation # print '#'*100 # sys.exit(1) # R = pose.get_rotation_matrix() # R[:,1], R[:,2] = R[:,2], R[:,1] # pose.set_rotation_matrix(R) # R = pose.get_rotation_matrix() # R[:,1], R[:,2] = R[:,2], R[:,1] # pose.set_rotation_matrix(R.T) # t = pose.translation # t = pose.get_rotation_matrix() * np.matrix(pose.translation.reshape((3,1))) # print pose.translation pose.translation = 20.0 * pose.translation # print pose.translation # sys.exit(1) shot = types.Shot() shot.camera = camera shot.pose = pose shot.id = subsampled_images[i] sm = types.ShotMetadata() sm.orientation = 1 sm.gps_position = [0.0, 0.0, 0.0] sm.gps_dop = 999999.0 # sm.capture_time = 0.0 shot.metadata = sm # add shot to reconstruction recon.add_shot(shot) data = dataset.DataSet(dataset_path) data.save_reconstruction([recon], 'reconstruction_gt.json')
def _add_shot(graph, rec, shot_id): graph.add_node(shot_id, bipartite=0) shot = types.Shot() shot.id = shot_id rec.add_shot(shot)
def convert_image(self, image, img, max_size): image_camera_model = types.SphericalCamera() image_camera_model.id = "v2 nctech pulsar 11000 5500 equirectangular 0.1666" image_camera_model.width = 11000 image_camera_model.height = 5500 undist_tile_size = max_size // 4 undist_img = np.zeros((max_size // 2, max_size, 3), np.uint8) undist_mask = np.full((max_size // 2, max_size, 1), 255, np.uint8) undist_mask[undist_tile_size:2 * undist_tile_size, 2 * undist_tile_size:3 * undist_tile_size] = 0 undist_mask[undist_tile_size:2 * undist_tile_size, undist_tile_size:2 * undist_tile_size] = 0 spherical_shot = types.Shot() spherical_shot.pose = types.Pose() spherical_shot.id = image spherical_shot.camera = image_camera_model perspective_shots = undistort.perspective_views_of_a_panorama( spherical_shot, undist_tile_size) for subshot in perspective_shots: undistorted = undistort.render_perspective_view_of_a_panorama( img, spherical_shot, subshot) subshot_id_prefix = '{}_perspective_view_'.format( spherical_shot.id) subshot_name = subshot.id[ len(subshot_id_prefix):] if subshot.id.startswith( subshot_id_prefix) else subshot.id (subshot_name, ext) = os.path.splitext(subshot_name) if subshot_name == 'front': undist_img[:undist_tile_size, :undist_tile_size] = undistorted # print( 'front') elif subshot_name == 'left': undist_img[:undist_tile_size, undist_tile_size:2 * undist_tile_size] = undistorted # print( 'left') elif subshot_name == 'back': undist_img[:undist_tile_size, 2 * undist_tile_size:3 * undist_tile_size] = undistorted # print( 'back') elif subshot_name == 'right': undist_img[:undist_tile_size, 3 * undist_tile_size:4 * undist_tile_size] = undistorted # print( 'right') elif subshot_name == 'top': undist_img[undist_tile_size:2 * undist_tile_size, 3 * undist_tile_size:4 * undist_tile_size] = undistorted # print( 'top') elif subshot_name == 'bottom': undist_img[undist_tile_size:2 * undist_tile_size, :undist_tile_size] = undistorted # print( 'bottom') # data.save_undistorted_image(subshot.id, undist_img) return undist_img
def _add_shot(graph, reconstruction, shot_id): graph.add_node(shot_id, bipartite=0) shot = types.Shot() shot.id = shot_id reconstruction.add_shot(shot)