def test_composition_with_identity_works():
so3 = SO3.from_angle_axis(np.pi / 4, np.array([[0, 1, 0]]).T)
comp_with_identity = so3.compose(SO3())
comp_from_identity = SO3().compose(so3)
np.testing.assert_almost_equal(comp_with_identity.matrix, so3.matrix, 14)
np.testing.assert_almost_equal(comp_from_identity.matrix, so3.matrix, 14)
def test_construct_with_matrix():
R = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]])
so3 = SO3(R)
np.testing.assert_array_equal(so3.matrix, R)
# Matrices not elements of SO(3) should be fitted to SO(3)
R_noisy = R + 0.1 + np.random.rand(3)
so3_fitted = SO3(R_noisy)
R_fitted = so3_fitted.matrix
assert np.any(np.not_equal(R_fitted, R_noisy))
np.testing.assert_almost_equal(np.linalg.det(R_fitted), 1, 14)
np.testing.assert_almost_equal((R_fitted.T @ R_fitted), np.identity(3), 14)
def test_construct_with_tuple():
so3 = SO3() + np.array([[0.1, -0.3, 0.2]]).T
t = np.array([[1, 2, 3]]).T
se3 = SE3((so3, t))
np.testing.assert_equal(se3.rotation.matrix, so3.matrix)
np.testing.assert_equal(se3.translation, t)
def test_difference_for_simple_rotation_with_operator_works():
X = SO3()
theta_vec = 3 * np.pi / 4 * np.array([[1, 0, -1]]).T / np.sqrt(2)
Y = SO3.Exp(theta_vec)
theta_vec_diff = Y - X
np.testing.assert_almost_equal(theta_vec_diff, theta_vec, 14)
def test_difference_for_simple_rotation_works():
X = SO3()
theta_vec = np.pi / 3 * np.array([[0, 1, 0]]).T
Y = SO3.Exp(theta_vec)
theta_vec_diff = Y.ominus(X)
np.testing.assert_array_equal(theta_vec_diff, theta_vec)
def test_to_rotation_matrix_results_in_close_rotation():
angle = 0.5 * np.pi
axis = np.array([[1, 0, 0]]).T
R = SO3.Exp(angle * axis).matrix
# Invalidate a valid rotation matrix by scaling it.
R_scaled = 3 * R
# Fit to SO(3).
R_closest = SO3.to_so3_matrix(R_scaled)
# Result should be the same rotation matrix.
np.testing.assert_almost_equal(R_closest, R, 14)
# Perturb the rotation matrix with random noise.
R_noisy = R + 0.01 * np.random.rand(3, 3)
# Fit to SO(3)
so3_closest = SO3(R_noisy)
# Extract angle-axis representation.
angle_closest, axis_closest = so3_closest.Log(True)
# Result should be close to the same rotation.
np.testing.assert_almost_equal(angle_closest, angle, 2)
np.testing.assert_almost_equal(axis_closest, axis, 2)
def __init__(self, pose_tuple=(SO3(), np.zeros((3, 1)))):
"""Constructs an SE(3) element.
The default is the identity element.
:param pose_tuple: A tuple (rotation (SO3), translation (3D column vector) (optional).
"""
self.rotation, self.translation = pose_tuple
def from_matrix(cls, T):
"""Construct an SE(3) element corresponding from a pose matrix.
The rotation is fitted to the closest rotation matrix, the bottom row of the 4x4 matrix is ignored.
:param T: 4x4 or 3x4 pose matrix.
:return: The SE(3) element.
"""
return cls((SO3(T[:3, :3]), T[:3, 3:4]))
def looks_at_pose(camera_pos_w: np.ndarray, target_pos_w: np.ndarray, up_vector_w: np.ndarray):
cam_to_target_w = target_pos_w - camera_pos_w
cam_z_w = cam_to_target_w.flatten() / np.linalg.norm(cam_to_target_w)
cam_to_right_w = np.cross(-up_vector_w.flatten(), cam_z_w)
cam_x_w = cam_to_right_w / np.linalg.norm(cam_to_target_w)
cam_y_w = np.cross(cam_z_w, cam_x_w)
return SE3((SO3(np.vstack((cam_x_w, cam_y_w, cam_z_w)).T), camera_pos_w))
def test_log_with_no_rotation():
X = SE3((SO3(), np.array([[-1, 2, -3]]).T))
xi_vec = X.Log()
# Translation part:
np.testing.assert_equal(xi_vec[:3], X.translation)
# Rotation part:
np.testing.assert_equal(xi_vec[3:], np.zeros((3, 1)))
def estimate_two_view_relative_pose(frame0: MatchedFrame, kp_0: np.ndarray,
frame1: MatchedFrame, kp_1: np.ndarray):
num_matches = kp_0.shape[1]
if num_matches < 8:
return None
# Compute fundamental matrix from matches.
F_0_1, _ = cv2.findFundamentalMat(kp_1.T, kp_0.T, cv2.FM_8POINT)
# Extract the calibration matrices.
K_0 = frame0.camera_model().calibration_matrix()
K_1 = frame1.camera_model().calibration_matrix()
# Compute the essential matrix from the fundamental matrix.
E_0_1 = K_0.T @ F_0_1 @ K_1
# Compute the relative pose.
# Transform detections to normalized image plane (since cv2.recoverPose() only supports common K)
kp_n_0 = frame0.camera_model().pixel_to_normalised(kp_0)
kp_n_1 = frame1.camera_model().pixel_to_normalised(kp_1)
K_n = np.identity(3)
_, R_0_1, t_0_1, _ = cv2.recoverPose(E_0_1, kp_n_1.T, kp_n_0.T, K_n)
return SE3((SO3(R_0_1), t_0_1))
# Plot the body frame (a body-fixed Forward-Right-Down (FRD) frame).
roll = np.radians(-10)
pitch = np.radians(0)
yaw = np.radians(135)
t_w_b = np.array([[-10, -10, -2]]).T
T_w_b = SE3((SO3.from_roll_pitch_yaw(roll, pitch, yaw), t_w_b))
vg.plot_pose(ax, T_w_b.to_tuple(), scale=3, text='$\mathcal{F}_b$')
# Plot the camera frame.
# The camera is placed 2 m directly above the body origin.
# Its optical axis points to the left (in opposite direction of the y-axis in F_b).
# Its y-axis points downwards along the z-axis of F_b.
R_b_c = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
t_b_c = np.array([[0, 0, -2]]).T
T_b_c = SE3((SO3(R_b_c), t_b_c))
T_w_c = T_w_b @ T_b_c
vg.plot_pose(ax, T_w_c.to_tuple(), scale=3, text='$\mathcal{F}_c$')
# Plot obstacle frame.
# The cube is placed at (North: 10 m, East: 10 m, Down: -1 m).
# Its top points upwards, and its front points south.
R_w_o = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])
t_w_o = np.array([[10, 10, -1]]).T
T_w_o = SE3((SO3(R_w_o), t_w_o))
vg.plot_pose(ax, T_w_o.to_tuple(), scale=3, text='$\mathcal{F}_o$')
# Plot the cube with sides 3 meters.
points_o = vg.utils.generate_box(scale=3)
points_w = T_w_o * points_o
vg.utils.plot_as_box(ax, points_w)
def test_adjoint_returns_rotation_matrix():
so3_1 = SO3()
so3_2 = SO3.from_angle_axis(3 * np.pi / 4, np.array([[1, 0, 0]]).T)
np.testing.assert_array_equal(so3_1.adjoint(), so3_1.matrix)
np.testing.assert_array_equal(so3_2.adjoint(), so3_2.matrix)
def test_construct_identity_with_no_args():
so3 = SO3()
np.testing.assert_array_equal(so3.matrix, np.identity(3))
def test_has_len_that_returns_correct_dimension():
X = SO3()
# Should have 6 DOF.
np.testing.assert_equal(len(X), 3)
def read_bundler_file(filename,
img_paths,
shortest_track_length=3,
principal_point=np.array([[702, 468]]).T):
# We will use a camera coordinate system where z points forward, y down.
pose_bundlerc_c = SE3((SO3(np.array([[1, 0, 0], [0, -1, 0],
[0, 0, -1]])), np.zeros([3, 1])))
matched_frames = []
sfm_map = SfmMap()
with open(filename, 'r') as file:
line = file.readline()
if not line.startswith("# Bundle file v0.3"):
warnings.warn(
"Warning: Expected v0.3 Bundle file, but first line did not match the convention"
)
# Read number of cameras and points.
num_cameras, num_points = [int(x) for x in next(file).split()]
# Initialize list of matched frames.
matched_frames = num_cameras * [None]
# Read all camera parameters.
for i in range(num_cameras):
# Read the intrinsics from the first line.
f, k1, k2 = [float(x) for x in next(file).split()]
# We will assume that the data has been undistorted (for simplicity).
if k1 != 0 or k2 != 0:
warnings.warn(
"The current implementation assumes undistorted data. Distortion parameters are ignored"
)
# Read the rotation matrix from the next three lines.
R = np.array([[float(x) for x in next(file).split()]
for y in range(3)]).reshape([3, 3])
# Read the translation from the last line.
t = np.array([float(x)
for x in next(file).split()]).reshape([3, 1])
# Add matched frame to list.
f = f * np.ones([2, 1])
matched_frames[i] = MatchedFrame(
i, PerspectiveCamera(f, principal_point), img_paths[i])
sfm_map.add_keyframe(
Keyframe(matched_frames[i],
SE3((SO3(R), t)).inverse() @ pose_bundlerc_c))
# Read the points and matches.
for i in range(num_points):
# First line is the position of the point.
p_w = np.array([float(x)
for x in next(file).split()]).reshape([3, 1])
# Next line is the color.
color = np.array([int(x)
for x in next(file).split()]).reshape([3, 1])
# The last line is the correspondences in each image where the point is observed.
view_list = next(file).split()
num_observed = int(view_list[0])
# Drop observation if seen from less than shortest_track_length cameras.
if num_observed < shortest_track_length:
continue
# Add observations and detections for each camera the point is observed in.
curr_track = FeatureTrack()
curr_map_point = MapPoint(i, p_w)
for c in range(num_observed):
view_data = view_list[1 + 4 * c:1 + 4 * (c + 1)]
cam_ind = int(view_data[0])
det_id = int(view_data[1])
# We have to invert the y-values because we have chosen to let y point downwards.
det_point = np.array([[
float(view_data[2]), -float(view_data[3])
]]).T + principal_point
curr_track.add_observation(matched_frames[cam_ind], det_id)
matched_frames[cam_ind].add_keypoint(
det_id, KeyPoint(det_point, color, curr_track))
curr_map_point.add_observation(sfm_map.get_keyframe(cam_ind),
det_id)
sfm_map.add_map_point(curr_map_point)
for frame in matched_frames:
frame.update_covisible_frames()
return matched_frames, sfm_map
def main():
# Define camera parameters.
f = np.array([[100, 100]]).T # "Focal lengths" [fu, fv].T
c = np.array([[50, 50]]).T # Principal point [cu, cv].T
# Define camera pose distribution.
R_w_c = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]])
t_w_c = np.array([[1, 0, 0]]).T
mean_T_w_c = SE3((SO3(R_w_c), t_w_c))
Sigma_T_w_c = np.diag(np.array([0.1, 0.1, 0.1, 0.01, 0.01, 0.01])**2)
# Define pixel position distribution.
mean_u = np.array([[50, 50]]).T
Sigma_u = np.diag(np.array([1, 1])**2)
# Define depth distribution.
mean_z = 10
var_z = 0.1**2
# TODO 2: Approximate the distribution of the 3D point in world coordinates:
# TODO 2: Propagate the expected point.
x_w = np.ones((3, 1)) # Mock implementation, do something else!
# TODO 2: Propagate the uncertainty.
cov_x_w = np.identity(3) # Mock implementation, do something else!
print(cov_x_w)
# Simulate points from the true distribution.
num_draws = 1000
random_poses = draw_random_poses(mean_T_w_c, Sigma_T_w_c, num_draws)
random_u = np.random.multivariate_normal(mean_u.flatten(), Sigma_u,
num_draws).T
random_z = np.random.normal(mean_z, np.sqrt(var_z), num_draws)
rand_x_c = backproject(random_u, random_z, f, c)
rand_x_w = np.zeros((3, num_draws))
for i in range(num_draws):
rand_x_w[:, [i]] = random_poses[i] * rand_x_c[:, [i]]
# Plot result.
# Use Qt 5 backend in visualisation.
matplotlib.use('qt5agg')
# Create figure and axis.
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
# Plot camera poses.
vg.plot_pose(ax, mean_T_w_c.to_tuple())
# Plot simulated points.
ax.plot(rand_x_w[0, :], rand_x_w[1, :], rand_x_w[2, :], 'k.', alpha=0.1)
# Plot the estimated mean pose.
vg.plot_covariance_ellipsoid(ax, x_w, cov_x_w)
# Show figure.
vg.plot.axis_equal(ax)
plt.show()
def poses_and_cameras():
# Use Qt 5 backend in visualisation.
matplotlib.use('qt5agg')
# Create figure and axis.
fig = plt.figure()
ax = plt.axes(projection='3d')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
# Plot the pose of the world North-East-Down (NED) frame (relative to the world frame).
T_w_w = SE3()
vg.plot_pose(ax, T_w_w.to_tuple(), scale=3, text='$\mathcal{F}_w$')
# Plot the body frame (a body-fixed Forward-Right-Down (FRD) frame).
roll = np.radians(-10)
pitch = np.radians(0)
yaw = np.radians(135)
t_w_b = np.array([[-10, -10, -2]]).T
T_w_b = SE3((SO3.from_roll_pitch_yaw(roll, pitch, yaw), t_w_b))
vg.plot_pose(ax, T_w_b.to_tuple(), scale=3, text='$\mathcal{F}_b$')
# Plot the camera frame.
# The camera is placed 2 m directly above the body origin.
# Its optical axis points to the left (in opposite direction of the y-axis in F_b).
# Its y-axis points downwards along the z-axis of F_b.
R_b_c = np.array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]])
t_b_c = np.array([[0, 0, -2]]).T
T_b_c = SE3((SO3(R_b_c), t_b_c))
T_w_c = T_w_b @ T_b_c
vg.plot_pose(ax, T_w_c.to_tuple(), scale=3, text='$\mathcal{F}_c$')
# Plot obstacle frame.
# The cube is placed at (North: 10 m, East: 10 m, Down: -1 m).
# Its top points upwards, and its front points south.
R_w_o = np.array([[-1, 0, 0],
[0, 1, 0],
[0, 0, -1]])
t_w_o = np.array([[10, 10, -1]]).T
T_w_o = SE3((SO3(R_w_o), t_w_o))
vg.plot_pose(ax, T_w_o.to_tuple(), scale=3, text='$\mathcal{F}_o$')
# Plot the cube with sides 3 meters.
points_o = vg.utils.generate_box(scale=3)
points_w = T_w_o * points_o
vg.utils.plot_as_box(ax, points_w)
# Plot the image plane.
img_plane_scale = 3
K = np.array([[50, 0, 40],
[0, 50, 30],
[0, 0, 1]])
vg.plot_camera_image_plane(ax, K, T_w_c.to_tuple(), scale=img_plane_scale)
# Project the box onto the normalised image plane (at z=img_plane_scale).
points_c = T_w_c.inverse() @ T_w_o * points_o
xn = points_c / points_c[2, :]
xn_w = T_w_c * (img_plane_scale * xn)
vg.utils.plot_as_box(ax, xn_w)
# Show figure.
vg.plot.axis_equal(ax)
ax.invert_zaxis()
ax.invert_yaxis()
plt.show()
def test_perturbation_by_adding_a_simple_rotation_with_operator_works():
ident = SO3()
pert = ident + (np.pi / 2 * np.array([[0, 1, 0]]).T)
expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
np.testing.assert_almost_equal(pert.matrix, expected, 14)
def test_string_representation_is_matrix():
X = SO3()
# Should be the same as the string representation of the matrix.
np.testing.assert_equal(str(X), str(X.matrix))
