def se3_comp_over_timesteps(inputs, initial_pose):
with tf.variable_scope("se3_comp_over_timesteps"):
# position + orientation in quat
poses = []
pose = initial_pose
fc_ypr_poses = tf.unstack(inputs[:, 0:6],
axis=0) # take the x, y, z, y, p, r
for d_ypr_pose in fc_ypr_poses:
pose = se3.se3_comp(pose, d_ypr_pose)
poses.append(pose)
return tf.stack(poses)
def se3_comp_over_timesteps(fc_timesteps):
with tf.variable_scope("se3_comp_over_timesteps"):
# position + orientation in quat
initial_pose = tf.constant([0, 0, 0, 1, 0, 0, 0], tf.float32)
poses = []
pose = initial_pose
fc_ypr_poses = tf.unstack(fc_timesteps[:, 0:6],
axis=0) # take the x, y, z, y, p, r
for d_ypr_pose in fc_ypr_poses:
pose = se3.se3_comp(pose, d_ypr_pose)
poses.append(pose)
return tf.stack(poses)