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()
