def import_images_reconstruction(path_images, keypoints, rec):
"""
Read images.bin, building shots and tracks graph
"""
logger.info("Importing images from {}".format(path_images))
tracks_manager = pysfm.TracksManager()
image_ix_to_shot_id = {}
with open(path_images, "rb") as f:
n_ims = unpack("<Q", f.read(8))[0]
for image_ix in range(n_ims):
image_id = unpack("<I", f.read(4))[0]
q0 = unpack("<d", f.read(8))[0]
q1 = unpack("<d", f.read(8))[0]
q2 = unpack("<d", f.read(8))[0]
q3 = unpack("<d", f.read(8))[0]
t0 = unpack("<d", f.read(8))[0]
t1 = unpack("<d", f.read(8))[0]
t2 = unpack("<d", f.read(8))[0]
camera_id = unpack("<I", f.read(4))[0]
filename = ""
while True:
c = f.read(1).decode()
if c == "\0":
break
filename += c
q = np.array([q0, q1, q2, q3])
q /= np.linalg.norm(q)
t = np.array([t0, t1, t2])
pose = pygeometry.Pose(rotation=quaternion_to_angle_axis(q),
translation=t)
shot = rec.create_shot(filename, str(camera_id), pose)
image_ix_to_shot_id[image_ix] = shot.id
n_points_2d = unpack("<Q", f.read(8))[0]
for point2d_ix in range(n_points_2d):
x = unpack("<d", f.read(8))[0]
y = unpack("<d", f.read(8))[0]
point3d_id = unpack("<Q", f.read(8))[0]
if point3d_id != np.iinfo(np.uint64).max:
kp = keypoints[image_id][point2d_ix]
r, g, b = rec.points[str(point3d_id)].color
obs = pysfm.Observation(
x,
y,
kp[2],
int(r),
int(g),
int(b),
point2d_ix,
)
tracks_manager.add_observation(shot.id, str(point3d_id),
obs)
return tracks_manager, image_ix_to_shot_id
def test_pair_non_rigid(bundle_adjuster) -> None:
"""Simple two rigs test"""
sa = bundle_adjuster
sa.add_rig_instance(
"1",
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[int]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[int]`.
pygeometry.Pose([0, 0, 0], [0, 0, 0]),
{"1": "cam1"},
{"1": "rig_cam1"},
False,
)
sa.add_rig_instance(
"2",
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[int]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[int]`.
pygeometry.Pose([0, 0, 0], [0, 0, 0]),
{"2": "cam1"},
{"2": "rig_cam1"},
False,
)
sa.add_reconstruction("12", False)
sa.add_reconstruction_instance("12", 4, "1")
sa.add_reconstruction_instance("12", 4, "2")
sa.set_scale_sharing("12", False)
sa.add_relative_similarity(
# pyre-fixme[6]: For 5th param expected `ndarray` but got `List[int]`.
# pyre-fixme[6]: For 6th param expected `ndarray` but got `List[int]`.
pybundle.RelativeSimilarity("12", "1", "12", "2", [0, 0, 0],
[-1, 0, 0], 1, 1))
sa.add_rig_instance_position_prior("1", [0, 0, 0], [1, 1, 1], "")
sa.add_rig_instance_position_prior("2", [2, 0, 0], [1, 1, 1], "")
sa.run()
s1 = sa.get_rig_instance_pose("1")
s2 = sa.get_rig_instance_pose("2")
r12 = sa.get_reconstruction("12")
assert np.allclose(s1.translation, [0, 0, 0], atol=1e-6)
assert np.allclose(s2.translation, [-2, 0, 0], atol=1e-6)
assert np.allclose(r12.get_scale("1"), 0.5)
assert np.allclose(r12.get_scale("2"), 0.5)
def _create_rig_instance():
rec = _create_reconstruction(1, {"0": 2})
rig_camera = rec.add_rig_camera(_create_rig_camera())
rig_instance = pymap.RigInstance("1")
shot = pymap.Shot(
"0",
pygeometry.Camera.create_spherical(),
pygeometry.Pose(),
)
rig_instance.add_shot(rig_camera, shot)
return rec, rig_instance, shot
def create_pano_shot(self,
shot_id: str,
camera_id: str,
pose: Optional[pygeometry.Pose] = None) -> pymap.Shot:
if pose is None:
pose = pygeometry.Pose()
rig_camera_id = f"{PANOSHOT_RIG_PREFIX}{camera_id}"
if rig_camera_id not in self.rig_cameras:
self.add_rig_camera(
pymap.RigCamera(pygeometry.Pose(), rig_camera_id))
rig_instance_id = f"{PANOSHOT_RIG_PREFIX}{shot_id}"
if rig_instance_id not in self.rig_instances:
self.add_rig_instance(pymap.RigInstance(rig_instance_id))
created_shot = self.map.create_pano_shot(shot_id, camera_id,
rig_camera_id,
rig_instance_id, pose)
return created_shot
def test_linear_motion_prior_rotation(bundle_adjuster) -> None:
"""Three rigs, middle has no gps or orientation info"""
sa = bundle_adjuster
sa.add_rig_instance(
"1",
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[int]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[int]`.
pygeometry.Pose([0, 0, 0], [0, 0, 0]),
{"1": "cam1"},
{"1": "rig_cam1"},
True,
)
sa.add_rig_instance(
"2",
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[int]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[int]`.
pygeometry.Pose([0, 0, 0], [0, 0, 0]),
{"2": "cam1"},
{"2": "rig_cam1"},
False,
)
sa.add_rig_instance(
"3",
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[int]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[int]`.
pygeometry.Pose([0, 1, 0], [0, 0, 0]),
{"3": "cam1"},
{"3": "rig_cam1"},
True,
)
sa.add_reconstruction("123", False)
sa.add_reconstruction_instance("123", 1, "1")
sa.add_reconstruction_instance("123", 1, "2")
sa.add_reconstruction_instance("123", 1, "3")
sa.set_scale_sharing("123", True)
sa.add_linear_motion("1", "2", "3", 0.3, 0.1, 0.1)
sa.run()
s2 = sa.get_rig_instance_pose("2")
assert np.allclose(s2.rotation, [0, 0.3, 0], atol=1e-6)
def rig_instance_from_json(
reconstruction: types.Reconstruction, instance_id: str, obj: Dict[str, Any]
) -> None:
"""
Read any rig instance from a json shot object
"""
reconstruction.add_rig_instance(pymap.RigInstance(instance_id))
pose = pygeometry.Pose()
pose.rotation = obj["rotation"]
pose.translation = obj["translation"]
reconstruction.rig_instances[instance_id].pose = pose
def resect(tracks_manager, 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, obs in tracks_manager.get_shot_observations(shot_id).items():
if track in reconstruction.points:
b = camera.pixel_bearing(obs.point)
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, 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])
assert shot_id not in reconstruction.shots
shot = reconstruction.create_shot(shot_id, camera.id,
pygeometry.Pose(R, t))
shot.metadata = metadata
for i, succeed in enumerate(inliers):
if succeed:
add_observation_to_reconstruction(tracks_manager,
reconstruction, shot_id,
ids[i])
return True, report
else:
return False, report
def add_shots_to_reconstruction(
shot_ids: List[str],
positions: List[np.ndarray],
rotations: List[np.ndarray],
camera: pygeometry.Camera,
reconstruction: types.Reconstruction,
):
reconstruction.add_camera(camera)
for shot_id, position, rotation in zip(shot_ids, positions, rotations):
pose = pygeometry.Pose(rotation)
pose.set_origin(position)
reconstruction.create_shot(shot_id, camera.id, pose)
def _return_shot() -> pymap.Shot:
"""Create a reconstruction and return a shot from it.
After leaving this function, the reconstruction object will no-longer
exist but the shot should keep alive the Map object it belongs to.
"""
rec = types.Reconstruction()
camera1 = pygeometry.Camera.create_spherical()
camera1.id = "camera1"
rec.add_camera(camera1)
rec.create_shot("shot1", "camera1", pygeometry.Pose())
return rec.shots["shot1"]
def test_shot_view_ref_count() -> None:
"""Test that accessing shots via shot views maintains the map alive."""
rec = types.Reconstruction()
camera1 = pygeometry.Camera.create_spherical()
camera1.id = "camera1"
rec.add_camera(camera1)
rec.create_shot("shot1", "camera1", pygeometry.Pose())
rec.create_shot("shot2", "camera1", pygeometry.Pose())
# The reconstruction has ref count = 2
count = sys.getrefcount(rec)
assert count == 2
# The map has a bigger ref count because all the views are referencing it
count = sys.getrefcount(rec.map)
assert count == 9
# The shot_view starts with ref count = 2
base_count = sys.getrefcount(rec.shot_view)
assert base_count == 2
# The getting a shot raises shot_view's ref count
shot = rec.shot_view["shot1"]
count = sys.getrefcount(rec.shot_view)
assert count == 3
# Creating an iterator also raises shot_view's ref count
vals = rec.shot_view.values()
count = sys.getrefcount(rec.shot_view)
assert count == 4
# Deleting the shot decreases the ref count
del shot
count = sys.getrefcount(rec.shot_view)
assert count == 3
# Deleting the iterator also decreases the ref count
del vals
count = sys.getrefcount(rec.shot_view)
assert count == 2
def test_rig_instance_create():
rec = _create_reconstruction(1, {"0": 2})
rig_model = _create_rig_model()
rig_instance = pymap.RigInstance(rig_model)
shot = pymap.Shot("0", pygeometry.Camera.create_spherical(),
pygeometry.Pose())
rig_instance.add_shot("rig_camera", shot)
rec.add_rig_instance(rig_instance)
assert len(rec.rig_instances) == 1
assert list(rec.rig_models.keys()) == ["rig_model"]
assert list(rec.rig_instances[0].shots.keys()) == ["0"]
def test_many_observations_delete() -> None:
# Given a map with 10 shots, 1000 landmarks ...
m = pymap.Map()
n_cams = 2
n_shots = 10
n_landmarks = 1000
for cam_id in range(n_cams):
cam = pygeometry.Camera.create_perspective(0.5, 0, 0)
cam.id = "cam" + str(cam_id)
m.create_camera(cam)
m.create_rig_camera(pymap.RigCamera(pygeometry.Pose(), cam.id))
for shot_id in range(n_shots):
cam_id = "cam" + str(int(np.random.rand(1) * 10 % n_cams))
shot_id = str(shot_id)
m.create_rig_instance(shot_id)
m.create_shot(shot_id, cam_id, cam_id, shot_id, pygeometry.Pose())
for point_id in range(n_landmarks):
m.create_landmark(str(point_id), np.random.rand(3))
# ... and random connections (observations) between shots and points
n_total_obs = 0
for lm in m.get_landmarks().values():
n_obs = 0
for shot in m.get_shots().values():
# create a new observation
obs = pymap.Observation(100, 200, 0.5, 255, 0, 0, int(lm.id))
m.add_observation(shot, lm, obs)
n_obs += 1
n_total_obs += 1
# (we expect it to be created correctly)
for lm in m.get_landmarks().values():
n_total_obs -= lm.number_of_observations()
assert n_total_obs == 0
# and when we clear all the observations
m.clear_observations_and_landmarks()
def test_clean_landmarks_with_min_observations() -> None:
m = pymap.Map()
n_cams = 2
n_shots = 2
n_landmarks = 10
for cam_id in range(n_cams):
cam = pygeometry.Camera.create_perspective(0.5, 0, 0)
cam.id = "cam" + str(cam_id)
m.create_camera(cam)
m.create_rig_camera(pymap.RigCamera(pygeometry.Pose(), cam.id))
for shot_id in range(n_shots):
cam_id = "cam" + str(int(np.random.rand(1) * 10 % n_cams))
m.create_rig_instance(str(shot_id))
m.create_shot(str(shot_id), cam_id, cam_id, str(shot_id),
pygeometry.Pose())
for point_id in range(n_landmarks):
m.create_landmark(str(point_id), np.random.rand(3))
for point_id in range(int(n_landmarks / 2)):
for shot in m.get_shots().values():
# create a new observation
obs = pymap.Observation(100, 200, 0.5, 255, 0, 0, point_id)
m.add_observation(shot, m.get_landmark(str(point_id)), obs)
for point_id in range(int(n_landmarks / 2), n_landmarks):
shot = m.get_shot("0")
# create a new observation
obs = pymap.Observation(100, 200, 0.5, 255, 0, 0, point_id)
m.add_observation(shot, m.get_landmark(str(point_id)), obs)
m.clean_landmarks_below_min_observations(n_shots)
assert len(m.get_landmarks()) == int(n_landmarks / 2)
m.clean_landmarks_below_min_observations(n_shots + 1)
assert len(m.get_landmarks()) == 0
def test_python_vs_cpp_pose():
# identity pose
py_pose = Pose()
cpp_pose = pygeometry.Pose()
_helper_poses_equal_py_cpp(py_pose, cpp_pose)
R_cw = special_ortho_group.rvs(3)
t_cw = np.random.rand(3)
py_pose = Pose(cv2.Rodrigues(R_cw)[0].flatten(), t_cw)
cpp_pose = pygeometry.Pose(R_cw, t_cw)
_helper_poses_equal_py_cpp(py_pose, cpp_pose)
new_origin = np.random.rand(3)
py_pose.set_origin(new_origin)
cpp_pose.set_origin(new_origin)
_helper_poses_equal_py_cpp(py_pose, cpp_pose)
R_cw_2 = special_ortho_group.rvs(3)
t_cw_2 = np.random.rand(3)
py_pose_2 = Pose(cv2.Rodrigues(R_cw_2)[0].flatten(), t_cw_2)
cpp_pose_2 = pygeometry.Pose(R_cw_2, t_cw_2)
_helper_poses_equal_py_cpp(py_pose_2, cpp_pose_2)
_helper_poses_equal_py_cpp(py_pose.compose(
py_pose_2.inverse()), cpp_pose.relative_to(cpp_pose_2))
def rig_instance_from_json(reconstruction, key, obj):
"""
Read any rig instance from a json shot object
"""
instance_id = int(key)
reconstruction.add_rig_instance(pymap.RigInstance(instance_id))
pose = pygeometry.Pose()
pose.rotation = obj["rotation"]
pose.translation = obj["translation"]
reconstruction.rig_instances[instance_id].pose = pose
for shot_id, rig_camera_id in obj["rig_camera_ids"].items():
reconstruction.rig_instances[instance_id].add_shot(
reconstruction.rig_cameras[rig_camera_id],
reconstruction.shots[shot_id])
def add_rigs_to_reconstruction(shots, positions, rotations, rig_cameras,
reconstruction):
rec_rig_cameras = []
for rig_camera in rig_cameras:
if rig_camera.id not in reconstruction.rig_cameras:
rec_rig_cameras.append(reconstruction.add_rig_camera(rig_camera))
else:
rec_rig_cameras.append(reconstruction.rig_cameras[rig_camera.id])
for i, (i_shots, position,
rotation) in enumerate(zip(shots, positions, rotations)):
rig_instance = reconstruction.add_rig_instance(pymap.RigInstance(i))
for j, s in enumerate(i_shots):
rig_instance.add_shot(rec_rig_cameras[j],
reconstruction.get_shot(s[0]))
rig_instance.pose = pygeometry.Pose(rotation, -rotation.dot(position))
def perspective_views_of_a_panorama(
spherical_shot: pymap.Shot,
width: int,
reconstruction: types.Reconstruction,
image_format: str,
rig_instance_count: Iterator[int],
):
"""Create 6 perspective views of a panorama."""
camera = pygeometry.Camera.create_perspective(0.5, 0.0, 0.0)
camera.id = "perspective_panorama_camera"
camera.width = width
camera.height = width
reconstruction.add_camera(camera)
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)),
]
rig_instance = pymap.RigInstance(next(rig_instance_count))
rig_instance.pose = spherical_shot.pose
shots = []
for name, rotation in zip(names, rotations):
if name not in reconstruction.rig_cameras:
rig_camera_pose = pygeometry.Pose()
rig_camera_pose.set_rotation_matrix(rotation[:3, :3])
rig_camera = pymap.RigCamera(rig_camera_pose, name)
reconstruction.add_rig_camera(rig_camera)
rig_camera = reconstruction.rig_cameras[name]
shot_id = add_image_format_extension(
f"{spherical_shot.id}_perspective_view_{name}", image_format
)
shot = reconstruction.create_shot(shot_id, camera.id)
shot.metadata = spherical_shot.metadata
rig_instance.add_shot(rig_camera, shot)
shots.append(shot)
reconstruction.add_rig_instance(rig_instance)
return shots
def test_pose_setter():
R_cw = special_ortho_group.rvs(3)
t_cw = np.random.rand(3)
T_cw = np.vstack((np.column_stack((R_cw, t_cw)), np.array([0, 0, 0, 1])))
T_wc = np.linalg.inv(T_cw)
r_cw = Rotation.from_dcm(R_cw).as_rotvec()
r_wc = -r_cw
# set world to cam
p1 = pygeometry.Pose()
p1.set_from_world_to_cam(T_cw)
_helper_pose_equal_to_T(p1, T_cw)
p2 = pygeometry.Pose()
p2.set_from_world_to_cam(R_cw, t_cw)
_helper_pose_equal_to_T(p2, T_cw)
p3 = pygeometry.Pose()
p3.set_from_world_to_cam(r_cw, t_cw)
_helper_pose_equal_to_T(p3, T_cw)
# set cam to world
p4 = pygeometry.Pose()
p4.set_from_cam_to_world(T_wc)
_helper_pose_equal_to_T(p4, T_cw)
p5 = pygeometry.Pose()
p5.set_from_cam_to_world(T_wc[0:3, 0:3], T_wc[0:3, 3])
_helper_pose_equal_to_T(p5, T_cw)
p6 = pygeometry.Pose()
p6.set_from_cam_to_world(r_wc, T_wc[0:3, 3])
_helper_pose_equal_to_T(p6, T_cw)
# set rotation, translation
p7 = pygeometry.Pose()
p7.rotation = r_cw
p7.translation = t_cw
_helper_pose_equal_to_T(p7, T_cw)
p8 = pygeometry.Pose()
p8.set_rotation_matrix(R_cw)
p8.translation = t_cw
_helper_pose_equal_to_T(p7, T_cw)
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
r = types.Reconstruction()
r.add_camera(camera)
shot = r.create_shot("shot1", camera.id,
pygeometry.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_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 = pygeometry.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_sigleton(bundle_adjuster) -> None:
"""Single camera test"""
sa = bundle_adjuster
sa.add_rig_instance(
"1",
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[float]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[int]`.
pygeometry.Pose([0.5, 0, 0], [0, 0, 0]),
{"1": "cam1"},
{"1": "rig_cam1"},
False,
)
sa.add_rig_instance_position_prior("1", [1, 0, 0], [1, 1, 1], "")
sa.add_absolute_up_vector("1", [0, -1, 0], 1)
sa.add_absolute_pan("1", np.radians(180), 1)
sa.run()
s1 = sa.get_rig_instance_pose("1")
assert np.allclose(s1.translation, [1, 0, 0], atol=1e-6)
def add_camera_sequence(
self,
camera: pygeometry.Camera,
length: float,
height: float,
interval: float,
position_noise: List[float],
rotation_noise: float,
positions_shift: Optional[List[float]] = None,
end: Optional[float] = None,
) -> "SyntheticStreetScene":
default_noise_interval = 0.25 * interval
actual_end = length if end is None else end
generator = self.generator
assert generator
positions, rotations = sg.generate_cameras(
sg.samples_generator_interval(length, actual_end, interval,
default_noise_interval),
generator,
height,
)
sg.perturb_points(positions, position_noise)
sg.perturb_rotations(rotations, rotation_noise)
if positions_shift:
positions += np.array(positions_shift)
shift = 0 if len(self.shot_ids) == 0 else sum(
len(s) for s in self.shot_ids)
new_shot_ids = [f"Shot {shift+i:04d}" for i in range(len(positions))]
self.shot_ids.append(new_shot_ids)
self.cameras.append([camera])
rig_camera = pymap.RigCamera(pygeometry.Pose(), camera.id)
self.rig_cameras.append([rig_camera])
rig_instances = []
for shot_id in new_shot_ids:
rig_instances.append([(shot_id, camera.id)])
self.rig_instances.append(rig_instances)
self.instances_positions.append(positions)
self.instances_rotations.append(rotations)
return self
def add_shot(
data: DataSetBase,
reconstruction: types.Reconstruction,
rig_assignments: Dict[str, Tuple[int, str, List[str]]],
shot_id: str,
pose: pygeometry.Pose,
) -> Set[str]:
"""Add a shot to the recontruction.
In case of a shot belonging to a rig instance, the pose of
shot will drive the initial pose setup of the rig instance.
All necessary shots and rig models will be created.
"""
added_shots = set()
if shot_id not in rig_assignments:
camera_id = data.load_exif(shot_id)["camera"]
shot = reconstruction.create_shot(shot_id, camera_id, pose)
shot.metadata = get_image_metadata(data, shot_id)
added_shots = {shot_id}
else:
instance_id, _, instance_shots = rig_assignments[shot_id]
created_shots = {}
for shot in instance_shots:
camera_id = data.load_exif(shot)["camera"]
created_shots[shot] = reconstruction.create_shot(
shot, camera_id, pygeometry.Pose()
)
created_shots[shot].metadata = get_image_metadata(data, shot)
rig_instance = reconstruction.add_rig_instance(pymap.RigInstance(instance_id))
for shot in instance_shots:
_, rig_camera_id, _ = rig_assignments[shot]
rig_instance.add_shot(
reconstruction.rig_cameras[rig_camera_id], created_shots[shot]
)
rig_instance.update_instance_pose_with_shot(shot_id, pose)
added_shots = set(instance_shots)
return added_shots
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 = pygeometry.Pose()
pose.set_rotation_matrix([ex, ey, ez])
pose.set_origin(position)
return pose
def _reconstruction_from_rigs_and_assignments(data: DataSetBase):
assignments = data.load_rig_assignments()
rig_cameras = data.load_rig_cameras()
if not data.reference_lla_exists():
data.invent_reference_lla()
base_rotation = np.zeros(3)
reconstruction = types.Reconstruction()
reconstruction.cameras = data.load_camera_models()
for instance in assignments:
for image, camera_id in instance:
rig_camera = rig_cameras[camera_id]
rig_pose = pygeometry.Pose(base_rotation)
rig_pose.set_origin(orec.get_image_metadata(data, image).gps_position.value)
d = data.load_exif(image)
shot = reconstruction.create_shot(image, d["camera"])
shot.pose = rig_camera.pose.compose(rig_pose)
shot.metadata = orec.get_image_metadata(data, image)
return [reconstruction]
def test_depthmap_to_ply() -> None:
height, width = 2, 3
camera = pygeometry.Camera.create_perspective(0.8, 0.0, 0.0)
camera.id = "cam1"
camera.height = height
camera.width = width
r = types.Reconstruction()
r.add_camera(camera)
shot = r.create_shot(
# pyre-fixme[6]: For 1st param expected `ndarray` but got `List[float]`.
# pyre-fixme[6]: For 2nd param expected `ndarray` but got `List[float]`.
"shot1",
camera.id,
pygeometry.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_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"
r = types.Reconstruction()
r.add_camera(camera)
shot = r.create_shot("1", camera.id,
pygeometry.Pose(np.random.rand(3), np.random.rand(3)))
shot.metadata.gps_position.value = [0, 0, 0]
shot.metadata.gps_accuracy.value = 1
camera_priors = {camera.id: camera}
gcp = []
myconfig = config.default_config()
reconstruction.bundle(r, camera_priors, gcp, myconfig)
shot = r.shots[shot.id]
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 camera_pose(position: np.ndarray, lookat: np.ndarray,
up: np.ndarray) -> pygeometry.Pose:
"""
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
"""
def normalized(x: np.ndarray) -> np.ndarray:
return x / np.linalg.norm(x)
ez = normalized(np.array(lookat) - np.array(position))
ex = normalized(np.cross(ez, up))
ey = normalized(np.cross(ez, ex))
pose = pygeometry.Pose()
pose.set_rotation_matrix(np.array([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 = pygeometry.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())
cameras.append(_get_fisheye_opencv_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)
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 reconstruction_from_metadata(data: DataSetBase, images: Iterable[str]) -> types.Reconstruction:
"""Initialize a reconstruction by using EXIF data for constructing shot poses and cameras."""
data.init_reference()
rig_assignments = rig.rig_assignments_per_image(data.load_rig_assignments())
reconstruction = types.Reconstruction()
reconstruction.reference = data.load_reference()
reconstruction.cameras = data.load_camera_models()
for image in images:
camera_id = data.load_exif(image)["camera"]
if image in rig_assignments:
rig_instance_id, rig_camera_id, _ = rig_assignments[image]
else:
rig_instance_id = image
rig_camera_id = camera_id
reconstruction.add_rig_camera(pymap.RigCamera(pygeometry.Pose(), rig_camera_id))
reconstruction.add_rig_instance(pymap.RigInstance(rig_instance_id))
shot = reconstruction.create_shot(
shot_id=image,
camera_id=camera_id,
rig_camera_id=rig_camera_id,
rig_instance_id=rig_instance_id,
)
shot.metadata = get_image_metadata(data, image)
if not shot.metadata.gps_position.has_value:
reconstruction.remove_shot(image)
continue
gps_pos = shot.metadata.gps_position.value
shot.pose.set_rotation_matrix(rotation_from_shot_metadata(shot))
shot.pose.set_origin(gps_pos)
shot.scale = 1.0
return reconstruction