def plot_estimate(self, fignum=0):
"""
VisualISAMPlot plots current state of ISAM2 object
Author: Ellon Paiva
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
# gtsam.plot_3d_points(result, [], marginals)
gtsam_plot.plot_3d_points(fignum, self.current_estimate, 'rx')
# Plot cameras
i = 0
while self.current_estimate.exists(iSAM2Wrapper.get_key('x', i)):
pose_i = self.current_estimate.atPose3(iSAM2Wrapper.get_key(
'x', i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
i += 1
# draw
#axes.set_xlim3d(-40, 40)
#axes.set_ylim3d(-40, 40)
#axes.set_zlim3d(-40, 40)
axes.view_init(90, 0)
plt.pause(.01)
def visual_ISAM2_plot(result):
# Declare an id for the figure
fignum = 0
fig = None
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
gtsam_plot.plot_trajectory(fignum, result)
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 1)
i += 1
# draw
# axes.set_xlim3d(-40, 40)
# axes.set_ylim3d(-40, 40)
# axes.set_zlim3d(-40, 40)
# plt.show()
axes.view_init(elev=180, azim=90)
plt.savefig("Figures/traj.png")
pickle.dump(fig, open("Figures/fig.pickle", "wb"))
plt.show()
def plot(self,
values: gtsam.Values,
title: str = "Estimated Trajectory",
fignum: int = POSES_FIG + 1,
show: bool = False):
"""
Plot poses in values.
Args:
values: The values object with the poses to plot.
title: The title of the plot.
fignum: The matplotlib figure number.
POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure.
show: Flag indicating whether to display the figure.
"""
i = 0
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
plot_pose3(fignum, pose_i, 1)
i += 1
plt.title(title)
gtsam.utils.plot.set_axes_equal(fignum)
print("Bias Values", values.atConstantBias(BIAS_KEY))
plt.ioff()
if show:
plt.show()
def visual_ISAM2_plot(result):
"""
VisualISAMPlot plots current state of ISAM2 object
Author: Ellon Paiva
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
"""
# Declare an id for the figure
fignum = 0
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
# gtsam.plot_3d_points(result, [], marginals)
gtsam_plot.plot_3d_points(fignum, result, 'rx')
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
i += 1
# draw
axes.set_xlim3d(-40, 40)
axes.set_ylim3d(-40, 40)
axes.set_zlim3d(-40, 40)
plt.pause(1)
def run(self, T=12):
gtNavStates = []
predictedNavStates = []
# simulate the loop
for i, t in enumerate(np.arange(0, T, self.dt)):
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
if i % 100 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
gtNavState = self.plotGroundTruthPose(t, time_interval=0.001)
pim = self.runner.integrate(t, self.actualBias, True)
predictedNavState = self.runner.predict(pim, self.actualBias)
plot_pose3(POSES_FIG, predictedNavState.pose(), 1)
gtNavStates.append(gtNavState)
predictedNavStates.append(predictedNavState)
ATE = []
for gt, pred in zip(gtNavStates, predictedNavStates):
predPose = pred.pose()
delta = gt.inverse().compose(predPose)
ATE.append(np.linalg.norm(delta.Logmap(delta))**2)
print("ATE={}".format(np.sqrt(np.mean(ATE))))
plt.ioff()
plt.show()
def plot_sfm_result(self,result):
# Declare an id for the figure
fignum = 0
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
# gtsam.plot_3d_points(result, [], marginals)
gtsam_plot.plot_3d_points(fignum, result, 'rx')
# gtsam_plot.plot_pose3(fignum, result, 'rx')
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 2)
i += 1
# draw
axes.set_xlim3d(-50, 50)
axes.set_ylim3d(-50, 50)
axes.set_zlim3d(-50, 50)
plt.legend()
plt.show()
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_with_result(result,
x_axe=30,
y_axe=30,
z_axe=30,
axis_length=2,
figure_number=0):
"""plot the result of sfm"""
# Declare an id for the figure
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
gtsam_plot.plot_3d_points(figure_number, result, 'rx')
# Plot cameras
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
axis_length = axis_length
gtsam_plot.plot_pose3(figure_number, pose_i, axis_length)
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 main():
"""Main runner."""
parser = argparse.ArgumentParser(
description="A 3D Pose SLAM example that reads input from g2o, and "
"initializes Pose3")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument(
'-o',
'--output',
help="the path to the output file with optimized graph")
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
else args.input
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel.Diagonal.Variances(
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print("Done!")
if args.plot:
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()
def plotGroundTruthPose(i):
# plot ground truth pose, as well as prediction from integrated IMU measurements
actualPose = gtsam.Pose3(true_poses[i])
plot_pose3(POSES_FIG, actualPose, 0.3)
ax = plt.gca()
ax.set_xlim3d(-5, 5)
ax.set_ylim3d(-5, 5)
ax.set_zlim3d(-5, 5)
plt.pause(0.01)
def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
# plot ground truth pose, as well as prediction from integrated IMU measurements
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, scale)
t = actualPose.translation()
self.maxDim = max([max(np.abs(t)), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)
ax.set_zlim3d(-self.maxDim, self.maxDim)
plt.pause(time_interval)
def plotGroundTruthPose(self, t):
# plot ground truth pose, as well as prediction from integrated IMU measurements
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, 0.3)
t = actualPose.translation()
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)
ax.set_zlim3d(-self.maxDim, self.maxDim)
plt.pause(0.01)
def run(self, T: int = 12):
"""Simulate the loop."""
for i, t in enumerate(np.arange(0, T, self.dt)):
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
if i % 25 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
self.plotGroundTruthPose(t)
pim = self.runner.integrate(t, self.actualBias, True)
predictedNavState = self.runner.predict(pim, self.actualBias)
plot_pose3(POSES_FIG, predictedNavState.pose(), 0.1)
plt.ioff()
plt.show()
def plot_cameras_gtsam(transform_matrix_filename: str):
set_cams = np.load(
'/home/sushmitawarrier/clevr-dataset-gen/output/images/CLEVR_new000000/set_cams.npz'
)
with open(transform_matrix_filename) as f:
transforms = json.load(f)
figure_number = 0
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot cameras
idx = 0
for i in range(len(set_cams['set_rot'])):
print(set_cams['set_rot'][i])
print("i", i)
rot = set_cams['set_rot'][i]
t = set_cams['set_loc'][i]
rot_3 = gtsam.Rot3.RzRyRx(rot)
pose_i = gtsam.Pose3(rot_3, t)
if idx % 2 == 0:
gtsam_plot.plot_pose3(figure_number, pose_i, 1)
break
idx += 1
for i in transforms['frames']:
T = i['transform_matrix']
pose_i = gtsam.Pose3(T)
#print("T", T)
#print("pose_i",pose_i)
#print(set_cams['set_loc'][0])
#print(set_cams['set_rot'][0])
# Reference pose
# trying to look along x-axis, 0.2m above ground plane
# x forward, y left, z up (x-y is the ground plane)
upright = gtsam.Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
pose_j = gtsam.Pose3(upright, gtsam.Point3(0, 0, 0.2))
axis_length = 1
# plotting alternate poses
if idx % 2 == 0:
# gtsam_plot.plot_pose3(figure_number, pose_i, axis_length)
gtsam_plot.plot_pose3(figure_number, pose_j, axis_length)
idx += 1
x_axe, y_axe, z_axe = 10, 10, 10
# Draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(0, 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_it(result):
resultPoses = gtsam.allPose3s(result)
xyz = [
resultPoses.atPose3(i).translation() for i in range(resultPoses.size())
]
x_ = [pose.x() for pose in xyz]
y_ = [pose.y() for pose in xyz]
z_ = [pose.z() for pose in xyz]
fig = plt.figure()
ax = fig.gca(projection='3d')
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
ax.text(x_[i], y_[i], z_[i], str(i))
plt.plot(x_, y_, z_, 'k-')
plt.show()
def test_pose_estimate_generator(self):
"""test pose estimate generator"""
# theta = 45
# delta_x = 1
# delta_y = 2
# delta_z = 3
# rows = 2
# cols = 2
# angles = 8
theta = 45
delta_x = 1
delta_y = 2
delta_z = 3
rows = 2
cols = 2
angles = 4
degree = math.radians(45)
R = np.array([[math.cos(degree), -math.sin(degree), 0],
[math.sin(degree), math.cos(degree), 0], [0, 0, 1]])
prior1_delta = [5, 5, 5, 30]
prior2_delta = [-5, -5, -3, -90]
actual_pose_estimates = pose_estimate_generator(
theta, delta_x, delta_y, delta_z, prior1_delta, prior2_delta, rows,
cols, angles, R)
# Declare an id for the figure
figure_number = 0
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot cameras
pose_origin = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(0, 0, 0))
axis_length = 1
gtsam_plot.plot_pose3(figure_number, pose_origin, axis_length)
for pose in actual_pose_estimates:
gtsam_plot.plot_pose3(figure_number, pose, axis_length)
# Draw
x_axe = y_axe = z_axe = 20
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_with_results(result1,
result2,
x_axe=30,
y_axe=30,
z_axe=30,
figure_number1=0,
figure_number2=1):
"""plot the two results of sfm at the same time"""
# Declare an id for the figure
fig = plt.figure(figure_number1)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
gtsam_plot.plot_3d_points(figure_number1, result1, 'rx')
# Plot cameras
i = 0
while result1.exists(X(i)):
pose_i = result1.atPose3(X(i))
gtsam_plot.plot_pose3(figure_number1, pose_i, 2)
i += 1
# draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(-z_axe, z_axe)
# Declare an id for the figure
fig = plt.figure(figure_number2)
axes = fig.gca(projection='3d')
plt.cla()
# Plot points
gtsam_plot.plot_3d_points(figure_number2, result2, 'rx')
# Plot cameras
i = 0
while result2.exists(X(i)):
pose_i = result2.atPose3(X(i))
gtsam_plot.plot_pose3(figure_number2, pose_i, 2)
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()
class TestPoses(unittest.TestCase):
"""
class to check if poses are correct
"""
# World coordinate system: right handed. X forward, Y left, Z up
# Create a reference object, facing along X-axis and elevation of 0 deg.
az = 90
ele = 0
tilt = 0
dist = 3
# get location and rotation vector from az,ele and dist values
loc, rot = config_cam(az, ele, dist)
# compute transformation matrix using pose_spherical function
computed_T = pose_spherical(az, ele, dist)
pose_i = gtsam.Pose3(computed_T)
# Construct pose using rotation and location vectors
pose_j = gtsam.Pose3(gtsam.Rot3.RzRyRx(rot), loc)
# Compares rotation vector from constructed pose with rotation vector initially computed
rot_p = pose_j.rotation()
rot_from_ele = rot_p.xyz()
# Returns true
assert_array_almost_equal(rot_from_ele, rot)
figure_number = 0
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
upright = gtsam.Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
pose_j = gtsam.Pose3(upright, gtsam.Point3(0, 0, 0.2))
axis_length = 1
gtsam_plot.plot_pose3(figure_number, pose_i, axis_length)
gtsam_plot.plot_pose3(figure_number, pose_j, axis_length)
x_axe, y_axe, z_axe = 10, 10, 10
# Draw
axes.set_xlim3d(-x_axe, x_axe)
axes.set_ylim3d(-y_axe, y_axe)
axes.set_zlim3d(0, z_axe)
plt.legend()
#plt.show()
filename = "/home/sushmitawarrier/results/test_pose_clevr3.png"
plt.savefig(filename)
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 plotGroundTruthPose(self,
t: float,
scale: float = 0.3,
time_interval: float = 0.01):
"""
Plot ground truth pose, as well as prediction from integrated IMU measurements.
Args:
t: Time at which the pose was obtained.
scale: The scaling factor for the pose axes.
time_interval: The time to wait before showing the plot.
"""
actualPose = self.scenario.pose(t)
plot_pose3(POSES_FIG, actualPose, scale)
translation = actualPose.translation()
self.maxDim = max([max(np.abs(translation)), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)
ax.set_zlim3d(-self.maxDim, self.maxDim)
plt.pause(time_interval)
def test_pose_estimate_generator(self):
"""test pose estimate generator"""
# Camera to world rotation
wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name
theta = 45
delta_x = [0, 3.7592, 3.7592 + 5]
delta_y = [0, 1.75895, 1.75895 * 2, 1.75895 * 3]
delta_z = 0.9652
rows = 4
cols = 3
angles = 1
degree = math.radians(theta)
R = np.array([[math.cos(degree), -math.sin(degree), 0],
[math.sin(degree), math.cos(degree), 0], [0, 0, 1]])
prior1_delta = [0, 0, delta_z, 0]
prior2_delta = [3.7592, 0, delta_z, 0]
actual_pose_estimates = pose_estimate_generator_rectangle(
theta, delta_x, delta_y, delta_z, prior1_delta, prior2_delta, rows,
cols, angles)
# Declare an id for the figure
figure_number = 0
fig = plt.figure(figure_number)
axes = fig.gca(projection='3d')
plt.cla()
# Plot cameras
pose_origin = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(0, 0, 0))
axis_length = 1
gtsam_plot.plot_pose3(figure_number, pose_origin, axis_length)
for pose in actual_pose_estimates:
gtsam_plot.plot_pose3(figure_number, pose, axis_length)
# Draw
x_axe = y_axe = z_axe = 20
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 ISAM2_plot(values, fignum=0):
"""Plot poses."""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plt.cla()
i = 0
min_bounds = 0, 0, 0
max_bounds = 0, 0, 0
while values.exists(X(i)):
pose_i = values.atPose3(X(i))
gtsam_plot.plot_pose3(fignum, pose_i, 10)
position = pose_i.translation().vector()
min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
# max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
i += 1
# draw
axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
plt.pause(1)
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)
params.dt)
############################### Optimize the factor graph ####################################
# Use the Levenberg-Marquardt algorithm
optimization_params = gtsam.LevenbergMarquardtParams()
optimization_params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(factor_graph, initial_values,
optimization_params)
optimization_result = optimizer.optimize()
############################### Plot the solution ####################################
for i in preintegration_steps:
# Ground truth pose
plot_pose3(1, gtsam.Pose3(true_poses[i]), 0.3)
# Estimated pose
plot_pose3(1, optimization_result.atPose3(symbol('x', i)), 0.1)
ax = plt.gca()
ax.set_xlim3d(-5, 5)
ax.set_ylim3d(-5, 5)
ax.set_zlim3d(-5, 5)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.figure(1).suptitle("Large poses: ground truth, small poses: estimate")
plt.ioff()
plt.show()
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print ("Done!")
if args.plot:
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()
def run(self):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
T = 12
num_poses = T + 1 # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity())
# simulate the loop
i = 0 # state index
actual_state_i = self.scenario.navState(0)
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
# Plot every second
if k % int(1 / self.dt) == 0:
self.plotGroundTruthPose(t)
# create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(
i + 1), V(i + 1), BIAS_KEY, pim)
graph.push_back(factor)
if True:
print(factor)
print(pim.predict(actual_state_i, self.actualBias))
pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt)
i += 1
# add priors on beginning and end
self.addPrior(0, graph)
self.addPrior(num_poses - 1, graph)
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1
print(result.atimuBias_ConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
# Use the Levenberg-Marquardt algorithm
optimization_params = gtsam.LevenbergMarquardtParams()
optimization_params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(factor_graph, initial_values,
optimization_params)
optimization_result = optimizer.optimize()
############################### Plot the solution ####################################
figure = plt.figure(1)
axes = plt.gca(projection='3d')
axes.set_xlim3d(-2, 2)
axes.set_ylim3d(-2, 2)
axes.set_zlim3d(-2, 2)
axes.set_xlabel('x')
axes.set_ylabel('y')
axes.set_zlabel('z')
axes.margins(0)
figure.suptitle("Large/small poses: initial/optimized estimate")
for i in preintegration_steps:
# Initial estimate from IMU preintegration
plot_pose3(1, initial_values.atPose3(symbol('x', i)), 0.2)
# Optimized estimate
plot_pose3(1, optimization_result.atPose3(symbol('x', i)), 0.08)
plt.pause(0.01)
plt.ioff()
plt.show()
for i,image in enumerate(fe.image_list):
# for i in range(1,2):
# image = fe.image_list[1]
assert counter != 3, "Localization Failed"
superpoint_features = estimator.superpoint_generator(image)
# print(superpoint_features.key_points)
pre_pose = estimator.trajectory[len(estimator.atrium_map.trajectory)-1]
projected_features, map_indices = estimator.landmarks_projection(
pre_pose)
# print(projected_features.key_points)
# print(map_indices)
features, visible_map = estimator.data_association(
superpoint_features, projected_features, map_indices)
# if features.get_length() < 5:
# counter += 1
# print("Not enough correspondences.")
# continue
# elif counter > 0:
# counter = 0
# print(features.key_points)
cur_pose = estimator.trajectory_estimator(features, visible_map)
# gtsam_plot.plot_pose3(fignum, sim3.pose(cur_pose), 5)
gtsam_plot.plot_pose3(fignum, cur_pose, 5)
print("cur_pose",i,":",cur_pose)
# # print(estimator.atrium_map.trajectory)
plt.ioff()
plt.show()
