def plot(self, errs=None):
time_samples = np.linspace(self.T_L_I.node_times[1] + 0.1,
self.T_L_I.node_times[-3] - 0.1, 1000)
# Plot initial situation (xy and in time):
plt.figure(0)
imu_positions = np.array(
[self.T_L_I.sample(t).t.ravel() for t in time_samples])
prism_positions = np.array([
(self.T_L_I.sample(t) * self.alignment.p_I_P).ravel()
for t in time_samples
])
plots.xyPlot(imu_positions, prism_positions, self.p_L_Ps)
plt.title('x,y trajectory of %s' % flags.sequenceSensorString())
plt.figure(1)
plots.xyztPlot(time_samples, imu_positions, prism_positions,
self.t_G0_Ps(), self.p_L_Ps)
plt.title('x,y,z trajectory of %s' % flags.sequenceSensorString())
if errs is not None:
plt.figure(2)
plt.semilogx(sorted(errs),
np.arange(len(errs), dtype=float) / len(errs))
plt.grid()
plt.xlabel('Leica error [m]')
plt.ylabel('Fraction of measurements with smaller error')
plt.title('Cumulative distribution of Leica error for %s' %
flags.sequenceSensorString())
plt.show()
def callback(sequence_i):
if sequence_i % FLAGS.num_workers == FLAGS.worker_i:
if FLAGS.bag:
print('De-ground truthing %s bag...' %
flags.sequenceSensorString())
deGroundTruthBag()
if FLAGS.zip:
print('De-ground truthing %s zip...' %
flags.sequenceSensorString())
deGroundTruthZip()
def callback(sequence_i):
if sequence_i % FLAGS.num_workers == FLAGS.worker_i:
if FLAGS.bag:
if FLAGS.redo_min_bags or not os.path.exists(
flags.minImuGroundTruthOdometryBagPath()):
print('Generating min bag for %s' %
flags.sequenceSensorString())
makeMinImuGtoBag()
print('Comparing bag gyro for %s' % flags.sequenceSensorString())
compareGyro()
if FLAGS.zip:
print('Checking zip looks same as bag for %s' %
flags.sequenceSensorString())
checkZipIsSameAsBag()
def compareGyro():
gyro_stamps = []
gyros = []
gt_attitude_stamps = []
gt_attitudes = []
if not os.path.exists(flags.minImuGroundTruthOdometryBagPath()):
raise Exception('Run make_min_imu_gto_bag first!')
bag = rosbag.Bag(flags.minImuGroundTruthOdometryBagPath())
for bag_msg in bag:
msg = bag_msg.message
if bag_msg.topic == flags.imuTopic():
gyro_stamps.append(msg.header.stamp)
gyros.append(np.array([msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z]))
if bag_msg.topic == '/groundtruth/odometry':
gt_attitude_stamps.append(msg.header.stamp)
gt_attitudes.append(pyquaternion.Quaternion(
msg.pose.pose.orientation.w,
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z))
gyro_times = [(i - gyro_stamps[0]).to_sec() for i in gyro_stamps]
gt_times = [(i - gyro_stamps[0]).to_sec() for i in gt_attitude_stamps]
if FLAGS.gt_from_txt != '':
print('Loading custom ground truth...')
loaded = np.loadtxt(FLAGS.gt_from_txt)
gt_attitude_stamps = None
gt_times = loaded[:, 0] - gyro_stamps[0].to_sec()
gt_attitudes = [pyquaternion.Quaternion(i[7], i[4], i[5], i[6]) for i in loaded]
gt_Rs = [i.rotation_matrix for i in gt_attitudes]
gt_twists = []
for i in range(1, len(gt_attitudes) - 1):
relative = np.dot(gt_Rs[i - 1].T, gt_Rs[i + 1])
gt_twists.append(np.array([
(relative[2, 1] - relative[1, 2]) / 2,
(relative[0, 2] - relative[2, 0]) / 2,
(relative[1, 0] - relative[0, 1]) / 2
]) / (gt_times[i + 1] - gt_times[i - 1]))
plt.plot(gyro_times, [i[0] for i in gyros], '.')
plt.plot(gt_times[1:-1], [i[0] for i in gt_twists])
plt.grid()
plt.title('x gyro to gt comparison %s' % flags.sequenceSensorString())
plt.xlabel('Time[s]')
plt.ylabel('Angular rate [rad]')
plt.show()
def callback(sequence_i):
if FLAGS.sens == 'davis':
plt.figure(0)
else:
assert FLAGS.sens == 'snap'
plt.figure(1)
files = flags.SplineLeicaAlignmentFiles('gt_leica_alignment')
errs = np.loadtxt(files.errors)
if FLAGS.env == 'i':
color = plt.get_cmap('tab10').colors[0]
else:
color = plt.get_cmap('tab10').colors[1]
if (FLAGS.sens, FLAGS.env) not in done:
label = 'outdoor' if FLAGS.env == 'o' else 'indoor'
done.add((FLAGS.sens, FLAGS.env))
else:
label = None
plt.semilogx(sorted(errs), np.arange(len(errs), dtype=float) / len(errs), '.', color=color, ms=0.5, label=label)
errss.append((flags.sequenceSensorString(), errs))
p_I_Ps.append((flags.sequenceSensorString(), np.loadtxt(files.p_I_P).ravel()))
def callback(sequence_i):
print('%s' % flags.sequenceSensorString())
def noGtCallback(sequence_i):
if FLAGS.bag:
filenames.append(flags.sequenceSensorString() + '.bag')
if FLAGS.zip:
filenames.append(flags.sequenceSensorString() + '.zip')
def gtCallback(sequence_i):
if FLAGS.bag:
filenames.append(flags.sequenceSensorString() + '_with_gt.bag')
if FLAGS.zip:
filenames.append(flags.sequenceSensorString() + '_with_gt.zip')
mRerr = Rerrs.mean()
print('Mean rotation error is %f degrees' % mRerr)
mRerrs.append(mRerr)
plt.figure(d_i)
for dim, name in enumerate(['x', 'y', 'z']):
plt.plot(gt_times, [i.t[dim] for i in gt_poses], label=name)
plt.plot(traj.node_times, [i.t[dim] for i in traj.node_poses],
'x',
label='%s nodes' % name)
plt.plot(test_times, [i.t[dim] for i in test_poses],
'.',
label='%s samples' % name)
plt.grid()
# plt.legend()
plt.title('Disc %.3f s, %s' % (disc, flags.sequenceSensorString()))
plt.xlabel('t')
plt.ylabel('position')
plt.figure(d_i + 1)
fig, ax1 = plt.subplots()
color = 'tab:red'
ax1.set_xlabel('dt = 2^-i')
ax1.set_ylabel('position error [m]', color=color)
ax1.semilogy(mterrs, color=color)
ax1.grid()
ax2 = ax1.twinx()
def run():
print('Getting samples from bag...')
gt_times, gt_poses, t_GT_spline = evaluate_discretizations.getSamplesFromBag(
)
gt_traj = linear_interpolation.Trajectory(gt_times, gt_poses)
print('Calculating initial guess...')
traj, valid_times, valid_poses, valid_mask = \
evaluate_discretizations.considerDiscretization(gt_times, gt_poses, FLAGS.dt)
valid_gt_poses = [i for i, v in zip(gt_poses, valid_mask) if v]
print('Formulating symbolic trajectory...')
sym_traj = bspline_opt.SymbolicTrajectory(traj)
print('Calculating error terms...')
want_num_error_terms = FLAGS.errs
step = (len(valid_times) / want_num_error_terms) + 1
error_times = valid_times[::step]
error_gt_poses = valid_gt_poses[::step]
print('Using %d error times' % len(error_times))
eRvec = None
etvec = None
gtposes = []
symposes = []
for t, gtps in zip(error_times, error_gt_poses):
eR, et, gtp, syp = errorAtTime(gt_traj, sym_traj, t, gt_pose=gtps)
if eR is not None:
if eRvec is None:
eRvec = eR
etvec = et
else:
eRvec = casadi.vertcat(eRvec, eR)
etvec = casadi.vertcat(etvec, et)
gtposes.append(gtp)
symposes.append(syp)
print('Setting up problem...')
nlp = {'x': sym_traj.xvec, 'f': casadi.sum1(casadi.vertcat(eRvec, etvec))}
n_iter = 3
ipopt_options = {'max_iter': n_iter}
nlp_options = {'ipopt': ipopt_options, 'verbose': False}
S = casadi.nlpsol('S', 'ipopt', nlp, nlp_options)
print('3 iterations at a time...')
iters = [0]
print('Old trajectory:')
mRerr, mterr = numericTrajectoryError(valid_gt_poses, valid_poses)
mterrs = [mterr]
mRerrs = [mRerr]
x0 = np.zeros(sym_traj.xvec.shape)
xs = [x0]
for iter in range(10):
r = S(x0=x0)
new_traj = sym_traj.newTrajectory(r['x'])
new_valid_poses = [new_traj.sample(i) for i in valid_times]
iters.append((iter + 1) * n_iter)
print('New trajectory, %d iters:' % iters[-1])
mRerr, mterr = numericTrajectoryError(valid_gt_poses, new_valid_poses)
mterrs.append(mterr)
mRerrs.append(mRerr)
x0 = r['x']
xs.append(r['x'])
files = flags.GtFittedSplineFiles()
if not os.path.exists(files.root):
os.makedirs(files.root)
new_traj.serialize(files.spline)
util.writeRostimeToFile(files.t_GT_spline, t_GT_spline)
mtns = [maxTwistNorm(x) for x in xs]
atns = [avgTwistNorm(x) for x in xs]
# Axis 1: Relative error
fig, ax1 = plt.subplots()
color = 'tab:red'
ax1.set_xlabel('iteration')
ax1.set_ylabel('Relative error', color=color)
ax1.semilogy(iters, [i / max(mterrs) for i in mterrs],
color=color,
label='translation')
ax1.semilogy(iters, [i / max(mRerrs) for i in mRerrs],
'--',
color=color,
label='rotation')
ax1.legend()
ax1.grid()
ax2 = ax1.twinx()
color = 'tab:blue'
ax2.set_ylabel('twist norm (rotation)', color=color)
ax2.plot(iters, mtns, color=color, label='max')
ax2.plot(iters, atns, '--', color=color, label='mean')
ax1.set_title('%s, dt=%.3f, %d nodes, %d t(err)s' %
(flags.sequenceSensorString(), FLAGS.dt, len(
traj.node_times), len(error_times)))
fig.tight_layout() # otherwise the right y-label is slightly clipped
if FLAGS.gui:
plt.show()
else:
plt.savefig(os.path.join(flags.plotPath(), 'fit_spline_to_gt.pdf'),
bbox_inches='tight')
def run():
alignment = SplineLeicaAlignment()
alignment.deserialize()
ground_truth = min_bags.readGroundTruthFromImuGtoBag()
t_L_Ps, p_L_Ps = leica.parseTextFile(
os.path.join(flags.rawDataPath(), 'leica.txt'))
while True:
aligned_gt = GroundTruthInLeicaFrame(ground_truth, alignment)
t_L_PGTs = aligned_gt.times()
t_G0_PGTs = [(i - t_L_PGTs[0]).to_sec() for i in t_L_PGTs]
t_G0_Ps = [(i - t_L_PGTs[0]).to_sec() for i in t_L_Ps]
# Calculate errors
T_L_I_of_t_G0 = aligned_gt.getLinearInterpolation()
num_errors = numericalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps,
aligned_gt.alignment.p_I_P)
print('Mean square error is %f' % np.mean(np.square(num_errors)))
# Empirically, optimization only made stuff worse after this (?)
# break
opti = casadi.Opti()
t_corr_G0 = opti.variable()
T_corr_L_twist = opti.variable(6)
p_I_P = opti.variable(3)
opti.set_initial(p_I_P, aligned_gt.alignment.p_I_P)
errvec = symbolicalErrors(T_L_I_of_t_G0, t_G0_Ps, p_L_Ps, p_I_P,
t_corr_G0, T_corr_L_twist)
opti.minimize(casadi.sum1(errvec))
opti.solver('ipopt')
sol = opti.solve()
upd_coeffs = np.hstack(
(sol.value(t_corr_G0), sol.value(T_corr_L_twist),
sol.value(p_I_P) - alignment.p_I_P.ravel()))
if np.max(np.abs(upd_coeffs)) < 1e-5:
break
alignment.t_G_L = alignment.t_G_L + sol.value(t_corr_G0)
alignment.T_L_G = pose.fromTwist(
sol.value(T_corr_L_twist)) * alignment.T_L_G
alignment.p_I_P = sol.value(p_I_P).reshape((3, 1))
alignment.serialize('gt_leica_alignment')
out_files = flags.SplineLeicaAlignmentFiles('gt_leica_alignment')
np.savetxt(out_files.errors, np.array(num_errors))
if FLAGS.gui:
plt.figure(0)
plots.xyPlot(aligned_gt.imuPositions(), aligned_gt.prismPositions(),
p_L_Ps)
plt.title('x,y trajectory of %s' % flags.sequenceSensorString())
plt.figure(1)
plots.xyztPlot(t_G0_PGTs, aligned_gt.imuPositions(),
aligned_gt.prismPositions(), t_G0_Ps, p_L_Ps)
plt.title('x,y,z trajectory of %s' % flags.sequenceSensorString())
plt.figure(2)
plt.semilogx(sorted(num_errors),
np.arange(len(num_errors), dtype=float) / len(num_errors))
plt.grid()
plt.xlabel('Leica error [m]')
plt.ylabel('Fraction of measurements with smaller error')
plt.title('Cumulative distribution of Leica error for %s' %
flags.sequenceSensorString())
plt.show()
