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))