def h_imu(self, u):
"""
Transforms the imu measurement (gyro, acc) in pre-integrated measurement
:param u: imu measurements, shape [k, 6]
:return: pre-integrated measurement
"""
delta_R_prev = torch.eye(3)
delta_v_prev = torch.zeros(3)
delta_p_prev = torch.zeros(3)
self.J = torch.zeros(u.shape[0], 9, 8)
for k in range(u.shape[0]):
self.J[k, :3, :3] = delta_R_prev * self.delta_t
self.J[
k,
3:6, :3] = -delta_R_prev.mm(self.skew(u[k, 3:])) * self.delta_t
self.J[k, 3:6, 3:6] = delta_R_prev * self.delta_t
self.J[k, 3:6, :3] = -1 / 2 * delta_R_prev.mm(self.skew(
u[k, 3:])) * (self.delta_t**2)
self.J[k, 6:9, 3:6] = 1 / 2 * delta_R_prev * (self.delta_t**2)
delta_R = delta_R_prev.mm(
SO3.exp(u[k, :3] * self.delta_t).as_matrix())
delta_v = delta_v_prev + delta_R.mv(u[k, 3:] * self.delta_t)
delta_p = delta_p_prev + delta_v * self.delta_t + delta_R.mv(
u[k, 3:] * self.delta_t) * (self.delta_t**2) / 2
delta_R_prev = SO3.from_matrix(delta_R, normalize=True).as_matrix()
delta_v_prev = delta_v
delta_p_prev = delta_p
return torch.cat((SO3.from_matrix(delta_R).log(), delta_v, delta_p), 0)
def test_from_matrix():
C_good = SO3.from_matrix(torch.eye(3))
assert isinstance(C_good, SO3) \
and C_good.mat.dim() == 2 \
and C_good.mat.shape == (3, 3) \
and SO3.is_valid_matrix(C_good.mat).all()
C_bad = SO3.from_matrix(torch.eye(3).add_(1e-3), normalize=True)
assert isinstance(C_bad, SO3) \
and C_bad.mat.dim() == 2 \
and C_bad.mat.shape == (3, 3) \
and SO3.is_valid_matrix(C_bad.mat).all()
def test_from_matrix_batch():
C_good = SO3.from_matrix(torch.eye(3).repeat(5, 1, 1))
assert isinstance(C_good, SO3) \
and C_good.mat.dim() == 3 \
and C_good.mat.shape == (5, 3, 3) \
and SO3.is_valid_matrix(C_good.mat).all()
C_bad = copy.deepcopy(C_good.mat)
C_bad[3].add_(0.1)
C_bad = SO3.from_matrix(C_bad, normalize=True)
assert isinstance(C_bad, SO3) \
and C_bad.mat.dim() == 3 \
and C_bad.mat.shape == (5, 3, 3) \
and SO3.is_valid_matrix(C_bad.mat).all()
def h_hat(self, u):
delta_R_prev = torch.eye(3).repeat(u.shape[0], 1, 1)
delta_v_prev = torch.zeros(3).repeat(u.shape[0], 1)
delta_p_prev = torch.zeros(3).repeat(u.shape[0], 1)
for k in range(u.shape[1]):
delta_R = delta_R_prev.matmul(
SO3.exp(u[:, k, :3] * self.delta_t).as_matrix())
delta_v = delta_v_prev + bmv(delta_R, u[:, k, 3:]) * self.delta_t
delta_p = delta_p_prev + delta_v * self.delta_t + bmv(
delta_R, u[:, k, 3:] * self.delta_t) * (self.delta_t**2) / 2
delta_R_prev = SO3.from_matrix(delta_R, normalize=True).as_matrix()
delta_v_prev = delta_v
delta_p_prev = delta_p
return torch.cat((SO3.from_matrix(delta_R).log(), delta_v, delta_p), 1)
def correct(self, x, u_odo, u_fog, compute_G=False, full_cov=False):
u_odo_fog = torch.cat((u_odo, u_fog), 1).unsqueeze(0)
u_odo_fog.requires_grad = True
Xnew = self.normalize(u_odo_fog)
# take mean to speed up correction
y_cor_nor, _ = self.gp_f.forward(Xnew, full_cov)
# # sample corrections and take mean
# N = 100
# mean, cov = self.gp_f.forward(Xnew, full_cov=True)
# y_cor_nor = torch.zeros(6)
# dist = torch.distributions.MultivariateNormal(loc=mean, cov)
# for i in range(N):
# y_cor_nor += 1/N * dist.sample()
y_cor = self.unnormalize(y_cor_nor.t(), var="y_odo_fog").squeeze()
G_cor = self.correct_cov(u_odo_fog, y_cor, compute_G)
u_odo_fog.requires_grad = False
y_cor = y_cor.detach()
y_cor[[3, 4]] = 0 # pitch and roll corrections are set to 0
G_cor[[3, 4], :] = 0
Rot = SO3.from_rpy(x[3:6]).as_matrix()
# correct state
dRot_cor = SO3.exp(y_cor[3:]).as_matrix()
x[:3] = x[:3] + Rot.mv(SE3.exp(y_cor).as_matrix()[:3, 3])
x[3:6] = SO3.from_matrix(Rot.mm(dRot_cor)).to_rpy()
return x, G_cor
def se3_to_SE3(self, f2f_x, f2f_r):
batch_size, seq_size, _ = f2f_x.shape
f2g_q = torch.zeros((batch_size, seq_size, 4), dtype=f2f_x.dtype, device=f2f_x.device)
f2g_x = torch.zeros((batch_size, seq_size, 3), dtype=f2f_x.dtype, device=f2f_x.device)
for b in range(batch_size):
R_prev = torch.zeros((3, 3), dtype=f2f_x.dtype, device=f2f_x.device)
R_prev[:] = torch.eye(3, dtype=f2f_x.dtype, device=f2f_x.device)
t_prev = torch.zeros((3), dtype=f2f_x.dtype, device=f2f_x.device)
for s in range(0, seq_size):
t_cur = f2f_x[b, s]
#q_cur = spatial.euler_to_rotation_matrix (f2f_r[b, s])
w_cur = f2f_r[b, s]
R_cur = SO3.exp(w_cur).as_matrix() # spatial.quaternion_to_rotation_matrix(q_cur)
if not torch.isclose(torch.det(R_cur), torch.FloatTensor([1.]).to(self.device)).all():
raise ValueError("Det error:\nR\n{}\nq:\n{}".format(R_cur, w_cur))
t_prev = torch.matmul(R_prev, t_cur) + t_prev
R_prev = torch.matmul(R_prev, R_cur)
if not torch.isclose(torch.det(R_prev), torch.FloatTensor([1.]).to(self.device)).all():
raise ValueError("Det error:\nR\n{}".format(R_prev))
f2g_q[b, s] = SO3.from_matrix(R_prev, normalize=True).to_quaternion()
f2g_x[b, s] = t_prev
return f2g_x, f2g_q
def h_hat(self, u_odo):
def odo2speed(u):
v = 1 / 2 * (u[0] + u[1])
return torch.Tensor([
v * torch.cos(self.x_prev[5]), v * torch.sin(self.x_prev[5]), 0
])
# initial speed
v0 = odo2speed(u_odo[0])
# end speed
v_end = odo2speed(u_odo[1])
R0 = SO3.from_rpy(self.x_prev[3:6]).as_matrix()
Rend = SO3.from_rpy(self.x[3:6]).as_matrix()
p0 = self.x_prev[:3]
p_end = self.x[:3]
delta_R = SO3.from_matrix(R0.t().mm(Rend)).log()
delta_v = R0.t().mv(v_end - v0 - self.g * self.Delta_t)
delta_p = R0.t().mv(p_end - p0 - v0 * self.Delta_t - 1 / 2 * self.g *
(self.Delta_t**2))
return torch.cat((delta_R, delta_v, delta_p), 0)
def process_ground_turth(self, gts):
T_global = []
v_global = []
for gt in gts:
t = gt[0:3]
R = gt[3:12].reshape(3, 3)
T = torch.eye(4)
T[:3, 3] = t
T[:3, :3] = R
T_global.append(T)
v = gt[12:]
v_global.append(v)
state_f2f = []
for combi in self.combinations:
T_i = T_global[combi[0]]
T_i_inv = inv_SE3(T_i)
T_ip1 = T_global[combi[1]]
T_i_ip1 = torch.matmul(T_i_inv, T_ip1)
dx = T_i_ip1[:3, 3].contiguous()
dq = SO3.from_matrix(T_i_ip1[:3, :3], normalize=False).log(
) # rotation_matrix_exp_to_log(T_i_ip1[:3, :3].unsqueeze(0).contiguous()).squeeze()
if torch.isnan(dq).any() or torch.isinf(dq).any():
raise ValueError("gt-f2f:\n{}".format(dq))
#dq = quaternion_exp_to_log(dq).squeeze()
state_f2f.append(torch.cat([dx, dq]))
T_0 = T_global[0]
T_0_inv = inv_SE3(T_0)
state_f2g = []
for combi in self.combinations:
T_ip1 = T_global[combi[1]]
T_i_ip1 = torch.matmul(T_0_inv, T_ip1)
dx = T_i_ip1[:3, 3].contiguous()
dq = SO3.from_matrix(T_i_ip1[:3, :3]).to_quaternion()
state_f2g.append(torch.cat([dx, dq]))
gt_f2f = torch.stack(state_f2f).to(self.device, non_blocking=True)
gt_f2g = torch.stack(state_f2g).to(self.device, non_blocking=True)
return gt_f2f, gt_f2g
def get_filter_data(self, i):
if type(i) != int:
i = self.datasets.index(i)
pickle_dict = self[i]
t = pickle_dict['t']
chi0 = pickle_dict['chi'][0]
Rot0 = chi0[:3, :3]
angles = SO3.from_matrix(Rot0).to_rpy()
p0 = chi0[:3, 3]
u_odo_fog = pickle_dict['u_odo_fog']
y_imu = pickle_dict['u_imu']
x0 = torch.zeros(9)
x0[:3] = p0
x0[3:6] = angles
return t, x0, u_odo_fog, y_imu
def f_hat(self, u):
u_odo = u[..., :2]
u_fog = u[..., 2:]
delta_t = self.Delta_t / u_odo.shape[1]
Rot_prev = torch.eye(3).repeat(u_odo.shape[0], 1, 1)
p_prev = torch.zeros(3).repeat(u_odo.shape[0], 1)
for i in range(u_odo.shape[1]):
dRot, dp = self.integrate_odo_fog(u_odo[:, i], u_fog[:, i],
delta_t)
Rot = Rot_prev.matmul(dRot)
p = p_prev + bmv(Rot_prev, dp)
Rot_prev = SO3.from_matrix(Rot, True).as_matrix()
p_prev = p
chi = torch.eye(4).repeat(u_odo.shape[0], 1, 1)
chi[:, :3, :3] = Rot
chi[:, :3, 3] = p
return chi
