def interpolate_measure(measure, alpha):
R = measure.R
t = measure.t
q = g.SE3_from_rotation_translation(R, t)
vel = g.SE3.algebra_from_group(q)
rel = g.SE3.group_from_algebra(vel * alpha)
newR, newt = g.rotation_translation_from_SE3(rel)
return g2o.Isometry3d(newR, newt)
def interpolate(self, old_time_stamp, new_time_stamp, measure):
"""Given a timestamp new_time_stamp at which an odometry message was
received and the timestamp old_time_stamp of the last odometry
message previously received, it might be the case that other
messages, of non-odometry type, have been received in the time
interval between these two timestamps. For instance, a Duckiebot
might be seen by a watchtower at two timestamps time_stamp1 and
time_stamp2 s.t. old_time_stamp < time_stamp1 < time_stamp2 <
new_time_stamp. This function creates edges in the graph between each
pair of vertices at consecutive timestamps (e.g.
old_time_stamp-->time_stamp1, time_stamp1-->time_stamp2,
time_stamp2-->new_time_stamp). The relative transform between each
pair of vertices is assigned by performing a linear interpolation in
the Lie algebra, based on the transform between old_time_stamp and
new_time_stamp (contained in the odometry message) and on the
relative time difference (e.g. time_stamp2 - time_stamp1).
Args:
old_time_stamp: Timestamp of the last odometry message that was
received before the current odometry message, for
the node of type node_type and ID node_id.
new_time_stamp: Timestamp of the current odometry message.
node_type: Type of the node.
node_id: ID of the node.
measure: Transform contained in the current odometry message,
between timestamp old_time_stamp and time_stamp
new_time_stamp.
"""
# Timestamps for which the interpolation should be performed. Note: also
# old_time_stamp and new_time_stamp are included.
with self.node_lock:
to_interpolate = {
time_stamp
for time_stamp in self.time_stamps_to_indices if
(time_stamp >= old_time_stamp and time_stamp <= new_time_stamp)
}
# Sort the time stamps.
sorted_time_stamps = sorted(to_interpolate)
# print("perfoming interpolation on %d nodes" % len(sorted_time_stamps))
# Find the total time (time between the last and the first timestamp).
total_delta_t = float(sorted_time_stamps[-1] - sorted_time_stamps[0])
if (total_delta_t == 0.0):
print("in interpolate, delta t is 0.0, with %s and list is:" %
self.node_id)
print(to_interpolate)
print("new_time_stamp is %f and old time stamp is %f" %
(old_time_stamp, new_time_stamp))
print(self.time_stamps_to_indices)
# Perform a linear interpolation in the Lie algebra associated to SE3
# group defined by the transform.
R = measure.R
t = measure.t
q = g.SE3_from_rotation_translation(R, t)
vel = g.SE3.algebra_from_group(q)
cumulative_alpha = 0.0
interpolation_list = []
for i in range(0, len(sorted_time_stamps) - 1):
# Find the time interval between each timestamp and the subsequent
# one and linearly interpolate accordingly in the algebra.
partial_delta_t = float(sorted_time_stamps[i + 1] -
sorted_time_stamps[i])
alpha = partial_delta_t / total_delta_t
cumulative_alpha += alpha
rel = g.SE3.group_from_algebra(vel * alpha)
newR, newt = g.rotation_translation_from_SE3(rel)
interpolated_measure = g2o.Isometry3d(newR, newt)
vertex0_index = self.get_g2o_index(sorted_time_stamps[i])
vertex1_index = self.get_g2o_index(sorted_time_stamps[i + 1])
# Add an edge to the graph, connecting each timestamp to the one
# following it and using the relative transform obtained by
# interpolation.
interpolation_list.append(
(vertex0_index, vertex1_index, interpolated_measure, 0.1))
if (cumulative_alpha != 1.0):
pass
for args in interpolation_list:
self.duckietown_graph.graph.add_edge(*args)