def main():
# World box.
points_w = vg.utils.generate_box()
# True observer pose.
true_pose_wo = SE3((SO3.rot_z(np.pi), np.array([[3, 0, 0]]).T))
# Observed box.
points_o = vg.utils.generate_box(pose=true_pose_wo.inverse().to_tuple())
# Perturb observer pose and use as initial state.
init_pose_wo = true_pose_wo + np.random.randn(6, 1)
# Estimate pose in the world frame from point correspondences.
model = NoiseFreePointAlignmentBasedPoseEstimatorObjective(
points_w, points_o)
x, cost, A, b = gauss_newton(init_pose_wo, model)
# Visualize (press a key to jump to the next iteration).
# 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 box and true state
vg.plot_pose(ax, true_pose_wo.to_tuple(), scale=1, alpha=0.4)
vg.utils.plot_as_box(ax, points_w, alpha=0.4)
# Plot initial state (to run axis equal first time).
ax.set_title('Cost: ' + str(cost[0]))
artists = vg.plot_pose(ax, x[0].to_tuple(), scale=1)
artists.extend(vg.utils.plot_as_box(ax, x[0] * points_o))
vg.plot.axis_equal(ax)
plt.draw()
while True:
if plt.waitforbuttonpress():
break
# Plot iterations
for i in range(1, len(x)):
for artist in artists:
artist.remove()
ax.set_title('Cost: ' + str(cost[i]))
artists = vg.plot_pose(ax, x[i].to_tuple(), scale=1)
artists.extend(vg.utils.plot_as_box(ax, x[i] * points_o))
plt.draw()
while True:
if plt.waitforbuttonpress():
break
def test_composition():
X = SE3((SO3.rot_x(np.pi / 2), np.array([[1, 2, 3]]).T))
Y = SE3((SO3.rot_z(np.pi / 2), np.array([[0, 1, 0]]).T))
Z = X.compose(Y)
rot_expected = SO3.from_angle_axis(2 * np.pi / 3, np.array([[1, -1, 1]]).T / np.sqrt(3))
t_expected = np.array([[1, 2, 4]]).T
np.testing.assert_almost_equal(Z.rotation.matrix, rot_expected.matrix, 14)
np.testing.assert_almost_equal(Z.translation, t_expected, 14)
def test_construct_with_roll_pitch_yaw():
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(np.pi / 2, 0, 0).matrix,
SO3.rot_x(np.pi / 2).matrix, 14)
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(0, np.pi / 2, 0).matrix,
SO3.rot_y(np.pi / 2).matrix, 14)
np.testing.assert_almost_equal(
SO3.from_roll_pitch_yaw(0, 0, np.pi / 2).matrix,
SO3.rot_z(np.pi / 2).matrix, 14)
roll = np.pi
pitch = np.pi / 2
yaw = np.pi
so3 = SO3.from_roll_pitch_yaw(roll, pitch, yaw)
expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
np.testing.assert_almost_equal(so3.matrix, expected, 14)
def test_rot_z_works():
rot_90_z = SO3.rot_z(0.5 * np.pi)
expected_matrix = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
np.testing.assert_almost_equal(rot_90_z.matrix, expected_matrix, 14)
def main():
# World box.
points_w = vg.utils.generate_box()
# True observer pose.
true_pose_wo = SE3((SO3.rot_z(np.pi), np.array([[3, 0, 0]]).T))
# Observed box with noise.
points_o = vg.utils.generate_box(pose=true_pose_wo.inverse().to_tuple())
num_points = points_o.shape[1]
point_covariances = [np.diag(np.array([1e-1, 1e-1, 1e-1]) ** 2)] * num_points
for c in range(num_points):
points_o[:, [c]] = points_o[:, [c]] + np.random.multivariate_normal(np.zeros(3), point_covariances[c]).reshape(
-1, 1)
# Perturb observer pose and use as initial state.
init_pose_wo = true_pose_wo + 10 * np.random.randn(6, 1)
# Estimate pose in the world frame from point correspondences.
model = NoisyPointAlignmentBasedPoseEstimatorObjective(points_w, points_o, point_covariances)
x, cost, A, b = gauss_newton(init_pose_wo, model)
cov_x_final = np.linalg.inv(A.T @ A)
# Print covariance.
with np.printoptions(precision=3, suppress=True):
print('Covariance:')
print(cov_x_final)
# Visualize (press a key to jump to the next iteration).
# 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 box and true state
vg.plot_pose(ax, true_pose_wo.to_tuple(), scale=1, alpha=0.4)
vg.utils.plot_as_box(ax, points_w, alpha=0.4)
# Plot initial state (to run axis equal first time).
ax.set_title('Cost: ' + str(cost[0]))
artists = vg.plot_pose(ax, x[0].to_tuple(), scale=1)
artists.extend(vg.utils.plot_as_box(ax, x[0] * points_o))
vg.plot.axis_equal(ax)
plt.draw()
while True:
if plt.waitforbuttonpress():
break
# Plot iterations
for i in range(1, len(x)):
for artist in artists:
artist.remove()
ax.set_title('Cost: ' + str(cost[i]))
artists = vg.plot_pose(ax, x[i].to_tuple(), scale=1)
artists.extend(vg.utils.plot_as_box(ax, x[i] * points_o))
plt.draw()
while True:
if plt.waitforbuttonpress():
break
