def gtsamOpt2PoseStampedarray(inputfname, pose_array):
pointID = 0
graph, initial = gtsam.readG2o(inputfname, True)
# 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 ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(
gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
resultPoses = gtsam.allPose3s(result)
keys = resultPoses.keys()
for i in range(keys.size()):
posetmp = gstamPose2Transform(resultPoses.atPose3(keys.at(i)))
posestampedtmp = PoseStamped()
posestampedtmp.header.frame_id = str(keys.at(i))
posestampedtmp.pose = posetmp
pose_array.poseArray.append(posestampedtmp)
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 draw_graph(graph, values, fig_num=1):
for i in range(graph.size()):
factor = graph.at(i)
fkeys = factor.keys()
if fkeys.size() == 2:
x = values.atPose3(fkeys.at(0)).translation()
if key_label(fkeys.at(1))[0] is 'x':
l = values.atPose3(fkeys.at(1)).translation()
else:
l = values.atPoint3(fkeys.at(1))
plot_line(fig_num, x, l, 'b')
poses = gtsam.allPose3s(values)
for i in range(poses.size()):
plot.plot_pose3(fig_num,
poses.atPose3(poses.keys().at(i)),
axis_length=2.0)
# after all images have been processed
plt.plot(x_val, y_val, label="Dead Reckoning x,y")
plt.plot(x_val, z_val, label="Dead Reckoning x,z")
plt.plot(z_val, y_val, label="Dead Reckoning y,z")
plt.title('Dead Reckoning')
plt.legend()
plt.show()
# if frame_count > 500:
# factor = gtsam.BetweenFactorPose3(frame_count-1, 0, gtsam.Pose3(), odom_noise)
# graph.add(factor)
gtparams = gtsam.GaussNewtonParams()
optimizer = gtsam.GaussNewtonOptimizer(graph, estimates, gtparams)
result = optimizer.optimize()
result_poses = gtsam.allPose3s(result)
gtsam_x_val = []
gtsam_y_val = []
gtsam_z_val = []
r_off = np.mat([[1., 0., 0.],
[0., 0.91363181, - 0.40654264],
[0., 0.40654264, 0.91363181]])
from mpl_toolkits.mplot3d import Axes3D
for k in range(result_poses.size()):
plot.plot_pose3(1, result_poses.atPose3(k))
p = result_poses.atPose3(k)
t = p.translation()
# r = p.roation().matrix()
# print(t)
t = np.array(np.mat([t.x(), t.y(), t.z()]) * r_off).reshape(3)
