Skip to content

🐍 Geometric Computer Vision Library for Spatial AI

fix(geometry): let an unbatched So3/Se3 transform a point set (#6445)

Debanjan Sarkarebdc3d6c006d
diff --git a/changelog.d/6445.fixed.md b/changelog.d/6445.fixed.md
new file mode 100644
index 000000000..3f49db57b
--- /dev/null
+++ b/changelog.d/6445.fixed.md
@@ -0,0 +1 @@
+`So3 * points` and `Se3 * points`, and so `NamedPose.transform_points` for a 3D pose, now accept an `(N, 3)` point set with an unbatched pose, as `Se2` does for `(N, 2)`. They used to raise in `torch.linalg.cross`. A point layout outside the contract of either group, such as a batch of poses with a point set `(B, N, 3)` or `(B, N, 2)`, or `B` poses with `N` points, now raises a `ValueError` that names the accepted layouts, instead of a `torch.linalg.cross` or broadcast error, or `Se2`'s "Invalid translation shape".
diff --git a/kornia/geometry/liegroup/se2.py b/kornia/geometry/liegroup/se2.py
index b12d28e9a..cf9fc9313 100644
--- a/kornia/geometry/liegroup/se2.py
+++ b/kornia/geometry/liegroup/se2.py
@@ -185,7 +185,8 @@ class Se2(nn.Module):
"""Compose two Se2 transformations, or transform points.
Args:
- right: the other Se2 transformation, or points of shape ``(B, 2)`` or ``(2,)``.
+ right: the other Se2 transformation, or points: ``(2,)`` or ``(N, 2)`` for an unbatched pose,
+ ``(B, 2)`` or ``(2,)`` for a batch of ``B`` (a batch of one also maps ``(N, 2)``).
Return:
The resulting Se2 transformation, or the transformed points.
diff --git a/kornia/geometry/liegroup/se3.py b/kornia/geometry/liegroup/se3.py
index ecdf60daf..1c8d7d19c 100644
--- a/kornia/geometry/liegroup/se3.py
+++ b/kornia/geometry/liegroup/se3.py
@@ -28,7 +28,7 @@ from torch import nn
from kornia.core.check import KORNIA_CHECK, KORNIA_CHECK_SAME_DEVICES, KORNIA_CHECK_SHAPE
from kornia.core.exceptions import DeviceError, TypeCheckError
from kornia.core.utils import register_module_state
-from kornia.geometry.liegroup.so3 import So3, _so3_rotate, _so3_small_angle_coefficients
+from kornia.geometry.liegroup.so3 import So3, _check_points_layout, _so3_rotate, _so3_small_angle_coefficients
from kornia.geometry.linalg import batched_dot_product
from kornia.geometry.quaternion import Quaternion
from kornia.geometry.vector import Vector3
@@ -174,8 +174,8 @@ class Se3(nn.Module):
Both compute in float32 for float16/bfloat16 inputs and return the input dtype.
Args:
- right: the other Se3 transformation, or points as a tensor or ``Vector3`` with the batch shape of the
- pose and a last dimension of 3.
+ right: the other Se3 transformation, or points as a tensor or ``Vector3``: ``(3,)`` or ``(N, 3)`` for
+ an unbatched pose, ``(B, 3)`` for a batch of ``B`` (a batch of one also maps ``(N, 3)``).
Return:
The resulting Se3 transformation, or the transformed points.
@@ -189,6 +189,7 @@ class Se3(nn.Module):
if isinstance(right, (Vector3, torch.Tensor)):
_right_data = right if isinstance(right, torch.Tensor) else right.data
KORNIA_CHECK_SHAPE(_right_data, ["*", "N"])
+ _check_points_layout(so3.q, _right_data)
# R p + t is rounded once, so float16/bfloat16 add the translation to the float32 rotated point
out = (_so3_rotate(so3.q, _right_data) + t).to(torch.promote_types(t.dtype, _right_data.dtype))
return Vector3(out) if isinstance(right, Vector3) else out
diff --git a/kornia/geometry/liegroup/so2.py b/kornia/geometry/liegroup/so2.py
index a36d886a6..e1523f5a2 100644
--- a/kornia/geometry/liegroup/so2.py
+++ b/kornia/geometry/liegroup/so2.py
@@ -155,8 +155,9 @@ class So2(nn.Module):
"""Perform a left-multiplication either rotation concatenation or point-transform.
Args:
- right: the other So2 transformation, or points of shape ``(B, 2)`` or ``(2,)`` as a tensor or
- a ``Vector2``.
+ right: the other So2 transformation, or points as a tensor or a ``Vector2``: ``(2,)`` or ``(N, 2)``
+ for an unbatched rotation, ``(B, 2)`` or ``(2,)`` for a batch of ``B`` (a batch of one also maps
+ ``(N, 2)``).
Return:
The resulting So2 transformation, or the rotated points with the type of ``right``.
@@ -167,11 +168,18 @@ class So2(nn.Module):
return So2(z * right.z)
if isinstance(right, (Vector2, torch.Tensor)):
if isinstance(right, torch.Tensor):
- # check_so2_t_shape
+ # (2,) under any pose, (N, 2) under an unbatched pose, and one point per pose for a batch
is_batch_shape = KORNIA_CHECK_SHAPE(right, ["B", "2"], raises=False)
is_single_shape = KORNIA_CHECK_SHAPE(right, ["2"], raises=False)
+ if is_batch_shape and z.dim() == 1:
+ is_batch_shape = right.shape[0] in (z.shape[0], 1) or z.shape[0] == 1
if not (is_batch_shape or is_single_shape):
- raise ValueError(f"Invalid translation shape, we expect [B, 2], or [2] Got: {right.shape}")
+ raise ValueError(
+ "Expected points of shape (2,) or (N, 2) for an unbatched pose, or (B, 2), one point per "
+ "pose, for a batch of B poses (a batch of one pose also maps (N, 2), and one point (2,) or "
+ f"(1, 2) maps under every pose). Got points of shape {tuple(right.shape)} for a pose of batch "
+ f"shape {tuple(z.shape)}."
+ )
right_data = _unwrap(right)
x = right_data[..., 0]
y = right_data[..., 1]
diff --git a/kornia/geometry/liegroup/so3.py b/kornia/geometry/liegroup/so3.py
index 979ca466d..fc790b68a 100644
--- a/kornia/geometry/liegroup/so3.py
+++ b/kornia/geometry/liegroup/so3.py
@@ -85,6 +85,9 @@ def _so3_rotate(q: Quaternion, points: torch.Tensor) -> torch.Tensor:
KORNIA_CHECK_SHAPE(points, ["*", "3"])
if _computes_in_float32(torch.promote_types(q.data.dtype, points.dtype)):
q, points = q.to(torch.float32), points.float()
+ if q.data.dim() == 1 and points.dim() == 2:
+ # an unbatched rotation rotates every point of an (N, 3) point set, as So2 does for (N, 2)
+ q = Quaternion(q.data.expand(points.shape[0], 4))
w = torch.zeros(*points.shape[:-1], 1, device=points.device, dtype=points.dtype)
quat = Quaternion(torch.cat((w, points), -1))
# q p q* scales p by |q|^2, so rotate by the unit direction of q
@@ -92,6 +95,27 @@ def _so3_rotate(q: Quaternion, points: torch.Tensor) -> torch.Tensor:
return (q * quat * q.conj()).vec
+def _check_points_layout(q: Quaternion, points: torch.Tensor) -> None:
+ """Raise unless So3 and Se3 map ``points``: ``(3,)`` or ``(N, 3)`` unbatched, one point per pose for a batch.
+
+ For a batch of ``B`` poses the points are ``(B, 3)``; a batch of one pose also maps ``(N, 3)``, and one point
+ ``(1, 3)`` maps under every pose.
+ """
+ batch = q.data.shape[:-1]
+ if len(batch) == 0:
+ accepted = points.dim() in (1, 2)
+ else:
+ accepted = points.dim() == len(batch) + 1 and all(
+ p in (b, 1) or b == 1 for p, b in zip(points.shape[:-1], batch)
+ )
+ if not accepted:
+ raise ValueError(
+ "Expected points of shape (3,) or (N, 3) for an unbatched pose, or (B, 3), one point per pose, for a batch "
+ "of B poses (a batch of one pose also maps (N, 3), and one point (1, 3) maps under every pose). Got points "
+ f"of shape {tuple(points.shape)} for a pose of batch shape {tuple(batch)}."
+ )
+
+
class So3(nn.Module):
r"""Base class to represent the So3 group.
@@ -107,8 +131,10 @@ class So3(nn.Module):
``q.to_axis_angle()``: it reads only the direction of ``q``, within the range limits stated on
:class:`~kornia.geometry.quaternion.Quaternion`, and returns the principal vector, of norm at most
:math:`\pi`, the same for ``q`` and ``-q`` below a half turn.
- - ``a * b`` composes like ``a.matrix() @ b.matrix()``, so ``b`` acts first; ``s * p`` rotates points ``p`` of
- shape ``(B, 3)``, a tensor or a ``Vector3``, as :math:`R p`; and ``adjoint()`` is :math:`R` itself.
+ - ``a * b`` composes like ``a.matrix() @ b.matrix()``, so ``b`` acts first; ``s * p`` rotates points ``p``, a
+ tensor or a ``Vector3``, as :math:`R p`: ``(3,)`` or ``(N, 3)`` for an unbatched ``So3``, and ``(B, 3)``,
+ one point per rotation, for a batch of ``B`` (a batch of one also maps ``(N, 3)``); other layouts raise
+ ``ValueError``. ``adjoint()`` is :math:`R` itself.
:ref:`Rotations and rigid motions <rotation-conventions>` compares these with scipy, Sophus and Eigen.
- A non-unit quaternion stands for the rotation of its direction :math:`q / |q|`, as for
:meth:`~kornia.geometry.quaternion.Quaternion.matrix`: ``matrix()`` and ``s * p`` normalise it where they
@@ -174,7 +200,8 @@ class So3(nn.Module):
Both compute in float32 for float16/bfloat16 inputs and return the input dtype.
Args:
- right: the other So3 transformation, or points of shape ``(B, 3)`` as a tensor or a ``Vector3``.
+ right: the other So3 transformation, or points as a tensor or a ``Vector3``: ``(3,)`` or ``(N, 3)``
+ for an unbatched rotation, ``(B, 3)`` for a batch of ``B`` (a batch of one also maps ``(N, 3)``).
Return:
The resulting So3 transformation, or the rotated points of the type of ``right``.
@@ -189,6 +216,7 @@ class So3(nn.Module):
return So3(self.q * right.q)
if isinstance(right, (torch.Tensor, Vector3)):
_right_data = _unwrap(right)
+ _check_points_layout(self.q, _right_data)
out = _so3_rotate(self.q, _right_data).to(torch.promote_types(self.q.data.dtype, _right_data.dtype))
return Vector3(out) if isinstance(right, Vector3) else out
raise TypeError(f"Not So3 or torch.Tensor type. Got: {type(right)}")
diff --git a/kornia/geometry/pose.py b/kornia/geometry/pose.py
index 172ec5f2a..19db06536 100644
--- a/kornia/geometry/pose.py
+++ b/kornia/geometry/pose.py
@@ -57,10 +57,10 @@ class NamedPose:
``b_from_a = NamedPose(pose, frame_src="a", frame_dst="b")`` wraps an :class:`~kornia.geometry.liegroup.Se3` or
:class:`~kornia.geometry.liegroup.Se2` pose that maps frame ``a`` to frame ``b``;
``b_from_a.transform_points(points)`` maps points from ``a`` to ``b``, and ``c_from_b * b_from_a`` composes two
- poses into ``c_from_a`` after checking that the frames chain. An unbatched ``Se3`` maps one point ``(3,)`` and a
- batch of ``B`` maps ``(B, 3)``, one point per pose (a batch of one also maps ``(N, 3)``); a point set ``(N, 3)``
- with an unbatched ``Se3`` raises (`#6426 <https://github.com/kornia/kornia/issues/6426>`_). An unbatched ``Se2``
- maps ``(2,)`` or ``(N, 2)``, and a batch of ``B`` maps ``(B, 2)``. Build one from a rotation and a translation with
+ poses into ``c_from_a`` after checking that the frames chain. An unbatched ``Se3`` maps ``(3,)`` or ``(N, 3)``,
+ and a batch of ``B`` maps ``(B, 3)``, one point per pose (a batch of one also maps ``(N, 3)``); an unbatched
+ ``Se2`` maps ``(2,)`` or ``(N, 2)``, and a batch of ``B`` maps ``(B, 2)``. Other layouts, such as a batch of poses
+ with a point set ``(B, N, 3)``, raise ``ValueError``. Build one from a rotation and a translation with
:meth:`from_rt`, or from a ``3 x 3`` or ``4 x 4`` matrix with :meth:`from_matrix`. For a pose without frame names,
use the Lie group classes directly; for ``4 x 4`` matrices,
:func:`~kornia.geometry.linalg.compose_transformations`.
diff --git a/tests/geometry/liegroup/test_se2.py b/tests/geometry/liegroup/test_se2.py
index 66dd12cc1..4396650ac 100644
--- a/tests/geometry/liegroup/test_se2.py
+++ b/tests/geometry/liegroup/test_se2.py
@@ -894,3 +894,32 @@ class TestSe2(BaseTester):
assert t.min() >= 0, (t.min(), t.max())
assert t.max() <= 1, (t.min(), t.max())
assert 0.45 < t.mean() < 0.55, t.mean()
+
+ # #6426: a pose maps (2,) or (N, 2) unbatched and one point per pose for a batch; other layouts, such as a
+ # batch of poses with a point set, raise a ValueError that names the accepted layouts.
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape"), [(None, (2, 5, 2)), (2, (7, 2)), (2, (2, 7, 2)), (1, (1, 7, 2))]
+ )
+ def test_point_layout_outside_the_contract_raises(self, batch_size, points_shape, device, dtype):
+ if dtype == torch.bfloat16:
+ pytest.skip("torch has no complex bfloat16 dtype, which So2 stores its rotation in")
+ pose = Se2.identity(batch_size, device, dtype)
+ with pytest.raises(ValueError, match=r"Expected points of shape \(2,\) or \(N, 2\)"):
+ pose * torch.zeros(points_shape, device=device, dtype=dtype)
+
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape", "out_shape"),
+ [
+ (None, (2,), (2,)),
+ (None, (7, 2), (7, 2)),
+ (1, (7, 2), (7, 2)),
+ (2, (2, 2), (2, 2)),
+ (2, (1, 2), (2, 2)),
+ (2, (2,), (2, 2)),
+ ],
+ )
+ def test_point_layout_in_the_contract_maps(self, batch_size, points_shape, out_shape, device, dtype):
+ if dtype == torch.bfloat16:
+ pytest.skip("torch has no complex bfloat16 dtype, which So2 stores its rotation in")
+ pose = Se2.identity(batch_size, device, dtype)
+ assert (pose * torch.zeros(points_shape, device=device, dtype=dtype)).shape == out_shape
diff --git a/tests/geometry/liegroup/test_se3.py b/tests/geometry/liegroup/test_se3.py
index 5113aec83..12e472126 100644
--- a/tests/geometry/liegroup/test_se3.py
+++ b/tests/geometry/liegroup/test_se3.py
@@ -192,6 +192,40 @@ class TestSe3(BaseTester):
self._assert_chains_agree(pt_in_s1, pt_in_s2_in_s1, pt_in_world, *translations)
self._assert_chains_agree(pt_in_s2, pt_in_s1_in_s2, pt_in_world, *translations)
+ @pytest.mark.parametrize("points_shape", [(7, 3), (1, 3)])
+ def test_unbatched_pose_transforms_a_point_set(self, points_shape, device, dtype):
+ # #6426: an unbatched Se3 maps every point of a point set, as Se2 does for (N, 2); it used to raise in
+ # torch.linalg.cross. Expected: R p + t for each point.
+ torch.manual_seed(0)
+ g = Se3.exp(torch.tensor([0.3, -0.5, 0.2, 1.0, -2.0, 0.5], device=device, dtype=dtype))
+ p = (2 * torch.rand(points_shape) - 1).to(device, dtype)
+
+ out = g * p
+
+ assert out.shape == p.shape
+ self.assert_close(out, torch.stack([g * q for q in p.reshape(-1, 3)]).reshape(points_shape))
+ # R p + t in float64, on the CPU, from the same rounded pose and points: only the output is rounded
+ r = So3(Quaternion(g.so3.q.data.cpu().double())).matrix()
+ self.assert_close(out.cpu(), (p.cpu().double() @ r.mT + g.t.cpu().double()).to(dtype))
+
+ # #6426: a pose maps (3,) or (N, 3) unbatched and one point per pose for a batch; other layouts, such as a
+ # batch of poses with a point set, raise a ValueError that names the accepted layouts.
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape"), [(None, (2, 5, 3)), (2, (3,)), (2, (7, 3)), (2, (2, 7, 3)), (1, (1, 7, 3))]
+ )
+ def test_point_layout_outside_the_contract_raises(self, batch_size, points_shape, device, dtype):
+ pose = Se3.identity(batch_size, device, dtype)
+ with pytest.raises(ValueError, match=r"Expected points of shape \(3,\) or \(N, 3\)"):
+ pose * torch.zeros(points_shape, device=device, dtype=dtype)
+
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape", "out_shape"),
+ [(None, (3,), (3,)), (None, (7, 3), (7, 3)), (1, (7, 3), (7, 3)), (2, (2, 3), (2, 3)), (2, (1, 3), (2, 3))],
+ )
+ def test_point_layout_in_the_contract_maps(self, batch_size, points_shape, out_shape, device, dtype):
+ pose = Se3.identity(batch_size, device, dtype)
+ assert (pose * torch.zeros(points_shape, device=device, dtype=dtype)).shape == out_shape
+
@pytest.mark.parametrize("batch_size", [None, 1, 2, 5])
def test_mul_vector(self, device, dtype, batch_size):
world_pose_s1: Se3 = self._make_rand_se3d(device, dtype, batch_size)
diff --git a/tests/geometry/liegroup/test_so2.py b/tests/geometry/liegroup/test_so2.py
index 90a1a2686..b9c855009 100644
--- a/tests/geometry/liegroup/test_so2.py
+++ b/tests/geometry/liegroup/test_so2.py
@@ -548,6 +548,35 @@ class TestSo2(BaseTester):
(rotation * point).sum().backward()
self.assert_close(angle.grad, -3 * angle.detach().sin() - angle.detach().cos())
+ # #6426: a pose maps (2,) or (N, 2) unbatched and one point per pose for a batch; other layouts, such as a
+ # batch of poses with a point set, raise a ValueError that names the accepted layouts.
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape"), [(None, (2, 5, 2)), (2, (7, 2)), (2, (2, 7, 2)), (1, (1, 7, 2))]
+ )
+ def test_point_layout_outside_the_contract_raises(self, batch_size, points_shape, device, dtype):
+ if dtype == torch.bfloat16:
+ pytest.skip("torch has no complex bfloat16 dtype, which So2 stores its rotation in")
+ pose = So2.identity(batch_size, device, dtype)
+ with pytest.raises(ValueError, match=r"Expected points of shape \(2,\) or \(N, 2\)"):
+ pose * torch.zeros(points_shape, device=device, dtype=dtype)
+
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape", "out_shape"),
+ [
+ (None, (2,), (2,)),
+ (None, (7, 2), (7, 2)),
+ (1, (7, 2), (7, 2)),
+ (2, (2, 2), (2, 2)),
+ (2, (1, 2), (2, 2)),
+ (2, (2,), (2, 2)),
+ ],
+ )
+ def test_point_layout_in_the_contract_maps(self, batch_size, points_shape, out_shape, device, dtype):
+ if dtype == torch.bfloat16:
+ pytest.skip("torch has no complex bfloat16 dtype, which So2 stores its rotation in")
+ pose = So2.identity(batch_size, device, dtype)
+ assert (pose * torch.zeros(points_shape, device=device, dtype=dtype)).shape == out_shape
+
class _So2PointTransform(torch.nn.Module):
def __init__(self, rotation: So2) -> None:
diff --git a/tests/geometry/liegroup/test_so3.py b/tests/geometry/liegroup/test_so3.py
index e32f4935d..51c837f3b 100644
--- a/tests/geometry/liegroup/test_so3.py
+++ b/tests/geometry/liegroup/test_so3.py
@@ -607,6 +607,47 @@ class TestSo3Conventions(BaseTester):
tol = 1e-2 if dtype == torch.float16 else None
self.assert_close((s * s2) * p, s * (s2 * p), rtol=tol, atol=tol)
+ @pytest.mark.parametrize("points_shape", [(7, 3), (1, 3)])
+ def test_unbatched_rotation_rotates_a_point_set(self, points_shape, device, dtype):
+ # #6426: an unbatched So3 rotates every point of a point set, as So2 does for (N, 2); it used to raise
+ # in torch.linalg.cross because the (4,) quaternion and the points had different ranks.
+ torch.manual_seed(0)
+ s = So3.exp(torch.tensor([0.3, -0.5, 0.2], device=device, dtype=dtype))
+ p = (2 * torch.rand(points_shape) - 1).to(device, dtype)
+
+ out = s * p
+
+ assert out.shape == p.shape
+ self.assert_close(out, torch.stack([s * q for q in p.reshape(-1, 3)]).reshape(points_shape))
+ # R p in float64, on the CPU, from the same rounded quaternion and points: only the output is rounded
+ r = So3(Quaternion(s.q.data.cpu().double())).matrix()
+ self.assert_close(out.cpu(), (p.cpu().double() @ r.mT).to(dtype))
+
+ def test_unbatched_rotation_point_set_gradcheck(self, device):
+ # #6426: gradients flow through the expansion of an unbatched rotation over a point set
+ torch.manual_seed(0)
+ q = torch.randn(4, dtype=torch.float64).to(device)
+ p = torch.randn(5, 3, dtype=torch.float64).to(device)
+ self.gradcheck(lambda q, p: So3(Quaternion(q)) * p, (q, p))
+
+ # #6426: a pose maps (3,) or (N, 3) unbatched and one point per pose for a batch; other layouts, such as a
+ # batch of poses with a point set, raise a ValueError that names the accepted layouts.
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape"), [(None, (2, 5, 3)), (2, (3,)), (2, (7, 3)), (2, (2, 7, 3)), (1, (1, 7, 3))]
+ )
+ def test_point_layout_outside_the_contract_raises(self, batch_size, points_shape, device, dtype):
+ pose = So3.identity(batch_size, device, dtype)
+ with pytest.raises(ValueError, match=r"Expected points of shape \(3,\) or \(N, 3\)"):
+ pose * torch.zeros(points_shape, device=device, dtype=dtype)
+
+ @pytest.mark.parametrize(
+ ("batch_size", "points_shape", "out_shape"),
+ [(None, (3,), (3,)), (None, (7, 3), (7, 3)), (1, (7, 3), (7, 3)), (2, (2, 3), (2, 3)), (2, (1, 3), (2, 3))],
+ )
+ def test_point_layout_in_the_contract_maps(self, batch_size, points_shape, out_shape, device, dtype):
+ pose = So3.identity(batch_size, device, dtype)
+ assert (pose * torch.zeros(points_shape, device=device, dtype=dtype)).shape == out_shape
+
def test_convention_so3_adjoint_conjugates_the_tangent(self, device, dtype):
# Ad(g) xi is the tangent of g exp(xi) g^-1: exp(Ad(g) xi) = g exp(xi) g^-1, with Ad(g) = R
g = So3.exp(torch.tensor([[0.7, -0.2, 0.4]], device=device, dtype=dtype))
diff --git a/tests/geometry/test_pose.py b/tests/geometry/test_pose.py
index 6dd205782..8a82fafe5 100644
--- a/tests/geometry/test_pose.py
+++ b/tests/geometry/test_pose.py
@@ -231,6 +231,22 @@ class TestNamedPose(BaseTester):
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