def test_sim3_pose(self):
"""Test generating similarity transform with Pose3 pairs."""
# Create expected sim3
expected_R = Rot3.Ry(math.radians(180))
expected_s = 2
expected_t = Point3(4, 6, 10)
print(expected_R)
# Create source poses
s_pose1 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])),
Point3(0, 0, 0))
s_pose2 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, 1]])),
Point3(4, 0, 0))
# Create destination poses
d_pose1 = Pose3(Rot3(np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])),
Point3(4, 6, 10))
d_pose2 = Pose3(Rot3(np.array([[1, 0, 0], [0, 1, 0], [0, 0, -1]])),
Point3(-4, 6, 10))
# Align
pose_pairs = [[s_pose1, d_pose1], [s_pose2, d_pose2]]
sim3 = Similarity3()
sim3.sim3_pose(pose_pairs)
# Check actual sim3 equals to expected sim3
assert (expected_R.equals(sim3._R, 1e-6))
self.assertAlmostEqual(sim3._s, expected_s, delta=1e-6)
self.assert_gtsam_equals(sim3._t, expected_t)
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_compute_cyclic_rotation_error() -> None:
"""Ensure cycle error is computed correctly within a triplet.
Imagine 3 poses, all centered at the origin, at different orientations.
Ground truth poses:
Let i0 face along +x axis (0 degrees in yaw)
Let i2 have a 30 degree rotation from the +x axis.
Let i4 have a 90 degree rotation from the +x axis.
However, suppose one edge measurement is corrupted (from i0 -> i4) by 5 degrees.
"""
i2Ri0 = Rot3.Ry(np.deg2rad(30))
i4Ri2 = Rot3.Ry(np.deg2rad(60))
i4Ri0 = Rot3.Ry(np.deg2rad(95))
cycle_error = geometry_comparisons.compute_cyclic_rotation_error(
i2Ri0, i4Ri2, i4Ri0)
assert np.isclose(cycle_error, 5)
def test_map_transform(self):
"""Test similarity transform on a map."""
expected_poses, expected_points = self.d_map
sim3 = Similarity3(Rot3.Ry(math.radians(180)), Point3(4, 6, 10), 2)
actual_poses, actual_points = sim3.map_transform(self.s_map)
for i, point in enumerate(actual_points):
self.assert_gtsam_equals(expected_points[i], point)
for i, pose in enumerate(actual_poses):
self.assert_gtsam_equals(expected_poses[i], pose)
def test_transform(self):
"""Test similarity transform on poses and points."""
expected_poses, expected_points = self.d_map
sim3 = Similarity3(Rot3.Ry(math.radians(180)), Point3(4, 6, 10), 2)
poses, points = self.s_map
for i, point in enumerate(points):
point = sim3.point(point)
self.assert_gtsam_equals(expected_points[i], point)
for i, pose in enumerate(poses):
pose = sim3.pose(pose)
self.assert_gtsam_equals(expected_poses[i], pose)
