π Geometric Computer Vision Library for Spatial AI
Python artificial-intelligencecomputer-visiondeep-learninghacktoberfestimage-processingmachine-learningneural-networkpythonpytorchroboticsspatial-ai
# LICENSE HEADER MANAGED BY add-license-header
#
# Copyright 2018 Kornia Team
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
import math
import pytest
import torch
from kornia.core.exceptions import TypeCheckError
from kornia.geometry import NamedPose
from kornia.geometry.liegroup import Se2, Se3, So2, So3
from kornia.geometry.quaternion import Quaternion
from testing.base import BaseTester
class TestNamedPose(BaseTester):
def test_smoke(self, device, dtype):
b_from_a = Se3.identity(device=device, dtype=dtype)
pose = NamedPose(b_from_a, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(pose, NamedPose)
assert isinstance(pose.pose, Se3)
@pytest.mark.skip(reason="not implemented yet")
def test_cardinality(self, device, dtype):
pass
@pytest.mark.skip(reason="not implemented yet")
def test_jit(self, device, dtype):
pass
@pytest.mark.skip(reason="not implemented yet")
def test_exception(self, device, dtype):
pass
@pytest.mark.skip(reason="not implemented yet")
def test_module(self, device, dtype):
pass
@pytest.mark.skip(reason="not implemented yet")
def test_gradcheck(self, device):
pass
def test_mul(self, device, dtype):
b_from_a = NamedPose(
Se3.trans_x(torch.tensor([1.0], device=device, dtype=dtype)), frame_src="frame_a", frame_dst="frame_b"
)
c_from_b = NamedPose(
Se3.trans_y(torch.tensor([1.0], device=device, dtype=dtype)), frame_src="frame_b", frame_dst="frame_c"
)
c_from_a = c_from_b * b_from_a
assert isinstance(c_from_a, NamedPose)
assert isinstance(c_from_a.pose, Se3)
assert c_from_a.frame_src == "frame_a"
assert c_from_a.frame_dst == "frame_c"
def test_from_rt(self, device, dtype):
b_from_a_rotation = So3.random(device=device, dtype=dtype)
b_from_a_translation = torch.tensor([1.0, 2.0, 3.0], device=device, dtype=dtype)
b_from_a = NamedPose.from_rt(b_from_a_rotation, b_from_a_translation, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(b_from_a, NamedPose)
assert isinstance(b_from_a.pose, Se3)
b_from_a_rotation = torch.eye(3, device=device, dtype=dtype)
b_from_a_translation = torch.tensor([1.0, 2.0, 3.0], device=device, dtype=dtype)
b_from_a = NamedPose.from_rt(b_from_a_rotation, b_from_a_translation, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(b_from_a, NamedPose)
assert isinstance(b_from_a.pose, Se3)
# the Se2 poses below cannot be built in bfloat16; test_se2_pose_in_bfloat16_raises pins that error
if dtype == torch.bfloat16:
pytest.skip("torch has no complex bfloat16 dtype, which So2 stores its rotation in")
b_from_a_rotation = So2.random(device=device, dtype=dtype)
b_from_a_translation = torch.tensor([1.0, 2.0], device=device, dtype=dtype)
b_from_a = NamedPose.from_rt(b_from_a_rotation, b_from_a_translation, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(b_from_a, NamedPose)
assert isinstance(b_from_a.pose, Se2)
b_from_a_rotation = torch.eye(2, device=device, dtype=dtype)
b_from_a_translation = torch.tensor([1.0, 2.0], device=device, dtype=dtype)
b_from_a = NamedPose.from_rt(b_from_a_rotation, b_from_a_translation, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(b_from_a, NamedPose)
assert isinstance(b_from_a.pose, Se2)
@pytest.mark.parametrize("batch_size", [None, 1, 2])
def test_from_rt_tensor_batch_3d(self, device, dtype, batch_size):
batch_shape = () if batch_size is None else (batch_size,)
rotation = So3.random(batch_size, device=device, dtype=dtype).matrix()
translation = torch.rand(*batch_shape, 3, device=device, dtype=dtype)
b_from_a = NamedPose.from_rt(rotation, translation, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(b_from_a.pose, Se3)
assert b_from_a.pose.matrix().shape == (*batch_shape, 4, 4)
assert b_from_a.translation.shape == (*batch_shape, 3)
matrix = torch.eye(4, device=device, dtype=dtype).repeat(*batch_shape, 1, 1)
matrix[..., :3, :3] = rotation
matrix[..., :3, 3] = translation
self.assert_close(b_from_a.pose.matrix(), NamedPose.from_matrix(matrix).pose.matrix())
self.assert_close(
b_from_a.pose.matrix(), NamedPose.from_rt(So3.from_matrix(rotation), translation).pose.matrix()
)
@pytest.mark.parametrize("batch_size", [None, 1, 2])
def test_from_rt_tensor_batch_2d(self, device, dtype, batch_size):
if dtype == torch.bfloat16:
pytest.skip("torch.complex has no bfloat16 overload, so So2 cannot be built at all")
batch_shape = () if batch_size is None else (batch_size,)
rotation = So2.random(batch_size, device=device, dtype=dtype).matrix()
translation = torch.rand(*batch_shape, 2, device=device, dtype=dtype)
b_from_a = NamedPose.from_rt(rotation, translation, frame_src="frame_a", frame_dst="frame_b")
assert isinstance(b_from_a.pose, Se2)
assert b_from_a.pose.matrix().shape == (*batch_shape, 3, 3)
assert b_from_a.translation.shape == (*batch_shape, 2)
matrix = torch.eye(3, device=device, dtype=dtype).repeat(*batch_shape, 1, 1)
matrix[..., :2, :2] = rotation
matrix[..., :2, 2] = translation
self.assert_close(b_from_a.pose.matrix(), NamedPose.from_matrix(matrix).pose.matrix())
self.assert_close(
b_from_a.pose.matrix(), NamedPose.from_rt(So2.from_matrix(rotation), translation).pose.matrix()
)
@pytest.mark.parametrize(
("rotation_shape", "translation_shape"),
[
((2, 3, 3), (1, 3)),
((2, 3, 3), (3,)),
((1, 3, 3), (3,)),
((3, 3), (1, 3)),
((2, 2, 2), (2,)),
((2, 2), (1, 2)),
],
)
def test_from_rt_tensor_batch_translation_mismatch(self, device, dtype, rotation_shape, translation_shape):
# The translation must carry exactly the rotation's batch shape, as with So3/So2 rotations: neither a
# broadcast translation nor a batched translation for an unbatched rotation is accepted.
rotation = torch.eye(rotation_shape[-1], device=device, dtype=dtype).expand(*rotation_shape)
translation = torch.zeros(*translation_shape, device=device, dtype=dtype)
with pytest.raises(ValueError, match="translation must have shape"):
NamedPose.from_rt(rotation, translation)
def test_from_matrix(self, device, dtype):
b_from_a_matrix = Se3.identity(device=device, dtype=dtype).matrix()
b_from_a = NamedPose.from_matrix(b_from_a_matrix, frame_src="frame_a", frame_dst="frame_b")
point_in_a = torch.tensor([1.0, 2.0, 3.0], device=device, dtype=dtype)
point_in_b = b_from_a.transform_points(point_in_a)
self.assert_close(point_in_b, point_in_a)
assert isinstance(b_from_a, NamedPose)
assert isinstance(b_from_a.pose, Se3)
# the Se2 pose below cannot be built in bfloat16; test_se2_pose_in_bfloat16_raises pins that error
if dtype == torch.bfloat16:
pytest.skip("torch has no complex bfloat16 dtype, which So2 stores its rotation in")
b_from_a_matrix = torch.eye(3, device=device, dtype=dtype)
b_from_a = NamedPose.from_matrix(b_from_a_matrix, frame_src="frame_a", frame_dst="frame_b")
point_in_a = torch.tensor([1.0, 2.0], device=device, dtype=dtype)
point_in_b = b_from_a.transform_points(point_in_a)
self.assert_close(point_in_b, point_in_a)
assert isinstance(b_from_a, NamedPose)
assert isinstance(b_from_a.pose, Se2)
def test_se2_pose_in_bfloat16_raises(self, device):
# So2 stores a complex rotation and PyTorch has no complex bfloat16 dtype, so a bfloat16 Se2 pose raises
# kornia's error naming the dtype (torch.complex raised a RuntimeError before). A pose built in float32 and
# converted with ``.bfloat16()`` keeps its complex rotation and composes as usual.
bf16 = torch.bfloat16
with pytest.raises(TypeCheckError, match=r"no complex bfloat16"):
NamedPose.from_rt(torch.eye(2, device=device, dtype=bf16), torch.ones(2, device=device, dtype=bf16))
with pytest.raises(TypeCheckError, match=r"no complex bfloat16"):
NamedPose.from_matrix(torch.eye(3, device=device, dtype=bf16))
converted = Se2.exp(torch.tensor([0.5, -1.0, 0.3], device=device)).bfloat16()
b_from_a = NamedPose(converted, frame_src="a", frame_dst="b")
assert b_from_a.translation.dtype == bf16
assert isinstance((b_from_a.inverse() * b_from_a).pose, Se2)
def test_wrong_matrix_shape_message_6422(self, device, dtype):
# The message used to print [2, 2]x[2, 2] and carry a run of spaces from a backslash continuation.
with pytest.raises(ValueError) as excinfo:
NamedPose.from_rt(torch.eye(4, device=device, dtype=dtype), torch.zeros(4, device=device, dtype=dtype))
assert str(excinfo.value) == "R must be either [2, 2] or [3, 3], got torch.Size([4, 4])"
with pytest.raises(ValueError) as excinfo:
NamedPose.from_matrix(torch.eye(5, device=device, dtype=dtype))
assert str(excinfo.value) == "RT must be either [3, 3] or [4, 4], got torch.Size([5, 5])"
def test_inverse(self, device, dtype):
b_from_a = NamedPose(
Se3.trans_x(torch.tensor([1.0], device=device, dtype=dtype)), frame_src="frame_a", frame_dst="frame_b"
)
a_from_b = b_from_a.inverse()
assert isinstance(a_from_b, NamedPose)
assert isinstance(a_from_b.pose, Se3)
assert a_from_b.frame_src == "frame_b"
assert a_from_b.frame_dst == "frame_a"
@pytest.mark.parametrize("batch_size", [None, 1, 2, 5])
def test_transform_points(self, device, dtype, batch_size):
if batch_size is None:
points_in_a = torch.randn(3, device=device, dtype=dtype)
b_from_a_se3 = Se3.rot_z(torch.tensor(0.5, device=device, dtype=dtype)) * Se3.trans_x(
torch.tensor(1.0, device=device, dtype=dtype)
)
else:
points_in_a = torch.randn(batch_size, 3, device=device, dtype=dtype)
b_from_a_se3 = Se3.rot_z(torch.tensor([0.5] * batch_size, device=device, dtype=dtype)) * Se3.trans_x(
torch.tensor([1.0] * batch_size, device=device, dtype=dtype)
)
b_from_a = NamedPose(b_from_a_se3, frame_src="frame_a", frame_dst="frame_b")
a_from_b = b_from_a.inverse()
points_in_b = b_from_a.transform_points(points_in_a)
assert points_in_b.shape == points_in_a.shape
# b_from_a = Rz(0.5) * Tx(1): translate by +1 along x, then rotate 0.5 rad about z. In half precision the
# rotation is rounded, and the round trip applies two rounded rotations: about one ulp at |p| ~ 2.
low_tolerance = dtype in (torch.float16, torch.bfloat16)
c, s = math.cos(0.5), math.sin(0.5)
rot_z = torch.tensor([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], device=device, dtype=dtype)
shift_x = torch.tensor([1.0, 0.0, 0.0], device=device, dtype=dtype)
self.assert_close(points_in_b, (points_in_a + shift_x) @ rot_z.T, low_tolerance=low_tolerance)
self.assert_close(a_from_b.transform_points(points_in_b), points_in_a, low_tolerance=low_tolerance)
def test_transform_points_point_set_with_unbatched_se3(self, device, dtype):
# #6426: an unbatched 3D pose maps an (N, 3) point set, as an unbatched 2D pose maps (N, 2).
torch.manual_seed(0)
points_in_a = (2 * torch.rand(7, 3) - 1).to(device, dtype)
b_from_a_se3 = Se3.rot_z(torch.tensor(0.5, device=device, dtype=dtype)) * Se3.trans_x(
torch.tensor(1.0, device=device, dtype=dtype)
)
b_from_a = NamedPose(b_from_a_se3, frame_src="frame_a", frame_dst="frame_b")
points_in_b = b_from_a.transform_points(points_in_a)
# R p + t in float64, on the CPU, from the same rounded pose and points: only the output is rounded
r = So3(Quaternion(b_from_a_se3.so3.q.data.cpu().double())).matrix()
expected = points_in_a.cpu().double() @ r.mT + b_from_a_se3.t.cpu().double()
self.assert_close(points_in_b.cpu(), expected.to(dtype))
class TestNamedPoseConventions(BaseTester):
@staticmethod
def _poses(device, dtype):
# Two rigid, non-commuting poses (M1 M2 != M2 M1 by 0.854), neither the identity nor a pure translation.
g1 = Se3.exp(torch.tensor([1.0, -2.0, 3.0, 0.4, 0.2, -0.3], device=device, dtype=dtype))
g2 = Se3.exp(torch.tensor([0.3, 0.1, -0.2, -0.1, 0.3, 0.05], device=device, dtype=dtype))
m1, m2 = g1.matrix(), g2.matrix()
assert (m1 @ m2 - m2 @ m1).abs().max() > 0.5
eye = torch.eye(3, device=device, dtype=dtype)
assert (m1[:3, :3] - eye).abs().max() > 0.1
assert (m2[:3, :3] - eye).abs().max() > 0.1
return g1, g2
def test_convention_named_pose_composition_order(self, device, dtype):
g1, g2 = self._poses(device, dtype)
# Unit rotations from exp, so the expected matrix below is a product of rotations.
self.assert_close(g1.r.q.norm(), torch.ones((), device=device, dtype=dtype))
self.assert_close(g2.r.q.norm(), torch.ones((), device=device, dtype=dtype))
b_from_a = NamedPose(g1, frame_src="a", frame_dst="b")
c_from_b = NamedPose(g2, frame_src="b", frame_dst="c")
# c_from_b * b_from_a is c_from_a, whose matrix is M2 @ M1; generated in float64 as
# Se3.exp([0.3, 0.1, -0.2, -0.1, 0.3, 0.05]).matrix() @ Se3.exp([1.0, -2.0, 3.0, 0.4, 0.2, -0.3]).matrix()
expected = torch.tensor(
[
[0.8365770733327115, 0.3564824932541303, 0.41600364466840206, 1.991550772972204],
[-0.23936674171706684, 0.9208664847598445, -0.30774742924320647, -2.2686923473188667],
[-0.4927903847822421, 0.15787700671524751, 0.85570584163999, 2.0241534574197697],
[0.0, 0.0, 0.0, 1.0],
],
device=device,
dtype=dtype,
)
c_from_a = c_from_b * b_from_a
assert (c_from_a.frame_src, c_from_a.frame_dst) == ("a", "c")
self.assert_close(c_from_a.pose.matrix(), expected)
# The source frame of the left operand must be the destination frame of the right one.
with pytest.raises(ValueError, match="Cannot compose"):
b_from_a * c_from_b
def test_convention_named_pose_inverse_swaps_frames(self, device, dtype):
g1, _ = self._poses(device, dtype)
a_from_b = NamedPose(g1, frame_src="a", frame_dst="b").inverse()
assert (a_from_b.frame_src, a_from_b.frame_dst) == ("b", "a")
# generated in float64 as torch.linalg.inv(Se3.exp([1.0, -2.0, 3.0, 0.4, 0.2, -0.3]).matrix())
expected = torch.tensor(
[
[0.9365557269934554, -0.24666617456316428, -0.2490364803841689, -0.893226697341087],
[0.32475143364814213, 0.8779917826797222, 0.35166309998400447, 1.2022440482882328],
[0.13190859175670216, -0.41022704429773776, 0.9023934261437778, -3.3894728975959616],
[0.0, 0.0, 0.0, 1.0],
],
device=device,
dtype=dtype,
)
# half precision: the quaternion inverse and the matrix rebuild round twice (up to 0.7 of the default
# half tolerance on this fixture)
low = dtype in (torch.float16, torch.bfloat16)
self.assert_close(a_from_b.pose.matrix(), expected, low_tolerance=low)
def test_convention_named_pose_se2_composition_and_inverse(self, device, dtype):
if dtype == torch.bfloat16:
pytest.skip("torch.complex has no bfloat16 overload")
# The Se2 half of the block: two non-commuting planar poses (M1 M2 != M2 M1 by 0.611), neither the identity.
g1 = Se2.exp(torch.tensor([0.5, -1.0, 0.3], device=device, dtype=dtype))
g2 = Se2.exp(torch.tensor([-0.2, 0.4, -0.7], device=device, dtype=dtype))
m1, m2 = g1.matrix(), g2.matrix()
assert (m1 @ m2 - m2 @ m1).abs().max() > 0.5
b_from_a = NamedPose(g1, frame_src="a", frame_dst="b")
c_from_b = NamedPose(g2, frame_src="b", frame_dst="c")
c_from_a = c_from_b * b_from_a
assert (c_from_a.frame_src, c_from_a.frame_dst) == ("a", "c")
# generated in float64 as Se2.exp([-0.2, 0.4, -0.7]).matrix() @ Se2.exp([0.5, -1.0, 0.3]).matrix()
expected = torch.tensor(
[
[0.9210609940028851, 0.3894183423086504, -0.14575008421958802],
[-0.3894183423086504, 0.9210609940028851, -0.6743834884300649],
[0.0, 0.0, 1.0],
],
device=device,
dtype=dtype,
)
self.assert_close(c_from_a.pose.matrix(), expected)
with pytest.raises(ValueError, match="Cannot compose"):
b_from_a * c_from_b
a_from_b = b_from_a.inverse()
assert (a_from_b.frame_src, a_from_b.frame_dst) == ("b", "a")
# generated in float64 as torch.linalg.inv(Se2.exp([0.5, -1.0, 0.3]).matrix())
expected_inv = torch.tensor(
[
[0.955336489125606, 0.29552020666133955, -0.34365530818758616],
[-0.29552020666133955, 0.9553364891256061, 1.0595065403284554],
[0.0, 0.0, 1.0],
],
device=device,
dtype=dtype,
)
self.assert_close(a_from_b.pose.matrix(), expected_inv)
def test_named_pose_rejects_wrong_types_4937(self, device, dtype):
if dtype == torch.bfloat16:
pytest.skip("torch.complex has no bfloat16 overload, so So2 (and an Se2 pose) cannot be built at all")
# https://github.com/kornia/kornia/issues/4937: the pose type is checked at construction, and composing poses
# of different groups, or a NamedPose with something else, raises a TypeError that names the offending type.
g1, _ = self._poses(device, dtype)
se2 = Se2.exp(torch.tensor([0.5, -1.0, 0.3], device=device, dtype=dtype))
for bad, name in (
(So3.identity(device=device, dtype=dtype), "so3.So3"),
(So2.identity(device=device, dtype=dtype), "so2.So2"),
(Quaternion.identity(device=device, dtype=dtype), "quaternion.Quaternion"),
(g1.matrix(), "torch.Tensor"),
):
with pytest.raises(TypeCheckError, match=rf"expected Se2 \| Se3, got <class '.*{name}'>"):
NamedPose(bad, frame_src="a", frame_dst="b")
c_from_b = NamedPose(g1, frame_src="b", frame_dst="c")
with pytest.raises(TypeError, match="Cannot compose an Se3 pose with an Se2 pose"):
_ = c_from_b * NamedPose(se2, frame_src="a", frame_dst="b")
with pytest.raises(TypeError, match="Cannot compose an Se2 pose with an Se3 pose"):
_ = NamedPose(se2, frame_src="b", frame_dst="c") * NamedPose(g1, frame_src="a", frame_dst="b")
with pytest.raises(TypeError, match=r"got <class 'kornia.geometry.liegroup.se3.Se3'>"):
_ = c_from_b * g1
with pytest.raises(TypeError, match=r"got <class 'torch.Tensor'>"):
_ = c_from_b * torch.ones(1, 3, device=device, dtype=dtype)
# A subclass of the group composes with the group, on either side.
class _Se3(Se3):
pass
sub_c_from_b = NamedPose(_Se3(g1.r, g1.t), frame_src="b", frame_dst="c")
sub_b_from_a = NamedPose(_Se3(g1.r, g1.t), frame_src="a", frame_dst="b")
self.assert_close((c_from_b * sub_b_from_a).pose.matrix(), g1.matrix() @ g1.matrix())
self.assert_close((sub_c_from_b * NamedPose(g1, "a", "b")).pose.matrix(), g1.matrix() @ g1.matrix())
# Two NamedPoses with mismatched frames still fail on the frames first.
with pytest.raises(ValueError, match="Cannot compose"):
_ = c_from_b * NamedPose(se2, frame_src="a", frame_dst="x")
# from_rt keeps accepting rotation-only groups, since it wraps them in an Se2 or an Se3 itself.
assert isinstance(NamedPose.from_rt(So3.identity(device=device, dtype=dtype), g1.t).pose, Se3)
assert isinstance(NamedPose.from_rt(So2.identity(device=device, dtype=dtype), se2.t).pose, Se2)