def test_dot():
C = SO3(torch.Tensor([[0, -1, 0], [1, 0, 0], [0, 0, 1]]))
pt = torch.Tensor([1, 2, 3])
CC = C.mat.mm(C.mat)
assert utils.allclose(C.dot(C).mat, CC)
Cpt = C.mat.matmul(pt)
assert utils.allclose(C.dot(pt), Cpt)
def test_left_jacobian():
xi1 = torch.Tensor([1, 2, 3])
assert utils.allclose(
torch.mm(SE2.left_jacobian(xi1), SE2.inv_left_jacobian(xi1)),
torch.eye(3))
xi2 = torch.Tensor([0, 0, 0])
assert utils.allclose(
torch.mm(SE2.left_jacobian(xi2), SE2.inv_left_jacobian(xi2)),
torch.eye(3))
def test_left_jacobians():
phi_small = torch.Tensor([0., 0., 0.])
phi_big = torch.Tensor([np.pi / 2, np.pi / 3, np.pi / 4])
left_jacobian_small = SO3.left_jacobian(phi_small)
inv_left_jacobian_small = SO3.inv_left_jacobian(phi_small)
assert utils.allclose(
torch.mm(left_jacobian_small, inv_left_jacobian_small), torch.eye(3))
left_jacobian_big = SO3.left_jacobian(phi_big)
inv_left_jacobian_big = SO3.inv_left_jacobian(phi_big)
assert utils.allclose(torch.mm(left_jacobian_big, inv_left_jacobian_big),
torch.eye(3))
def test_perturb_batch():
C = SO2.exp(torch.Tensor([-1., 0., 1.]))
C_copy1 = copy.deepcopy(C)
C_copy2 = copy.deepcopy(C)
phi = torch.Tensor([0.1])
C_copy1.perturb(phi)
assert utils.allclose(C_copy1.as_matrix(),
(SO2.exp(phi).dot(C)).as_matrix())
phis = torch.Tensor([0.1, 0.2, 0.3])
C_copy2.perturb(phis)
assert utils.allclose(C_copy2.as_matrix(),
(SO2.exp(phis).dot(C)).as_matrix())
def test_perturb_batch():
C = SO3.exp(torch.Tensor([[1, 2, 3], [4, 5, 6]]))
C_copy1 = copy.deepcopy(C)
C_copy2 = copy.deepcopy(C)
phi = torch.Tensor([0.1, 0.2, 0.3])
C_copy1.perturb(phi)
assert utils.allclose(C_copy1.as_matrix(),
(SO3.exp(phi).dot(C)).as_matrix())
phis = torch.Tensor([[0.1, 0.2, 0.3], [0.4, 0.5, 0.6]])
C_copy2.perturb(phis)
assert utils.allclose(C_copy2.as_matrix(),
(SO3.exp(phis).dot(C)).as_matrix())
def test_perturb_batch():
T = SE2.exp(0.1 * torch.Tensor([[1, 2, 3], [4, 5, 6]]))
T_copy1 = copy.deepcopy(T)
T_copy2 = copy.deepcopy(T)
xi = torch.Tensor([0.3, 0.2, 0.1])
T_copy1.perturb(xi)
assert utils.allclose(T_copy1.as_matrix(),
(SE2.exp(xi).dot(T)).as_matrix())
xis = torch.Tensor([[0.3, 0.2, 0.1], [-0.1, -0.2, -0.3]])
T_copy2.perturb(xis)
assert utils.allclose(T_copy2.as_matrix(),
(SE2.exp(xis).dot(T)).as_matrix())
def test_perturb():
C = SO3.exp(0.25 * np.pi * torch.ones(3))
C_copy = copy.deepcopy(C)
phi = torch.Tensor([0.1, 0.2, 0.3])
C.perturb(phi)
assert utils.allclose(C.as_matrix(),
(SO3.exp(phi).dot(C_copy)).as_matrix())
def test_left_jacobians_batch():
phis = torch.Tensor([[0., 0., 0.], [np.pi / 2, np.pi / 3, np.pi / 4]])
left_jacobian = SO3.left_jacobian(phis)
inv_left_jacobian = SO3.inv_left_jacobian(phis)
assert utils.allclose(torch.bmm(left_jacobian, inv_left_jacobian),
torch.eye(3).unsqueeze_(dim=0).expand(2, 3, 3))
def test_left_jacobian_batch():
xis = torch.Tensor([[1, 2, 3, 4, 5, 6],
[0, 0, 0, 0, 0, 0]])
assert utils.allclose(
SE3.left_jacobian(xis).bmm(SE3.inv_left_jacobian(xis)),
torch.eye(6).unsqueeze_(dim=0).expand(2, 6, 6)
)
def test_left_jacobians_batch():
phis = torch.Tensor([0., np.pi / 2])
left_jacobian = SO2.left_jacobian(phis)
inv_left_jacobian = SO2.inv_left_jacobian(phis)
assert utils.allclose(torch.bmm(left_jacobian, inv_left_jacobian),
torch.eye(2).unsqueeze_(dim=0).expand(2, 2, 2))
def test_perturb():
C = SO2.exp(torch.Tensor([np.pi / 4]))
C_copy = copy.deepcopy(C)
phi = torch.Tensor([0.1])
C.perturb(phi)
assert utils.allclose(
C.as_matrix(), (SO2.exp(phi).dot(C_copy)).as_matrix())
def test_rotz_batch():
C_got = SO3.rotz(torch.Tensor([np.pi / 2, np.pi]))
C_expected = torch.cat([
torch.Tensor([[0, -1, 0], [1, 0, 0], [0, 0, 1]]).unsqueeze_(dim=0),
torch.Tensor([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]).unsqueeze_(dim=0)
],
dim=0)
assert utils.allclose(C_got.mat, C_expected)
def test_dot():
T = torch.Tensor([[0, -1, -0.5], [1, 0, 0.5], [0, 0, 1]])
T_SE2 = SE2.from_matrix(T)
pt = torch.Tensor([1, 2])
pth = torch.Tensor([1, 2, 1])
TT = torch.mm(T, T)
TT_SE2 = T_SE2.dot(T_SE2).as_matrix()
assert utils.allclose(TT_SE2, TT)
Tpt = torch.matmul(T[0:2, 0:2], pt) + T[0:2, 2]
Tpt_SE2 = T_SE2.dot(pt)
assert utils.allclose(Tpt_SE2, Tpt)
Tpth = torch.matmul(T, pth)
Tpth_SE2 = T_SE2.dot(pth)
assert utils.allclose(Tpth_SE2, Tpth) and \
utils.allclose(Tpth_SE2[0:2], Tpt)
def test_dot():
T = torch.Tensor([[0, 0, -1, 0.1],
[0, 1, 0, 0.5],
[1, 0, 0, -0.5],
[0, 0, 0, 1]])
T_SE3 = SE3.from_matrix(T)
pt = torch.Tensor([1, 2, 3])
pth = torch.Tensor([1, 2, 3, 1])
TT = torch.mm(T, T)
TT_SE3 = T_SE3.dot(T_SE3).as_matrix()
assert utils.allclose(TT_SE3, TT)
Tpt = torch.matmul(T[0:3, 0:3], pt) + T[0:3, 3]
Tpt_SE3 = T_SE3.dot(pt)
assert utils.allclose(Tpt_SE3, Tpt)
Tpth = torch.matmul(T, pth)
Tpth_SE3 = T_SE3.dot(pth)
assert utils.allclose(Tpth_SE3, Tpth) and \
utils.allclose(Tpth_SE3[0:3], Tpt)
def from_quaternion(cls, quat, ordering='wxyz'):
"""Form a rotation matrix from a unit length quaternion.
Valid orderings are 'xyzw' and 'wxyz'.
"""
if quat.dim() < 2:
quat = quat.unsqueeze(dim=0)
if not utils.allclose(quat.norm(p=2, dim=1), 1.):
raise ValueError("Quaternions must be unit length")
if ordering is 'xyzw':
qx = quat[:, 0]
qy = quat[:, 1]
qz = quat[:, 2]
qw = quat[:, 3]
elif ordering is 'wxyz':
qw = quat[:, 0]
qx = quat[:, 1]
qy = quat[:, 2]
qz = quat[:, 3]
else:
raise ValueError(
"Valid orderings are 'xyzw' and 'wxyz'. Got '{}'.".format(
ordering))
# Form the matrix
mat = quat.__class__(quat.shape[0], cls.dim, cls.dim)
qw2 = qw * qw
qx2 = qx * qx
qy2 = qy * qy
qz2 = qz * qz
mat[:, 0, 0] = 1. - 2. * (qy2 + qz2)
mat[:, 0, 1] = 2. * (qx * qy - qw * qz)
mat[:, 0, 2] = 2. * (qw * qy + qx * qz)
mat[:, 1, 0] = 2. * (qw * qz + qx * qy)
mat[:, 1, 1] = 1. - 2. * (qx2 + qz2)
mat[:, 1, 2] = 2. * (qy * qz - qw * qx)
mat[:, 2, 0] = 2. * (qx * qz - qw * qy)
mat[:, 2, 1] = 2. * (qw * qx + qy * qz)
mat[:, 2, 2] = 1. - 2. * (qx2 + qy2)
return cls(mat.squeeze_())
def test_quaternion():
q1 = torch.Tensor([1, 0, 0, 0])
q2 = torch.Tensor([0, 1, 0, 0])
q3 = torch.Tensor([0, 0, 1, 0])
q4 = torch.Tensor([0, 0, 0, 1])
q5 = 0.5 * torch.ones(4)
q6 = -q5
assert utils.allclose(SO3.from_quaternion(q1).to_quaternion(), q1)
assert utils.allclose(SO3.from_quaternion(q2).to_quaternion(), q2)
assert utils.allclose(SO3.from_quaternion(q3).to_quaternion(), q3)
assert utils.allclose(SO3.from_quaternion(q4).to_quaternion(), q4)
assert utils.allclose(SO3.from_quaternion(q5).to_quaternion(), q5)
assert utils.allclose(
SO3.from_quaternion(q5).mat,
SO3.from_quaternion(q6).mat)
def test_from_angle_to_angle():
angle = torch.Tensor([np.pi / 2.])
assert utils.allclose(SO2.from_angle(angle).to_angle(), angle)
def test_inv_batch():
C = SO2.exp(torch.Tensor([-1., 0., 1.]))
assert utils.allclose(C.dot(C.inv()).mat, SO2.identity(C.mat.shape[0]).mat)
def test_inv():
C = SO2.exp(torch.Tensor([np.pi / 4]))
assert utils.allclose(C.dot(C.inv()).mat, SO2.identity().mat)
def test_exp_log():
T = SE2.exp(torch.Tensor([1, 2, 3]))
assert utils.allclose(SE2.exp(SE2.log(T)).as_matrix(), T.as_matrix())
def test_exp_log_batch():
T = SE2.exp(0.1 * torch.Tensor([[1, 2, 3], [4, 5, 6]]))
assert utils.allclose(SE2.exp(SE2.log(T)).as_matrix(), T.as_matrix())
def test_exp_log_batch():
C = SO2.exp(torch.Tensor([-1., 0., 1.]))
assert utils.allclose(SO2.exp(SO2.log(C)).mat, C.mat)
def test_exp_log():
C_big = SO2.exp(torch.Tensor([np.pi / 4]))
assert utils.allclose(SO2.exp(SO2.log(C_big)).mat, C_big.mat)
C_small = SO2.exp(torch.Tensor([0]))
assert utils.allclose(SO2.exp(SO2.log(C_small)).mat, C_small.mat)
def test_perturb():
T = SE2.exp(torch.Tensor([1, 2, 3]))
T_copy = copy.deepcopy(T)
xi = torch.Tensor([0.3, 0.2, 0.1])
T.perturb(xi)
assert utils.allclose(T.as_matrix(), (SE2.exp(xi).dot(T_copy)).as_matrix())
def test_dot_batch():
T1 = torch.Tensor([[0, -1, -0.5], [1, 0, 0.5], [0, 0, 1]]).expand(5, 3, 3)
T2 = torch.Tensor([[0, -1, -0.5], [1, 0, 0.5], [0, 0, 1]])
T1_SE2 = SE2.from_matrix(T1)
T2_SE2 = SE2.from_matrix(T2)
pt1 = torch.Tensor([1, 2])
pt2 = torch.Tensor([4, 5])
pt3 = torch.Tensor([7, 8])
pts = torch.cat(
[pt1.unsqueeze(dim=0),
pt2.unsqueeze(dim=0),
pt3.unsqueeze(dim=0)],
dim=0) # 3x2
ptsbatch = pts.unsqueeze(dim=0).expand(5, 3, 2)
pt1h = torch.Tensor([1, 2, 1])
pt2h = torch.Tensor([4, 5, 1])
pt3h = torch.Tensor([7, 8, 1])
ptsh = torch.cat(
[pt1h.unsqueeze(dim=0),
pt2h.unsqueeze(dim=0),
pt3h.unsqueeze(dim=0)],
dim=0) # 3x3
ptshbatch = ptsh.unsqueeze(dim=0).expand(5, 3, 3)
T1T1 = torch.bmm(T1, T1)
T1T1_SE2 = T1_SE2.dot(T1_SE2).as_matrix()
assert T1T1_SE2.shape == T1.shape and utils.allclose(T1T1_SE2, T1T1)
T1T2 = torch.matmul(T1, T2)
T1T2_SE2 = T1_SE2.dot(T2_SE2).as_matrix()
assert T1T2_SE2.shape == T1.shape and utils.allclose(T1T2_SE2, T1T2)
T1pt1 = torch.matmul(T1[:, 0:2, 0:2], pt1) + T1[:, 0:2, 2]
T1pt1_SE2 = T1_SE2.dot(pt1)
assert T1pt1_SE2.shape == (T1.shape[0], pt1.shape[0]) \
and utils.allclose(T1pt1_SE2, T1pt1)
T1pt1h = torch.matmul(T1, pt1h)
T1pt1h_SE2 = T1_SE2.dot(pt1h)
assert T1pt1h_SE2.shape == (T1.shape[0], pt1h.shape[0]) \
and utils.allclose(T1pt1h_SE2, T1pt1h) \
and utils.allclose(T1pt1h_SE2[:, 0:2], T1pt1_SE2)
T1pt2 = torch.matmul(T1[:, 0:2, 0:2], pt2) + T1[:, 0:2, 2]
T1pt2_SE2 = T1_SE2.dot(pt2)
assert T1pt2_SE2.shape == (T1.shape[0], pt2.shape[0]) \
and utils.allclose(T1pt2_SE2, T1pt2)
T1pt2h = torch.matmul(T1, pt2h)
T1pt2h_SE2 = T1_SE2.dot(pt2h)
assert T1pt2h_SE2.shape == (T1.shape[0], pt2h.shape[0]) \
and utils.allclose(T1pt2h_SE2, T1pt2h) \
and utils.allclose(T1pt2h_SE2[:, 0:2], T1pt2_SE2)
T1pts = torch.bmm(T1[:, 0:2, 0:2],
pts.unsqueeze(dim=0).expand(
T1.shape[0],
pts.shape[0],
pts.shape[1]).transpose(2, 1)).transpose(2, 1) + \
T1[:, 0:2, 2].unsqueeze(dim=1).expand(
T1.shape[0], pts.shape[0], pts.shape[1])
T1pts_SE2 = T1_SE2.dot(pts)
assert T1pts_SE2.shape == (T1.shape[0], pts.shape[0], pts.shape[1]) \
and utils.allclose(T1pts_SE2, T1pts) \
and utils.allclose(T1pt1, T1pts[:, 0, :]) \
and utils.allclose(T1pt2, T1pts[:, 1, :])
T1ptsh = torch.bmm(
T1,
ptsh.unsqueeze(dim=0).expand(T1.shape[0], ptsh.shape[0],
ptsh.shape[1]).transpose(2, 1)).transpose(
2, 1)
T1ptsh_SE2 = T1_SE2.dot(ptsh)
assert T1ptsh_SE2.shape == (T1.shape[0], ptsh.shape[0], ptsh.shape[1]) \
and utils.allclose(T1ptsh_SE2, T1ptsh) \
and utils.allclose(T1pt1h, T1ptsh[:, 0, :]) \
and utils.allclose(T1pt2h, T1ptsh[:, 1, :]) \
and utils.allclose(T1ptsh_SE2[:, :, 0:2], T1pts_SE2)
T1ptsbatch = torch.bmm(T1[:, 0:2, 0:2],
ptsbatch.transpose(2, 1)).transpose(2, 1) + \
T1[:, 0:2, 2].unsqueeze(dim=1).expand(ptsbatch.shape)
T1ptsbatch_SE2 = T1_SE2.dot(ptsbatch)
assert T1ptsbatch_SE2.shape == ptsbatch.shape \
and utils.allclose(T1ptsbatch_SE2, T1ptsbatch) \
and utils.allclose(T1pt1, T1ptsbatch[:, 0, :]) \
and utils.allclose(T1pt2, T1ptsbatch[:, 1, :])
T1ptshbatch = torch.bmm(T1, ptshbatch.transpose(2, 1)).transpose(2, 1)
T1ptshbatch_SE2 = T1_SE2.dot(ptshbatch)
assert T1ptshbatch_SE2.shape == ptshbatch.shape \
and utils.allclose(T1ptshbatch_SE2, T1ptshbatch) \
and utils.allclose(T1pt1h, T1ptshbatch[:, 0, :]) \
and utils.allclose(T1pt2h, T1ptshbatch[:, 1, :]) \
and utils.allclose(T1ptshbatch_SE2[:, :, 0:2], T1ptsbatch_SE2)
T2ptsbatch = torch.matmul(T2[0:2, 0:2],
ptsbatch.transpose(2, 1)).transpose(2, 1) + \
T1[:, 0:2, 2].unsqueeze(dim=1).expand(ptsbatch.shape)
T2ptsbatch_SE2 = T2_SE2.dot(ptsbatch)
assert T2ptsbatch_SE2.shape == ptsbatch.shape \
and utils.allclose(T2ptsbatch_SE2, T2ptsbatch) \
and utils.allclose(T2_SE2.dot(pt1), T2ptsbatch[:, 0, :]) \
and utils.allclose(T2_SE2.dot(pt2), T2ptsbatch[:, 1, :])
T2ptshbatch = torch.matmul(T2, ptshbatch.transpose(2, 1)).transpose(2, 1)
T2ptshbatch_SE2 = T2_SE2.dot(ptshbatch)
assert T2ptshbatch_SE2.shape == ptshbatch.shape \
and utils.allclose(T2ptshbatch_SE2, T2ptshbatch) \
and utils.allclose(T2_SE2.dot(pt1h), T2ptshbatch[:, 0, :]) \
and utils.allclose(T2_SE2.dot(pt2h), T2ptshbatch[:, 1, :]) \
and utils.allclose(T2ptshbatch_SE2[:, :, 0:2], T2ptsbatch_SE2)
def test_exp_log():
T = SE2.exp(torch.Tensor([1, 2, 3]))
print(T.trans)
print(T.rot.to_angle())
assert utils.allclose(SE2.exp(SE2.log(T)).as_matrix(), T.as_matrix())
def test_inv_batch():
T = SE2.exp(0.1 * torch.Tensor([[1, 2, 3], [4, 5, 6], [7, 8, 9]]))
assert utils.allclose(
T.dot(T.inv()).as_matrix(),
SE2.identity(T.trans.shape[0]).as_matrix())
def test_from_angle_to_angle_batch():
angles = torch.Tensor([-1., 0, 1.])
assert utils.allclose(SO2.from_angle(angles).to_angle(), angles)
def test_dot_batch():
C1 = SO2(torch.Tensor([[0, -1],
[1, 0]]).expand(5, 2, 2))
C2 = SO2(torch.Tensor([[-1, 0],
[0, -1]]))
pt1 = torch.Tensor([1, 2])
pt2 = torch.Tensor([4, 5])
pt3 = torch.Tensor([7, 8])
pts = torch.cat([pt1.unsqueeze(dim=0),
pt2.unsqueeze(dim=0),
pt3.unsqueeze(dim=0)], dim=0) # 3x2
ptsbatch = pts.unsqueeze(dim=0).expand(5, 3, 2)
C1C1 = torch.bmm(C1.mat, C1.mat)
C1C1_SO2 = C1.dot(C1).mat
assert C1C1_SO2.shape == C1.mat.shape and utils.allclose(C1C1_SO2, C1C1)
C1C2 = torch.matmul(C1.mat, C2.mat)
C1C2_SO2 = C1.dot(C2).mat
assert C1C2_SO2.shape == C1.mat.shape and utils.allclose(C1C2_SO2, C1C2)
C1pt1 = torch.matmul(C1.mat, pt1)
C1pt1_SO2 = C1.dot(pt1)
assert C1pt1_SO2.shape == (C1.mat.shape[0], pt1.shape[0]) \
and utils.allclose(C1pt1_SO2, C1pt1)
C1pt2 = torch.matmul(C1.mat, pt2)
C1pt2_SO2 = C1.dot(pt2)
assert C1pt2_SO2.shape == (C1.mat.shape[0], pt2.shape[0]) \
and utils.allclose(C1pt2_SO2, C1pt2)
C1pts = torch.matmul(C1.mat, pts.transpose(1, 0)).transpose(2, 1)
C1pts_SO2 = C1.dot(pts)
assert C1pts_SO2.shape == (C1.mat.shape[0], pts.shape[0], pts.shape[1]) \
and utils.allclose(C1pts_SO2, C1pts) \
and utils.allclose(C1pt1, C1pts[:, 0, :]) \
and utils.allclose(C1pt2, C1pts[:, 1, :])
C1ptsbatch = torch.bmm(C1.mat, ptsbatch.transpose(2, 1)).transpose(2, 1)
C1ptsbatch_SO2 = C1.dot(ptsbatch)
assert C1ptsbatch_SO2.shape == ptsbatch.shape \
and utils.allclose(C1ptsbatch_SO2, C1ptsbatch) \
and utils.allclose(C1pt1, C1ptsbatch[:, 0, :]) \
and utils.allclose(C1pt2, C1ptsbatch[:, 1, :])
C2ptsbatch = torch.matmul(C2.mat, ptsbatch.transpose(2, 1)).transpose(2, 1)
C2ptsbatch_SO2 = C2.dot(ptsbatch)
assert C2ptsbatch_SO2.shape == ptsbatch.shape \
and utils.allclose(C2ptsbatch_SO2, C2ptsbatch) \
and utils.allclose(C2.dot(pt1), C2ptsbatch[:, 0, :]) \
and utils.allclose(C2.dot(pt2), C2ptsbatch[:, 1, :])
def test_inv():
T = SE2.exp(torch.Tensor([1, 2, 3]))
assert utils.allclose((T.dot(T.inv())).as_matrix(), torch.eye(3))
