def predict_pose(self,t_idx):
if t_idx == 0:
self.poses_pred[:,:,t_idx] = Transform.calcualte_pose(np.eye(3),
np.zeros(3))
else:
u = self.imu.get_linear_angular_velocity(t_idx)
twist = Transform.calculate_twist(u)
self.poses_pred[:,:,t_idx] = self.poses_pred[:,:,t_idx-1] @ expm(self.imu.delta_t * twist)
return self.poses_pred[:,:,t_idx]
