def plot_sfm_result(result,
pose_indices,
point_indices,
x_axe=20,
y_axe=20,
z_axe=20):
"""
Plot mapping result.
"""
# Declare an id for the figure
figure_number = 0
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# gtsam_plot.plot_3d_points(figure_number, result, 'rx')
for idx in point_indices:
point_i = result.atPoint3(P(idx))
gtsam_plot.plot_point3(figure_number, point_i, 'rx')
# Plot cameras
for idx in pose_indices:
pose_i = result.atPose3(X(idx))
gtsam_plot.plot_pose3(figure_number, pose_i, 1)
# Draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(-z_axe, z_axe)
plt.legend()
plt.show()
def plot_trajectory_verification(landmarks,
poses,
trajectory,
x_axe=30,
y_axe=30,
z_axe=30,
axis_length=2,
figure_number=0):
"""Plot the map, mapping pose results, and the generated trajectory.
Parameters:
landmarks - a list of [x,y,z]
poses - a list of [x,y,z,1*9 rotation matrix]
trajectory - a list of Pose3 object
"""
# Declare an id for the figure
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot landmark points
for landmark in landmarks:
gtsam_plot.plot_point3(figure_number,
Point3(landmark[0], landmark[1], landmark[2]),
'rx')
# Plot mapping pose results
for pose in poses:
rotation = Rot3(np.array(pose[3:]).reshape(3, 3))
actual_pose_i = Pose3(rotation, Point3(np.array(pose[0:3])))
gtsam_plot.plot_pose3(figure_number, actual_pose_i, axis_length)
gRp = actual_pose_i.rotation().matrix() # rotation from pose to global
t = actual_pose_i.translation()
axes.scatter([t.x()], [t.y()], [t.z()],
s=20,
color='red',
alpha=1,
marker="^")
# Plot cameras
for pose in trajectory:
gtsam_plot.plot_pose3(figure_number, pose, axis_length)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
axes.scatter([t.x()], [t.y()], [t.z()], s=20, color='blue', alpha=1)
# draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(-z_axe, z_axe)
plt.legend()
plt.show()
def plot_localization(self, fignum, result):
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# Can't use data because self.img_pose[i+1]rent frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
# gtsam.plot_3d_points(result, [], marginals)
for point in result[1]:
gtsam_plot.plot_point3(fignum, point, 'rx')
# Plot cameras
for pose in result[0]:
gtsam_plot.plot_pose3(fignum, pose, 1)
# draw
axes.set_xlim3d(-20, 20)
axes.set_ylim3d(-20, 20)
axes.set_zlim3d(-20, 20)
plt.pause(60)
def plot_sfm_result(result, pose_indices, point_indices):
"""
Plot mapping result.
"""
# Declare an id for the figure
_fignum = 0
fig = plt.figure(_fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# gtsam_plot.plot_3d_points(_fignum, result, 'rx')
for idx in point_indices:
point_i = result.atPoint3(P(idx))
gtsam_plot.plot_point3(_fignum, point_i, 'rx')
# Plot cameras
for idx in pose_indices:
pose_i = result.atPose3(X(idx))
gtsam_plot.plot_pose3(_fignum, pose_i, 1)
# Draw
axes.set_xlim3d(-20, 20)
axes.set_ylim3d(-20, 20)
axes.set_zlim3d(-20, 20)
plt.legend()
plt.show()
def plot_trajectory(landmarks,
trajectory,
x_axe=30,
y_axe=30,
z_axe=30,
axis_length=2,
figure_number=0):
"""Plot the map and the generated trajectory.
Parameters:
landmarks - a list of [x,y,z]
trajectory - a list of Pose3 object
"""
# Declare an id for the figure
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot landmark points
for landmark in landmarks:
gtsam_plot.plot_point3(figure_number,
Point3(landmark[0], landmark[1], landmark[2]),
'rx')
# Plot cameras
for pose in trajectory:
gtsam_plot.plot_pose3(figure_number, pose, axis_length)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
axes.scatter([t.x()], [t.y()], [t.z()],
s=20,
color='red',
alpha=1,
marker="^")
# draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(-z_axe, z_axe)
plt.pause(1)