def get_pycolmap_camera(camera_intrinsics: Cal3Bundler) -> pycolmap.Camera:
"""Convert Cal3Bundler intrinsics to a pycolmap-compatible format (a dictionary).
See https://colmap.github.io/cameras.html#camera-models for info about the COLMAP camera models.
Both SIMPLE_PINHOLE and SIMPLE_RADIAL use 1 focal length.
Note: the image width and image height values approximated below are dummy placeholder values.
For some datasets we have intrinsics (including principal point) where image height, image width
would not necessarily be 2 * cy, 2 * cx. However, image dimensions aren't used anywhere
in the F / E / H estimation; rather cx and cy are used in the Essential matrix estimation:
https://github.com/colmap/colmap/blob/9f3a75ae9c72188244f2403eb085e51ecf4397a8/src/base/camera_models.h#L629)
Args:
camera_intrinsics: camera intrinsic parameters.
"""
focal_length = camera_intrinsics.fx()
cx, cy = camera_intrinsics.px(), camera_intrinsics.py()
width = int(cx * 2)
height = int(cy * 2)
camera_dict = pycolmap.Camera(
model="SIMPLE_PINHOLE",
width=width,
height=height,
params=[focal_length, cx, cy],
)
return camera_dict
def check_verifier_output_error(verifier: VerifierBase,
euler_angle_err_tol: float,
translation_err_tol: float) -> None:
"""Check error using annotated correspondences as input, instead of noisy detector-descriptor matches."""
fx, px, py, k1, k2 = load_log_front_center_intrinsics()
keypoints_i1, keypoints_i2 = load_argoverse_log_annotated_correspondences()
# match keypoints row by row
match_indices = np.vstack(
[np.arange(len(keypoints_i1)),
np.arange(len(keypoints_i1))]).T
i2Ri1, i2ti1, _ = verifier.verify(keypoints_i1, keypoints_i2,
match_indices,
Cal3Bundler(fx, k1, k2, px, py),
Cal3Bundler(fx, k1, k2, px, py))
# Ground truth is provided in inverse format, so invert SE(3) object
i2Ti1 = Pose3(i2Ri1, i2ti1.point3())
i1Ti2 = i2Ti1.inverse()
i1ti2 = i1Ti2.translation()
i1Ri2 = i1Ti2.rotation().matrix()
euler_angles = Rotation.from_matrix(i1Ri2).as_euler("zyx", degrees=True)
gt_euler_angles = np.array([-0.37, 32.47, -0.42])
assert np.allclose(gt_euler_angles, euler_angles, atol=euler_angle_err_tol)
gt_i1ti2 = np.array([0.21, -0.0024, 0.976])
assert np.allclose(gt_i1ti2, i1ti2, atol=translation_err_tol)
def test_triangulation_individualCal_without_ransac(self):
"""Tests that the triangulation is accurate for individual camera calibration, without RANSAC-based
triangulation. Checks if cameras and triangulated 3D point are as expected.
"""
k1 = 0
k2 = 0
f, u0, v0 = 1500, 640, 480
f_, u0_, v0_ = 1600, 650, 440
K1 = Cal3Bundler(f, k1, k2, u0, v0)
K2 = Cal3Bundler(f_, k1, k2, u0_, v0_)
keypoints_list, _, cameras = generate_noisy_2d_measurements(
world_point=self.expected_landmark,
calibrations=[K1, K2],
per_image_noise_vecs=np.zeros((2, 2)),
poses=get_pose3_vector(num_poses=2),
)
# create matches
# since there is only one measurement in each image, both assigned feature index 0
matches_dict = {(0, 1): np.array([[0, 0]])}
da = DataAssociation(
reproj_error_thresh=5, # 5 px
min_track_len=2, # at least 2 measurements required
mode=TriangulationParam.NO_RANSAC,
)
sfm_data, _ = da.run(len(cameras), cameras, matches_dict,
keypoints_list)
estimated_landmark = sfm_data.get_track(0).point3()
self.gtsamAssertEquals(estimated_landmark, self.expected_landmark,
1e-2)
for i in sfm_data.get_valid_camera_indices():
self.gtsamAssertEquals(sfm_data.get_camera(i), cameras.get(i))
def test_bundle_adjust(self):
"""Tests the bundle adjustment for relative pose on a simulated scene."""
two_view_estimator = TwoViewEstimator(matcher=None,
verifier=None,
inlier_support_processor=None,
bundle_adjust_2view=True,
eval_threshold_px=4)
# Extract example poses.
i1Ri2 = EXAMPLE_DATA.get_camera(1).pose().rotation()
i1ti2 = EXAMPLE_DATA.get_camera(1).pose().translation()
i2Ti1 = Pose3(i1Ri2, i1ti2).inverse()
i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation()))
# Perform bundle adjustment.
i2Ri1_optimized, i2Ui1_optimized, corr_idxs = two_view_estimator.bundle_adjust(
keypoints_i1=self.keypoints_i1,
keypoints_i2=self.keypoints_i2,
verified_corr_idxs=self.corr_idxs,
camera_intrinsics_i1=Cal3Bundler(),
camera_intrinsics_i2=Cal3Bundler(),
i2Ri1_initial=i2Ei1.rotation(),
i2Ui1_initial=i2Ei1.direction(),
i2Ti1_prior=None,
)
# Assert
rotation_angular_error = comp_utils.compute_relative_rotation_angle(
i2Ri1_optimized, i2Ei1.rotation())
translation_angular_error = comp_utils.compute_relative_unit_translation_angle(
i2Ui1_optimized, i2Ei1.direction())
self.assertLessEqual(rotation_angular_error, 1)
self.assertLessEqual(translation_angular_error, 1)
np.testing.assert_allclose(corr_idxs, self.corr_idxs)
def test_values(self):
values = Values()
E = EssentialMatrix(Rot3(), Unit3())
tol = 1e-9
values.insert(0, Point2(0, 0))
values.insert(1, Point3(0, 0, 0))
values.insert(2, Rot2())
values.insert(3, Pose2())
values.insert(4, Rot3())
values.insert(5, Pose3())
values.insert(6, Cal3_S2())
values.insert(7, Cal3DS2())
values.insert(8, Cal3Bundler())
values.insert(9, E)
values.insert(10, imuBias_ConstantBias())
# Special cases for Vectors and Matrices
# Note that gtsam's Eigen Vectors and Matrices requires double-precision
# floating point numbers in column-major (Fortran style) storage order,
# whereas by default, numpy.array is in row-major order and the type is
# in whatever the number input type is, e.g. np.array([1,2,3])
# will have 'int' type.
#
# The wrapper will automatically fix the type and storage order for you,
# but for performance reasons, it's recommended to specify the correct
# type and storage order.
# for vectors, the order is not important, but dtype still is
vec = np.array([1., 2., 3.])
values.insert(11, vec)
mat = np.array([[1., 2.], [3., 4.]], order='F')
values.insert(12, mat)
# Test with dtype int and the default order='C'
# This still works as the wrapper converts to the correct type and order for you
# but is nornally not recommended!
mat2 = np.array([[1, 2, ], [3, 5]])
values.insert(13, mat2)
self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol))
self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol))
self.assertTrue(values.atRot2(2).equals(Rot2(), tol))
self.assertTrue(values.atPose2(3).equals(Pose2(), tol))
self.assertTrue(values.atRot3(4).equals(Rot3(), tol))
self.assertTrue(values.atPose3(5).equals(Pose3(), tol))
self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol))
self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol))
self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol))
self.assertTrue(values.atEssentialMatrix(9).equals(E, tol))
self.assertTrue(values.atimuBias_ConstantBias(
10).equals(imuBias_ConstantBias(), tol))
# special cases for Vector and Matrix:
actualVector = values.atVector(11)
self.assertTrue(np.allclose(vec, actualVector, tol))
actualMatrix = values.atMatrix(12)
self.assertTrue(np.allclose(mat, actualMatrix, tol))
actualMatrix2 = values.atMatrix(13)
self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
def generate_random_input_for_verifier() -> Tuple[Keypoints, Keypoints, np.ndarray, Cal3Bundler, Cal3Bundler]:
"""Generates random inputs for verification.
Returns:
Keypoints for image #i1.
Keypoints for image #i2.
Indices of match between image pair (#i1, #i2).
Intrinsics for image #i1.
Intrinsics for image #i2.
"""
# Randomly generate number of keypoints
num_keypoints_i1 = random.randint(0, 100)
num_keypoints_i2 = random.randint(0, 100)
# randomly generate image shapes
image_shape_i1 = [random.randint(100, 400), random.randint(100, 400)]
image_shape_i2 = [random.randint(100, 400), random.randint(100, 400)]
# generate intrinsics from image shapes
intrinsics_i1 = Cal3Bundler(
fx=min(image_shape_i1[0], image_shape_i1[1]),
k1=0,
k2=0,
u0=image_shape_i1[0] / 2,
v0=image_shape_i1[1] / 2,
)
intrinsics_i2 = Cal3Bundler(
fx=min(image_shape_i2[0], image_shape_i2[1]),
k1=0,
k2=0,
u0=image_shape_i2[0] / 2,
v0=image_shape_i2[1] / 2,
)
# randomly generate the keypoints
keypoints_i1 = generate_random_keypoints(num_keypoints_i1, image_shape_i1)
keypoints_i2 = generate_random_keypoints(num_keypoints_i2, image_shape_i2)
# randomly generate matches
num_matches = random.randint(0, min(num_keypoints_i1, num_keypoints_i2))
if num_matches == 0:
matching_indices_i1i2 = np.array([], dtype=np.int32)
else:
matching_indices_i1i2 = np.empty((num_matches, 2), dtype=np.int32)
matching_indices_i1i2[:, 0] = np.random.choice(num_keypoints_i1, size=(num_matches,), replace=False)
matching_indices_i1i2[:, 1] = np.random.choice(num_keypoints_i2, size=(num_matches,), replace=False)
return (
keypoints_i1,
keypoints_i2,
matching_indices_i1i2,
intrinsics_i1,
intrinsics_i2,
)
def read_cameras_txt(fpath: str) -> Optional[List[Cal3Bundler]]:
"""Read camera calibrations from a COLMAP-formatted cameras.txt file.
Reference: https://colmap.github.io/format.html#cameras-txt
Args:
fpaths: path to cameras.txt file
Returns:
calibration object for each camera, or None if requested file is non-existent
"""
if not Path(fpath).exists():
logger.info("%s does not exist", fpath)
return None
with open(fpath, "r") as f:
lines = f.readlines()
# may not be one line per camera (could be only one line of text if shared calibration)
num_cams = int(lines[2].replace("# Number of cameras: ", "").strip())
calibrations = []
for line in lines[3:]:
cam_params = line.split()
# Note that u0 is px, and v0 is py
model = cam_params[1]
# Currently only handles SIMPLE RADIAL and RADIAL camera models
assert model in ["SIMPLE_RADIAL", "RADIAL"]
if model == "SIMPLE_RADIAL":
_, _, img_w, img_h, fx, u0, v0, k1 = cam_params[:8]
img_w, img_h, fx, u0, v0, k1 = int(img_w), int(img_h), float(
fx), float(u0), float(v0), float(k1)
# Convert COLMAP's SIMPLE_RADIAL to GTSAM's Cal3Bundler:
# Add second radial distortion coefficient of value zero.
k2 = 0
calibrations.append(Cal3Bundler(fx, k1, k2, u0, v0))
elif model == "RADIAL":
_, _, img_w, img_h, fx, u0, v0, k1, k2 = cam_params[:9]
img_w, img_h, fx, u0, v0, k1, k2 = (
int(img_w),
int(img_h),
float(fx),
float(u0),
float(v0),
float(k1),
float(k2),
)
calibrations.append(Cal3Bundler(fx, k1, k2, u0, v0))
assert len(calibrations) == num_cams
return calibrations
def test_two_view_correspondences(self):
"""Tests the bundle adjustment for relative pose on a simulated scene."""
i1Ri2 = EXAMPLE_DATA.get_camera(1).pose().rotation()
i1ti2 = EXAMPLE_DATA.get_camera(1).pose().translation()
i2Ti1 = Pose3(i1Ri2, i1ti2)
camera_i1 = PinholeCameraCal3Bundler(Pose3(), Cal3Bundler())
camera_i2 = PinholeCameraCal3Bundler(i2Ti1, Cal3Bundler())
tracks_3d, valid_indices = TwoViewEstimator.triangulate_two_view_correspondences(
camera_i1, camera_i2, self.keypoints_i1, self.keypoints_i2,
self.corr_idxs)
self.assertEqual(len(tracks_3d), 5)
self.assertEqual(len(valid_indices), 5)
def test_recover_relative_pose_from_essential_matrix(self):
"""Test for function to extract relative pose from essential matrix."""
# simulate correspondences and the essential matrix
corr_i1, corr_i2, i2Ei1 = simulate_two_planes_scene(10, 10)
i2Ri1, i2Ui1 = verification_utils.recover_relative_pose_from_essential_matrix(
i2Ei1.matrix(), corr_i1.coordinates, corr_i2.coordinates,
Cal3Bundler(), Cal3Bundler())
# compare the recovered R and U with the ground truth
self.assertTrue(i2Ri1.equals(i2Ei1.rotation(), 1e-3))
self.assertTrue(i2Ui1.equals(i2Ei1.direction(), 1e-3))
def fundamental_to_essential_matrix(
i2Fi1: np.ndarray, camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler) -> np.ndarray:
"""Converts the fundamental matrix to essential matrix using camera intrinsics.
Args:
i2Fi1: fundamental matrix which maps points in image #i1 to lines in image #i2.
camera_intrinsics_i1: intrinsics for image #i1.
camera_intrinsics_i2: intrinsics for image #i2.
Returns:
Estimated essential matrix i2Ei1 as numpy array of shape (3x3).
"""
return camera_intrinsics_i2.K().T @ i2Fi1 @ camera_intrinsics_i1.K()
def essential_to_fundamental_matrix(
i2Ei1: EssentialMatrix, camera_intrinsics_i1: Cal3Bundler,
camera_intrinsics_i2: Cal3Bundler) -> np.ndarray:
"""Converts the essential matrix to fundamental matrix using camera intrinsics.
Args:
i2Ei1: essential matrix which maps points in image #i1 to lines in image #i2.
camera_intrinsics_i1: intrinsics for image #i1.
camera_intrinsics_i2: intrinsics for image #i2.
Returns:
Fundamental matrix i2Fi1 as numpy array of shape (3x3).
"""
return np.linalg.inv(
camera_intrinsics_i2.K().T) @ i2Ei1.matrix() @ np.linalg.inv(
camera_intrinsics_i1.K())
def test_create_computation_graph(self):
"""Test the dask computation graph."""
sharedCal = Cal3Bundler(1500, 0, 0, 640, 480)
cameras = {
i: PinholeCameraCal3Bundler(x, sharedCal)
for (i, x) in enumerate(self.poses)
}
camera_graph = dask.delayed(cameras)
corr_idxs_graph = {
k: dask.delayed(v)
for (k, v) in self.dummy_corr_idxs_dict.items()
}
keypoints_graph = [dask.delayed(x) for x in self.keypoints_list]
da_graph = self.obj.create_computation_graph(camera_graph,
corr_idxs_graph,
keypoints_graph)
with dask.config.set(scheduler="single-threaded"):
dask_result = dask.compute(da_graph)[0]
self.assertIsInstance(dask_result, SfmData)
def test_round_trip_images_txt(self) -> None:
"""Starts with a pose. Writes the pose to images.txt (in a temporary directory). Then reads images.txt to recover
that same pose. Checks if the original wTc and recovered wTc match up."""
# fmt: off
# Rotation 45 degrees about the z-axis.
original_wRc = np.array([[np.cos(np.pi / 4), -np.sin(np.pi / 4), 0],
[np.sin(np.pi / 4),
np.cos(np.pi / 4), 0], [0, 0, 1]])
original_wtc = np.array([3, -2, 1])
# fmt: on
# Setup dummy GtsfmData Object with one image
original_wTc = Pose3(Rot3(original_wRc), original_wtc)
default_intrinsics = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0)
camera = PinholeCameraCal3Bundler(original_wTc, default_intrinsics)
gtsfm_data = GtsfmData(number_images=1)
gtsfm_data.add_camera(0, camera)
image = Image(value_array=None, file_name="dummy_image.jpg")
images = [image]
# Perform write and read operations inside a temporary directory
with tempfile.TemporaryDirectory() as tempdir:
images_fpath = os.path.join(tempdir, "images.txt")
io_utils.write_images(gtsfm_data, images, tempdir)
wTi_list, _ = io_utils.read_images_txt(images_fpath)
recovered_wTc = wTi_list[0]
npt.assert_almost_equal(original_wTc.matrix(),
recovered_wTc.matrix(),
decimal=3)
def simulate_two_planes_scene(M: int, N: int) -> Tuple[Keypoints, Keypoints, EssentialMatrix]:
"""Generate a scene where 3D points are on two planes, and projects the points to the 2 cameras. There are M points
on plane 1, and N points on plane 2.
The two planes in this test are:
1. -10x -y -20z +150 = 0
2. 15x -2y -35z +200 = 0
Args:
M: number of points on 1st plane.
N: number of points on 2nd plane.
Returns:
keypoints for image i1, of length (M+N).
keypoints for image i2, of length (M+N).
Essential matrix i2Ei1.
"""
# range of 3D points
range_x_coordinate = (-5, 7)
range_y_coordinate = (-10, 10)
# define the plane equation
plane1_coeffs = (-10, -1, -20, 150)
plane2_coeffs = (15, -2, -35, 200)
# sample the points from planes
plane1_points = sample_points_on_plane(plane1_coeffs, range_x_coordinate, range_y_coordinate, M)
plane2_points = sample_points_on_plane(plane2_coeffs, range_x_coordinate, range_y_coordinate, N)
points_3d = np.vstack((plane1_points, plane2_points))
# define the camera poses and compute the essential matrix
wti1 = np.array([0.1, 0, -20])
wti2 = np.array([1, -2, -20.4])
wRi1 = Rot3.RzRyRx(np.pi / 20, 0, 0.0)
wRi2 = Rot3.RzRyRx(0.0, np.pi / 6, 0.0)
wTi1 = Pose3(wRi1, wti1)
wTi2 = Pose3(wRi2, wti2)
i2Ti1 = wTi2.between(wTi1)
i2Ei1 = EssentialMatrix(i2Ti1.rotation(), Unit3(i2Ti1.translation()))
# project 3D points to 2D image measurements
intrinsics = Cal3Bundler()
camera_i1 = PinholeCameraCal3Bundler(wTi1, intrinsics)
camera_i2 = PinholeCameraCal3Bundler(wTi2, intrinsics)
uv_im1 = []
uv_im2 = []
for point in points_3d:
uv_im1.append(camera_i1.project(point))
uv_im2.append(camera_i2.project(point))
uv_im1 = np.vstack(uv_im1)
uv_im2 = np.vstack(uv_im2)
# return the points as keypoints and the essential matrix
return Keypoints(coordinates=uv_im1), Keypoints(coordinates=uv_im2), i2Ei1
def test_compute_keypoint_intersections(self) -> None:
"""Tests `compute_keypoint_intersections()` function."""
# Create cube mesh with side length one centered at origin.
box = trimesh.primitives.Box()
# Create arrangement of 4 cameras in x-z plane pointing at the cube.
fx, k1, k2, u0, v0 = 10, 0, 0, 1, 1
calibration = Cal3Bundler(fx, k1, k2, u0, v0)
cam_pos = [[2, 0, 0], [-2, 0, 0], [0, 0, 2], [0, 0, -2]]
target_pos = [0, 0, 0]
up_vector = [0, -1, 0]
cams = [
PinholeCameraCal3Bundler().Lookat(c, target_pos, up_vector,
calibration) for c in cam_pos
]
# Project keypoint at center of each simulated image and record intersection.
kpt = Keypoints(coordinates=np.array([[1, 1]]).astype(np.float32))
expected_intersections = [[0.5, 0, 0], [-0.5, 0, 0], [0, 0, 0.5],
[0, 0, -0.5]]
estimated_intersections = []
for cam in cams:
_, intersection = metric_utils.compute_keypoint_intersections(
kpt, cam, box, verbose=True)
estimated_intersections.append(intersection.flatten().tolist())
np.testing.assert_allclose(expected_intersections,
estimated_intersections)
def get_camera_intrinsics(self, index: int) -> Optional[Cal3Bundler]:
"""Get the camera intrinsics at the given index.
Args:
the index to fetch.
Returns:
intrinsics for the given camera.
"""
if len(self.explicit_intrinsics_paths) == 0:
# get intrinsics from exif
return io_utils.load_image(
self.image_paths[index]).get_intrinsics_from_exif()
else:
# TODO: handle extra inputs in the intrinsics array
intrinsics_array = np.load(self.explicit_intrinsics_paths[index])
return Cal3Bundler(
fx=min(intrinsics_array[0, 0], intrinsics_array[1, 1]),
k1=0,
k2=0,
u0=intrinsics_array[0, 2],
v0=intrinsics_array[1, 2],
)
def __read_calibrations(self) -> List[PinholeCameraCal3Bundler]:
"""Read camera params from the calibration stored as h5 files.
Returns:
list of all cameras.
"""
file_path_template = osp.join(self._folder, "calibration",
"calibration_{}.h5")
pose_list = []
for image_name in self._image_names:
file_path = file_path_template.format(image_name)
calib_data = io_utils.load_h5(file_path)
cTw = Pose3(Rot3(calib_data["R"]), calib_data["T"])
K_matrix = calib_data["K"]
# TODO: fix different fx and fy (and underparameterization of K)
K = Cal3Bundler(
fx=float(K_matrix[0, 0] + K_matrix[1, 1]) * 0.5,
k1=0.0,
k2=0.0,
u0=float(K_matrix[0, 2]),
v0=float(K_matrix[1, 2]),
)
pose_list.append(PinholeCameraCal3Bundler(cTw.inverse(), K))
return pose_list
def test_compute_track_reprojection_errors():
"""Ensure that reprojection error is computed properly within a track.
# For camera 0:
# [13] = [10,0,3] [1,0,0 | 0] [1]
# [24] = [0,10,4] * [0,1,0 | 0] *[2]
# [1] = [0, 0,1] [0,0,1 | 0] [1]
# [1]
# For camera 1:
# [-7] = [10,0,3] [1,0,0 |-2] [1]
# [44] = [0,10,4] * [0,1,0 | 2] *[2]
# [1] = [0, 0,1] [0,0,1 | 0] [1]
# [1]
"""
wTi0 = Pose3(Rot3.RzRyRx(0, 0, 0), np.zeros((3, 1)))
wTi1 = Pose3(Rot3.RzRyRx(0, 0, 0), np.array([2, -2, 0]))
f = 10
k1 = 0
k2 = 0
u0 = 3
v0 = 4
K0 = Cal3Bundler(f, k1, k2, u0, v0)
K1 = Cal3Bundler(f, k1, k2, u0, v0)
track_camera_dict = {
0: PinholeCameraCal3Bundler(wTi0, K0),
1: PinholeCameraCal3Bundler(wTi1, K1)
}
triangulated_pt = np.array([1, 2, 1])
track_3d = SfmTrack(triangulated_pt)
# in camera 0
track_3d.addMeasurement(idx=0, m=np.array([13, 24]))
# in camera 1
track_3d.addMeasurement(idx=1, m=np.array(
[-8, 43])) # should be (-7,44), 1 px error in each dim
errors, avg_track_reproj_error = reproj_utils.compute_track_reprojection_errors(
track_camera_dict, track_3d)
expected_errors = np.array([0, np.sqrt(2)])
np.testing.assert_allclose(errors, expected_errors)
assert avg_track_reproj_error == np.sqrt(2) / 2
def test_filter_to_cycle_consistent_edges() -> None:
"""Ensure correct edges are kept in a 2-triplet scenario.
Scenario Ground Truth: consider 5 camera poses in a line, connected as follows, all with identity rotations:
Spatial layout:
_________ ________
/ \\ / \
i4 -- i3 -- i2 -- i1 -- i0
Topological layout:
i4 i0
i2
i3 i1
In the measurements, suppose, the measurement for (i2,i4) was corrupted by 15 degrees.
"""
i2Ri1_dict = {
(0, 1): Rot3(),
(1, 2): Rot3(),
(0, 2): Rot3(),
(2, 3): Rot3(),
(3, 4): Rot3(),
(2, 4): Rot3.Ry(np.deg2rad(15)),
}
i2Ui1_dict = {
(0, 1): Unit3(np.array([1, 0, 0])),
(1, 2): Unit3(np.array([1, 0, 0])),
(0, 2): Unit3(np.array([1, 0, 0])),
(2, 3): Unit3(np.array([1, 0, 0])),
(3, 4): Unit3(np.array([1, 0, 0])),
(2, 4): Unit3(np.array([1, 0, 0])),
}
# The ViewGraphEstimator assumes these dicts contain the keys corresponding to the the rotation edges.
calibrations = {k: Cal3Bundler() for k in range(0, 5)}
corr_idxs_i1i2 = {i1i2: np.array([]) for i1i2 in i2Ri1_dict.keys()}
keypoints = {k: np.array([]) for k in range(0, 5)}
# populate dummy 2-view reports
two_view_reports = {}
for (i1, i2) in i2Ri1_dict.keys():
two_view_reports[(i1, i2)] = TwoViewEstimationReport(
v_corr_idxs=np.array([]), # dummy array
num_inliers_est_model=10, # dummy value
)
rcc_estimator = CycleConsistentRotationViewGraphEstimator(
EdgeErrorAggregationCriterion.MEDIAN_EDGE_ERROR)
viewgraph_edges = rcc_estimator.run(i2Ri1_dict=i2Ri1_dict,
i2Ui1_dict=i2Ui1_dict,
calibrations=calibrations,
corr_idxs_i1i2=corr_idxs_i1i2,
keypoints=keypoints,
two_view_reports=two_view_reports)
# non-self-consistent triplet should have been removed
expected_edges = {(0, 1), (1, 2), (0, 2)}
assert set(viewgraph_edges) == expected_edges
def test_get_camera_intrinsics_explicit(self):
"""Tests getter for intrinsics when explicit numpy arrays with intrinsics are present on disk."""
computed = self.loader.get_camera_intrinsics(5)
expected = Cal3Bundler(fx=2378.983, k1=0, k2=0, u0=968.0, v0=648.0)
self.assertTrue(expected.equals(computed, 1e-3))
def test_recover_relative_pose_from_essential_matrix_none(self):
"""Test for function to extract relative pose from essential matrix."""
# simulate correspondences and the essential matrix
corr_i1, corr_i2, _ = simulate_two_planes_scene(10, 10)
i2Ri1, i2Ui1 = verification_utils.recover_relative_pose_from_essential_matrix(
i2Ei1=None,
verified_coordinates_i1=corr_i1.coordinates,
verified_coordinates_i2=corr_i2.coordinates,
camera_intrinsics_i1=Cal3Bundler(),
camera_intrinsics_i2=Cal3Bundler(),
)
# compare the recovered R and U with the ground truth
self.assertIsNone(i2Ri1)
self.assertIsNone(i2Ui1)
def test_get_camera_intrinsics_exif(self):
"""Tests getter for intrinsics when explicit numpy arrays are absent and we fall back on exif."""
loader = OlssonLoader(EXIF_FOLDER,
image_extension="JPG",
use_gt_intrinsics=False)
computed = loader.get_camera_intrinsics(5)
expected = Cal3Bundler(fx=2378.983, k1=0, k2=0, u0=648.0, v0=968.0)
self.assertTrue(expected.equals(computed, 1e-3))
def test_verify_empty_matches(self):
"""Tests the output when there are no match indices."""
keypoints_i1 = feature_utils.generate_random_keypoints(10, [250, 300])
keypoints_i2 = feature_utils.generate_random_keypoints(12, [400, 300])
match_indices = np.array([], dtype=np.int32)
intrinsics_i1 = Cal3Bundler()
intrinsics_i2 = Cal3Bundler()
self.__execute_test(
keypoints_i1,
keypoints_i2,
match_indices,
intrinsics_i1,
intrinsics_i2,
i2Ri1_expected=None,
i2Ui1_expected=None,
verified_indices_expected=np.array([], dtype=np.uint32),
)
def test_normalize_coordinates(self):
coordinates = np.array([[10.0, 20.0], [25.0, 12.0], [30.0, 33.0]])
intrinsics = Cal3Bundler(fx=100, k1=0.0, k2=0.0, u0=20.0, v0=30.0)
normalized_coordinates = feature_utils.normalize_coordinates(coordinates, intrinsics)
expected_coordinates = np.array([[-0.1, -0.1], [0.05, -0.18], [0.1, 0.03]])
np.testing.assert_allclose(normalized_coordinates, expected_coordinates)
def test_get_camera_intrinsics_explicit(self):
"""Tests getter for intrinsics when explicit data.mat file with intrinsics are present on disk."""
expected_fx = 1392.10693
expected_px = 980.175985
expected_py = 604.353418
computed = self.loader.get_camera_intrinsics(0)
expected = Cal3Bundler(fx=expected_fx, k1=0, k2=0, u0=expected_px, v0=expected_py)
self.assertTrue(expected.equals(computed, 1e-3))
def test_5pt_algo_5correspondences(self) -> None:
""" """
fx, px, py, k1, k2 = load_log_front_center_intrinsics()
keypoints_i1, keypoints_i2 = load_argoverse_log_annotated_correspondences(
)
# match keypoints row by row
match_indices = np.vstack(
[np.arange(len(keypoints_i1)),
np.arange(len(keypoints_i1))]).T
intrinsics_i1 = Cal3Bundler(fx, k1, k2, px, py)
intrinsics_i2 = Cal3Bundler(fx, k1, k2, px, py)
match_indices = match_indices[:5]
i2Ri1, i2ti1, _, _ = self.verifier.verify(keypoints_i1, keypoints_i2,
match_indices, intrinsics_i1,
intrinsics_i2)
def setUp(self):
"""Set up the data association module."""
super().setUp()
# landmark ~5 meters infront of camera
self.expected_landmark = Point3(5, 0.5, 1.2)
# shared calibration
f, k1, k2, u0, v0 = 1500, 0, 0, 640, 480
self.sharedCal = Cal3Bundler(f, k1, k2, u0, v0)
def test_round_trip_cameras_txt(self) -> None:
"""Creates a two cameras and writes to cameras.txt (in a temporary directory). Then reads cameras.txt to recover
the information. Checks if the original and recovered cameras match up."""
# Create multiple calibration data
k1 = Cal3Bundler(fx=100, k1=0, k2=0, u0=0, v0=0)
k2 = Cal3Bundler(fx=200, k1=0.001, k2=0, u0=1000, v0=2000)
k3 = Cal3Bundler(fx=300, k1=0.004, k2=0.001, u0=1001, v0=2002)
original_calibrations = [k1, k2, k3]
gtsfm_data = GtsfmData(number_images=len(original_calibrations))
# Populate gtsfm_data with the generated vales
for i in range(len(original_calibrations)):
camera = PinholeCameraCal3Bundler(Pose3(),
original_calibrations[i])
gtsfm_data.add_camera(i, camera)
# Generate dummy images
image = Image(value_array=np.zeros((240, 320)),
file_name="dummy_image.jpg")
images = [image for i in range(len(original_calibrations))]
# Round trip
with tempfile.TemporaryDirectory() as tempdir:
cameras_fpath = os.path.join(tempdir, "cameras.txt")
io_utils.write_cameras(gtsfm_data, images, tempdir)
recovered_calibrations = io_utils.read_cameras_txt(cameras_fpath)
self.assertEqual(len(original_calibrations),
len(recovered_calibrations))
for i in range(len(recovered_calibrations)):
K_ori = original_calibrations[i]
K_rec = recovered_calibrations[i]
self.assertEqual(K_ori.fx(), K_rec.fx())
self.assertEqual(K_ori.px(), K_rec.px())
self.assertEqual(K_ori.py(), K_rec.py())
self.assertEqual(K_ori.k1(), K_rec.k1())
self.assertEqual(K_ori.k2(), K_rec.k2())
def test_pinholeCameraCal3Bundler_roundtrip(self):
"""Test the round-trip on Unit3 object."""
expected = PinholeCameraCal3Bundler(
Pose3(Rot3.RzRyRx(0, 0.1, -0.05), np.random.randn(3, 1)),
Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70),
)
header, frames = serialize(expected)
recovered = deserialize(header, frames)
self.assertTrue(expected.equals(recovered, 1e-5))
def test_verify_empty_matches(self):
"""Tests the output when there are no match indices."""
keypoints_i1 = generate_random_keypoints(10, [250, 300])
keypoints_i2 = generate_random_keypoints(12, [400, 300])
match_indices = np.array([], dtype=np.int32)
intrinsics_i1 = Cal3Bundler()
intrinsics_i2 = Cal3Bundler()
(i2Ri1, i2Ui1, verified_indices,) = self.verifier.verify_with_exact_intrinsics(
keypoints_i1,
keypoints_i2,
match_indices,
intrinsics_i1,
intrinsics_i2,
)
self.assertIsNone(i2Ri1)
self.assertIsNone(i2Ui1)
self.assertEqual(0, verified_indices.size)
