def compute_loop_distances_and_angles(current_pose: tf.Transform, linked_poses: typing.Iterable[tf.Transform]) -> \
typing.Tuple[typing.List[float], typing.List[float]]:
relative_poses = [
current_pose.find_relative(linked_pose) for linked_pose in linked_poses
]
return [
np.linalg.norm(relative_pose.location)
for relative_pose in relative_poses
], [
tf.quat_angle(relative_pose.rotation_quat(w_first=True))
for relative_pose in relative_poses
]
def test_interpolates_pose(self):
time_offset = 0.332
times = [time + time_offset for time in range(0, 10)]
builder = TrajectoryBuilder(times)
for time in range(
0, 11
): # includes 10, to make sure we get either end of the range
# Create sample points at the given times
builder.add_trajectory_point(
time,
Transform(location=(10 * time, -time, 0),
rotation=tf3d.quaternions.axangle2quat(
(4, -3, 2), time * np.pi / 20),
w_first=True))
result = builder.get_interpolated_trajectory()
self.assertEqual(set(times), set(result.keys()))
first_pose = Transform(location=(10 * time_offset, -time_offset, 0),
rotation=tf3d.quaternions.axangle2quat(
(4, -3, 2), time_offset * np.pi / 20),
w_first=True)
for time in times:
# This is the true pose at that time, relative to the true pose at the first time.
# need to do it this way, because of the rotation, which shifts things
# the build has never seen this transform, the timestamps are different
expected_pose = first_pose.find_relative(
Transform(location=(10 * time, -time, 0),
rotation=tf3d.quaternions.axangle2quat(
(4, -3, 2), time * np.pi / 20),
w_first=True))
interpolated_pose = result[time]
self.assertNPClose(expected_pose.location,
interpolated_pose.location,
atol=1e-13,
rtol=0)
self.assertNPClose(expected_pose.rotation_quat(True),
interpolated_pose.rotation_quat(True),
atol=1e-14,
rtol=0)
def test_interpolates_pose_over_long_range(self):
builder = TrajectoryBuilder(range(1, 10))
# Only add the start and end times, rely on the LERP for all other poses
builder.add_trajectory_point(0, Transform())
builder.add_trajectory_point(
10,
Transform( # the pose at time 10
location=(10 * 10, -10, 0),
rotation=tf3d.quaternions.axangle2quat((4, -3, 2),
10 * np.pi / 20),
w_first=True))
result = builder.get_interpolated_trajectory()
self.assertEqual(set(range(1, 10)), set(result.keys()))
first_pose = Transform(location=(10 * 1, -1, 0),
rotation=tf3d.quaternions.axangle2quat(
(4, -3, 2), 1 * np.pi / 20),
w_first=True)
for time in range(1, 10):
# This is the true pose at that time, relative to the true pose at the first time.
# need to do it this way, because of the rotation, which shifts things
# the build has never seen this transform, the timestamps are different
expected_pose = first_pose.find_relative(
Transform(location=(10 * time, -time, 0),
rotation=tf3d.quaternions.axangle2quat(
(4, -3, 2), time * np.pi / 20),
w_first=True))
interpolated_pose = result[time]
self.assertNPClose(expected_pose.location,
interpolated_pose.location,
atol=1e-13,
rtol=0)
self.assertNPClose(expected_pose.rotation_quat(True),
interpolated_pose.rotation_quat(True),
atol=1e-14,
rtol=0)
def rectify(left_extrinsics: tf.Transform, left_intrinsics: CameraIntrinsics,
right_extrinsics: tf.Transform, right_intrinsics: CameraIntrinsics) -> \
typing.Tuple[np.ndarray, np.ndarray, CameraIntrinsics,
np.ndarray, np.ndarray, CameraIntrinsics]:
"""
Compute mapping matrices for performing stereo rectification, from the camera properties.
Applying the returned transformation to the images using cv2.remap gives us undistorted stereo rectified images
:param left_extrinsics:
:param left_intrinsics:
:param right_extrinsics:
:param right_intrinsics:
:return: 4 remapping matrices: left x, left y, right x, right y
"""
shape = (left_intrinsics.width, left_intrinsics.height)
left_distortion = np.array([
left_intrinsics.k1, left_intrinsics.k2, left_intrinsics.p1,
left_intrinsics.p2, left_intrinsics.k3
])
right_distortion = np.array([
right_intrinsics.k1, right_intrinsics.k2, right_intrinsics.p1,
right_intrinsics.p2, right_intrinsics.k3
])
# We want the transform from the right to the left, which is the position of the left relative to the right
relative_transform = right_extrinsics.find_relative(left_extrinsics)
r_left = np.zeros((3, 3))
r_right = np.zeros((3, 3))
p_left = np.zeros((3, 4))
p_right = np.zeros((3, 4))
cv2.stereoRectify(cameraMatrix1=left_intrinsics.intrinsic_matrix(),
distCoeffs1=left_distortion,
cameraMatrix2=right_intrinsics.intrinsic_matrix(),
distCoeffs2=right_distortion,
imageSize=shape,
R=relative_transform.rotation_matrix,
T=relative_transform.location,
alpha=0,
flags=cv2.CALIB_ZERO_DISPARITY,
newImageSize=shape,
R1=r_left,
R2=r_right,
P1=p_left,
P2=p_right)
m1l, m2l = cv2.initUndistortRectifyMap(left_intrinsics.intrinsic_matrix(),
left_distortion, r_left,
p_left[0:3, 0:3], shape, cv2.CV_32F)
m1r, m2r = cv2.initUndistortRectifyMap(right_intrinsics.intrinsic_matrix(),
right_distortion, r_right,
p_right[0:3,
0:3], shape, cv2.CV_32F)
# Rectification has changed the camera intrinsics, return the new ones
rectified_left_intrinsics = CameraIntrinsics(width=shape[0],
height=shape[1],
fx=p_left[0, 0],
fy=p_left[1, 1],
cx=p_left[0, 2],
cy=p_left[1, 2],
s=p_left[0, 1])
rectified_right_intrinsics = CameraIntrinsics(width=shape[0],
height=shape[1],
fx=p_right[0, 0],
fy=p_right[1, 1],
cx=p_right[0, 2],
cy=p_right[1, 2],
s=p_right[0, 1])
return m1l, m2l, rectified_left_intrinsics, m1r, m2r, rectified_right_intrinsics