def test_J_left_SO3_inv(self):
phi = tr.tensor([-0.2, 1.5, -2])
J = torch_se3.J_left_SO3(phi)
J_inv = torch_se3.J_left_SO3_inv(phi)
self.assertTrue(tr.allclose(J.inverse(), J_inv, atol=1e-6))
self.assertTrue(
tr.allclose(torch_se3.J_left_SO3(tr.tensor([0., 0., 0.])),
tr.eye(3, 3),
atol=1e-15))
self.assertTrue(
tr.allclose(torch_se3.J_left_SO3_inv(tr.tensor([0., 0., 0.])),
tr.eye(3, 3),
atol=1e-15))
phi = tr.tensor([1e-7, -2e-7, 3e-7])
J = torch_se3.J_left_SO3(phi)
J_inv = torch_se3.J_left_SO3_inv(phi)
self.assertTrue(tr.allclose(J.inverse(), J_inv, atol=1e-10))
phi = tr.tensor([2, -0.5, -1.1])
lhs = tr.eye(3, 3) + tr.mm(torch_se3.skew3(phi),
torch_se3.J_left_SO3(phi))
rhs = torch_se3.exp_SO3(phi)
self.assertTrue(tr.allclose(lhs, rhs, atol=1e-6))
lhs = torch_se3.J_left_SO3(phi)
rhs = tr.mm(torch_se3.exp_SO3(phi), torch_se3.J_left_SO3(-phi))
self.assertTrue(tr.allclose(lhs, rhs, atol=1e-6))
def test_cuda_device_and_grad(self):
C = torch_se3.exp_SO3(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(C.requires_grad)
self.assertTrue(C.is_cuda)
C = torch_se3.exp_SO3(
tr.tensor([0.0, 0.0, 0.0], requires_grad=True).cuda())
self.assertTrue(C.requires_grad)
self.assertTrue(C.is_cuda)
phi = torch_se3.log_SO3(C)
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
phi = torch_se3.log_SO3(tr.eye(3, 3, requires_grad=True).cuda())
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
phi = torch_se3.log_SO3_eigen(C)
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
phi = torch_se3.log_SO3_eigen(tr.eye(3, 3, requires_grad=True).cuda())
self.assertTrue(phi.requires_grad)
self.assertTrue(phi.is_cuda)
v_skewed = torch_se3.skew3(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(v_skewed.requires_grad)
self.assertTrue(v_skewed.is_cuda)
v = torch_se3.unskew3(v_skewed)
self.assertTrue(v.requires_grad)
self.assertTrue(v.is_cuda)
J = torch_se3.J_left_SO3(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
J = torch_se3.J_left_SO3(
tr.tensor([0.0, 0.0, 0.0], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
J = torch_se3.J_left_SO3_inv(
tr.tensor([0.1, 0.2, 0.3], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
J = torch_se3.J_left_SO3_inv(
tr.tensor([0.0, 0.0, 0.0], requires_grad=True).cuda())
self.assertTrue(J.requires_grad)
self.assertTrue(J.is_cuda)
def test_meas_jacobian_numerically(self):
torch.set_default_tensor_type('torch.DoubleTensor')
device = "cpu"
T_imu_cam = torch.eye(4, 4)
T_imu_cam[0:3, 0:3] = torch_se3.exp_SO3(torch.tensor([1., -2., 3.]))
T_imu_cam[0:3, 3] = torch.tensor([-3., 2., 1.])
T_imu_cam = T_imu_cam.to(device).view(1, 4, 4)
ekf = IMUKalmanFilter()
C_pred = torch_se3.exp_SO3(torch.tensor([0.1, 3, -0.3
])).to(device).view(1, 3, 3)
r_pred = torch.tensor([-1.05, 20,
-1.]).view(3, 1).to(device).view(1, 3, 1)
vis_meas = torch.tensor([4., -6., 8., -7., 5.,
-9.]).to(device).view(1, 6, 1)
residual, H = ekf.meas_residual_and_jacobi(C_pred, r_pred, vis_meas,
T_imu_cam)
e = 1e-6
p = torch.eye(3, 3).view(1, 3, 3) * e
H_C_numerical = torch.zeros(1, 6, 3)
H_r_numerical = torch.zeros(1, 6, 3)
for i in range(0, 3):
pb = p[:, :, i:i + 1]
residual_minus_pb, _ = ekf.meas_residual_and_jacobi(
torch.matmul(C_pred, torch_se3.exp_SO3_b(-pb)), r_pred,
vis_meas, T_imu_cam)
residual_plus_pb, _ = ekf.meas_residual_and_jacobi(
torch.matmul(C_pred, torch_se3.exp_SO3_b(pb)), r_pred,
vis_meas, T_imu_cam)
H_C_numerical[:, :, i] = (residual_plus_pb -
residual_minus_pb).view(6) / (2 * e)
for i in range(0, 3):
pb = p[:, :, i:i + 1]
residual_minus_pb, _ = ekf.meas_residual_and_jacobi(
C_pred, r_pred - pb, vis_meas, T_imu_cam)
residual_plus_pb, _ = ekf.meas_residual_and_jacobi(
C_pred, r_pred + pb, vis_meas, T_imu_cam)
H_r_numerical[:, :, i] = (residual_plus_pb -
residual_minus_pb).view(6) / (2 * e)
self.assertTrue(torch.allclose(H_C_numerical, H[:, :, 3:6], atol=1e-7))
self.assertTrue(torch.allclose(H_r_numerical, H[:, :, 6:9], atol=1e-7))
torch.set_default_tensor_type('torch.FloatTensor')
def test_exp_SO3(self):
self.assertTrue(
tr.allclose(torch_se3.exp_SO3(tr.tensor([1., 2., 3.])),
tr.tensor(np.array(slinalg.expm(se3.skew3([1, 2, 3]))),
dtype=tr.float32),
atol=1e-8))
self.assertTrue(
tr.allclose(torch_se3.exp_SO3(tr.tensor([1e-8, 2e-8, 3e-8])),
tr.tensor(np.array(
slinalg.expm(se3.skew3([1e-8, 2e-8, 3e-8]))),
dtype=tr.float32),
atol=1e-8))
self.assertTrue(
tr.allclose(torch_se3.exp_SO3(tr.tensor([0., 0., 0.])),
tr.eye(3, 3),
atol=1e-8))
C = torch_se3.exp_SO3(tr.tensor([-1, 0.2, -0.75]))
u = tr.tensor([[1, -2, 0.1]]).transpose(0, 1)
self.assertTrue(
tr.allclose(torch_se3.skew3(tr.mm(C, u)),
tr.mm(tr.mm(C, torch_se3.skew3(u)), C.transpose(0, 1)),
atol=1e-7))
exp_Cu = torch_se3.exp_SO3(tr.mm(C, u))
C_expu_Ct = tr.mm(tr.mm(C, torch_se3.exp_SO3(u)), C.transpose(0, 1))
self.assertTrue(tr.allclose(exp_Cu, C_expu_Ct, atol=1e-7))
def test_process_model_F_G_Q_covar(self):
device = "cuda"
imu_noise = torch.eye(12, 12).to(device)
ekf = IMUKalmanFilter()
covar = torch.zeros(1, 18, 18).to(device).to(device)
for i in range(0, 5):
t_accum, C_accum, r_accum, v_accum, covar, F, G, Phi, Q = \
ekf.predict_one_step(C_accum=torch_se3.exp_SO3(torch.tensor([1., 2., 3.])).view(1, 3, 3).to(device),
r_accum=torch.tensor([-2, 0.25, -0.1]).view(1, 3, 1).to(device),
v_accum=torch.tensor([0.1, -0.1, 0.2]).view(1, 3, 1).to(device),
t_accum=torch.tensor(10.).view(1, 1, 1).to(device),
dt=torch.tensor(0.05).view(1, 1, 1).to(device),
g_k=torch.tensor([-0.1, 0.2, 11]).view(1, 3, 1).to(device),
v_k=torch.tensor([5, -2, 1.]).view(1, 3, 1).to(device),
bw_k=torch.tensor([0.10, -0.11, 0.12]).view(1, 3, 1).to(device),
ba_k=torch.tensor([-0.13, 0.14, -0.15]).view(1, 3, 1).to(device),
covar=covar,
gyro_meas=torch.tensor([1.0, -11, -1.2]).view(1, 3, 1).to(device),
accel_meas=torch.tensor([-.5, 4, 6]).view(1, 3, 1).to(device),
imu_noise_covar=imu_noise)
self.assertTrue(
torch.allclose(t_accum[-1].detach().cpu(),
torch.tensor(10.05),
atol=1e-8))
# check proper blocks to be zero
self.check_non_zero_3x3_sub_blocks(F[-1].detach().cpu(), [
(
1,
1,
),
(
1,
4,
),
(
2,
1,
),
(
2,
3,
),
(
3,
0,
),
(
3,
1,
),
(
3,
3,
),
(
3,
4,
),
(
3,
5,
),
])
self.check_non_zero_3x3_sub_blocks(G[-1].detach().cpu(), [
(
1,
0,
),
(
3,
0,
),
(
3,
2,
),
(
4,
1,
),
(
5,
3,
),
])
self.check_non_zero_3x3_sub_blocks(Phi[-1].detach().cpu(), [(
0,
0,
), (
1,
1,
), (
1,
4,
), (
2,
0,
), (
2,
1,
), (
2,
2,
), (
2,
3,
), (2, 5), (
3,
0,
), (
3,
1,
), (
3,
3,
), (
3,
4,
), (
3,
5,
), (
4,
4,
), (5, 5)])
F_np = F[-1].detach().cpu().numpy().astype(np.float64)
F_exp = scipy.linalg.expm(F_np * 0.05)
self.assertTrue(
np.allclose(Test_EKF.sub_block3x3(F_exp, 1, 1),
Test_EKF.sub_block3x3(Phi[-1], 1,
1).detach().cpu().numpy(),
atol=1e-7))
self.assertTrue(
np.allclose(Test_EKF.sub_block3x3(F_exp, 3, 3),
Test_EKF.sub_block3x3(Phi[-1], 3,
3).detach().cpu().numpy(),
atol=1e-7))
# check symmetrical
self.assertTrue(
torch.allclose(covar[-1], covar[-1].transpose(0, 1),
atol=1e-9))
# no correlation between gravity and any other stuff (yet)
self.assertTrue(
torch.allclose(covar[-1][0:3, 0:18].detach().cpu(),
torch.zeros(3, 18),
atol=0,
rtol=0))
self.assertTrue(
torch.allclose(covar[-1][0:18, 0:3].detach().cpu(),
torch.zeros(18, 3),
atol=0,
rtol=0))
# no correlation between rotation and accelerometer bias
self.assertTrue(
torch.allclose(covar[-1][15:18, 3:6].detach().cpu(),
torch.zeros(3, 3),
atol=0,
rtol=0))
self.assertTrue(
torch.allclose(covar[-1][3:6, 15:18].detach().cpu(),
torch.zeros(3, 3),
atol=0,
rtol=0))
def exp_SO3_over_batch2(phis):
Cs = []
for i in range(phis.shape[0]):
Cs.append(torch_se3.exp_SO3(phis[i]))
return torch.stack(Cs)
def test_simple_grad(self):
v = tr.tensor([0.1, 0.2, 0.3], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.log_SO3(C)
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))
v = tr.tensor([0.0, 0.0, 0.0], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.log_SO3(C)
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))
v = tr.tensor([0.1, 0.2, 0.3], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.unskew3(
tr.mm(C - tr.eye(3, 3), torch_se3.J_left_SO3_inv(v)))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))
v = tr.tensor([0.1, 0.2, 0.3], requires_grad=True)
C = torch_se3.exp_SO3(v)
v2 = torch_se3.unskew3(
tr.mm(C - tr.eye(3, 3),
torch_se3.J_left_SO3(v).inverse()))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[0], v, retain_graph=True)[0],
tr.tensor([1., 0., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[1], v, retain_graph=True)[0],
tr.tensor([0., 1., 0.]),
atol=1e-7))
self.assertTrue(
tr.allclose(tr.autograd.grad(v2[2], v, retain_graph=True)[0],
tr.tensor([0., 0., 1.]),
atol=1e-7))