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_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 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 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 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 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_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 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_pose_properties(): """Test pose constructor, getters and setters.""" p = types.Pose([1, 2, 3], [4, 5, 6]) assert np.allclose(p.rotation, [1, 2, 3]) assert type(p.rotation) == np.ndarray assert p.rotation.dtype == float assert np.allclose(p.translation, [4, 5, 6]) assert type(p.translation) == np.ndarray assert p.translation.dtype == float
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 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 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 get_camera_poses(file_path): filename = file_path + 'undistorted/reconstruction.json' camera_poses = {} with open(filename) as json_file: data = json.load(json_file) data = data[0] for shot in data['shots'].keys(): img_data = data['shots'][shot] cam_pose = types.Pose(img_data['rotation'], img_data['translation']) g = cam_pose.get_Rt() camera_poses[shot] = np.vstack((g, np.array([0, 0, 0, 1]))) return camera_poses
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 test_sigleton_pan_tilt_roll(): """Single camera test with pan, tilt, roll priors.""" pan, tilt, roll = 1, 0.3, 0.2 sa = pybundle.BundleAdjuster() sa.add_shot('1', 'cam1', [0.5, 0, 0], [0, 0, 0], False) sa.add_absolute_position('1', [1, 0, 0], 1, '1') sa.add_absolute_pan('1', pan, 1) sa.add_absolute_tilt('1', tilt, 1) sa.add_absolute_roll('1', roll, 1) sa.run() s1 = sa.get_shot('1') pose = types.Pose(s1.r, s1.t) assert np.allclose(pose.get_origin(), [1, 0, 0], atol=1e-6) ptr = geometry.ptr_from_rotation(pose.get_rotation_matrix()) assert np.allclose(ptr, (pan, tilt, roll))
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 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 camera_pose(position, lookat, up): ''' Pose from position and look at direction >>> position = [1.0, 2.0, 3.0] >>> lookat = [0., 10.0, 2.0] >>> up = [0.0, 0.0, 1.0] >>> pose = camera_pose(position, lookat, up) >>> np.allclose(pose.get_origin(), position) True >>> d = normalized(pose.transform(lookat)) >>> np.allclose(d, [0, 0, 1]) True ''' ez = normalized(np.array(lookat) - np.array(position)) ex = normalized(np.cross(ez, up)) ey = normalized(np.cross(ez, ex)) pose = types.Pose() pose.set_rotation_matrix([ex, ey, ez]) pose.set_origin(position) return pose
def test_single_vs_many(): points = np.array([[1, 2, 3], [4, 5, 6]], dtype=float) pixels = np.array([[0.1, 0.2], [0.3, 0.4]], dtype=float) depths = np.array([1, 2], dtype=float) pose = types.Pose([1, 2, 3], [4, 5, 6]) t_single = [pose.transform(p) for p in points] t_many = pose.transform_many(points) assert np.allclose(t_single, t_many) t_single = [pose.transform_inverse(p) for p in points] t_many = pose.transform_inverse_many(points) assert np.allclose(t_single, t_many) cameras = [ _get_perspective_camera(), _get_brown_perspective_camera(), _get_spherical_camera(), ] if context.OPENCV3: cameras.append(_get_fisheye_camera()) for camera, camera_cpp in cameras: p = camera.project_many(points) p_cpp = camera_cpp.project_many(points) assert np.allclose(p, p_cpp) b = camera.pixel_bearing_many(pixels) b_cpp = camera_cpp.pixel_bearing_many(pixels) print(camera) assert np.allclose(b, b_cpp) if hasattr(camera, 'back_project'): q_single = [ camera.back_project(p, d) for p, d in zip(pixels, depths) ] q_many = camera.back_project_many(pixels, depths) assert np.allclose(q_single, q_many)
def get_reconstruction_origin(r): """Compute the origin of a reconstruction.""" s = r.scale pose = types.Pose([r.rx, r.ry, r.rz], [r.tx / s, r.ty / s, r.tz / s]) return pose.get_origin()
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 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 test_pose_inverse(): p = types.Pose([1, 2, 3], [4, 5, 6]) inverse = p.inverse() identity = p.compose(inverse) assert np.allclose(identity.rotation, [0, 0, 0]) assert np.allclose(identity.translation, [0, 0, 0])
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 get_shot_origin(shot): """Compute the origin of a shot.""" pose = types.Pose([shot.rx, shot.ry, shot.rz], [shot.tx, shot.ty, shot.tz]) return pose.get_origin()