def compute_projection_matrix(self,
pinhole_src: PinholeCamera) -> 'DepthWarper':
r"""Computes the projection matrix from the source to destination frame.
"""
if not isinstance(self._pinhole_dst, PinholeCamera):
raise TypeError("Member self._pinhole_dst expected to be of class "
"PinholeCamera. Got {}".format(
type(self._pinhole_dst)))
if not isinstance(pinhole_src, PinholeCamera):
raise TypeError("Argument pinhole_src expected to be of class "
"PinholeCamera. Got {}".format(type(pinhole_src)))
# compute the relative pose between the non reference and the reference
# camera frames.
dst_trans_src: torch.Tensor = compose_transformations(
self._pinhole_dst.extrinsics,
inverse_transformation(pinhole_src.extrinsics),
)
# compute the projection matrix between the non reference cameras and
# the reference.
dst_proj_src: torch.Tensor = torch.matmul(self._pinhole_dst.intrinsics,
dst_trans_src)
# update class members
self._pinhole_src = pinhole_src
self._dst_proj_src = dst_proj_src
return self
def relative_transformation(
trans_01: torch.Tensor,
trans_02: torch.Tensor,
orthogonal_rotations: bool = False) -> torch.Tensor:
r"""Function that computes the relative homogenous transformation from a
reference transformation :math:`T_1^{0} = \begin{bmatrix} R_1 & t_1 \\
\mathbf{0} & 1 \end{bmatrix}` to destination :math:`T_2^{0} =
\begin{bmatrix} R_2 & t_2 \\ \mathbf{0} & 1 \end{bmatrix}`.
.. note:: Works with imperfect (non-orthogonal) rotation matrices as well.
The relative transformation is computed as follows:
.. math::
T_1^{2} = (T_0^{1})^{-1} \cdot T_0^{2}
Arguments:
trans_01 (torch.Tensor): reference transformation tensor of shape
:math:`(N, 4, 4)` or :math:`(4, 4)`.
trans_02 (torch.Tensor): destination transformation tensor of shape
:math:`(N, 4, 4)` or :math:`(4, 4)`.
orthogonal_rotations (bool): If True, will invert `trans_01` assuming `trans_01[:, :3, :3]` are
orthogonal rotation matrices (more efficient). Default: False
Shape:
- Output: :math:`(N, 4, 4)` or :math:`(4, 4)`.
Returns:
torch.Tensor: the relative transformation between the transformations.
Example::
>>> trans_01 = torch.eye(4) # 4x4
>>> trans_02 = torch.eye(4) # 4x4
>>> trans_12 = gradslam.geometry.geometryutils.relative_transformation(trans_01, trans_02) # 4x4
"""
if not torch.is_tensor(trans_01):
raise TypeError(
"Input trans_01 type is not a torch.Tensor. Got {}".format(
type(trans_01)))
if not torch.is_tensor(trans_02):
raise TypeError(
"Input trans_02 type is not a torch.Tensor. Got {}".format(
type(trans_02)))
if not trans_01.dim() in (2, 3) and trans_01.shape[-2:] == (4, 4):
raise ValueError("Input must be a of the shape Nx4x4 or 4x4."
" Got {}".format(trans_01.shape))
if not trans_02.dim() in (2, 3) and trans_02.shape[-2:] == (4, 4):
raise ValueError("Input must be a of the shape Nx4x4 or 4x4."
" Got {}".format(trans_02.shape))
if not trans_01.dim() == trans_02.dim():
raise ValueError(
"Input number of dims must match. Got {} and {}".format(
trans_01.dim(), trans_02.dim()))
trans_10: torch.Tensor = (inverse_transformation(trans_01)
if orthogonal_rotations else
torch.inverse(trans_01))
trans_12: torch.Tensor = compose_transformations(trans_10, trans_02)
return trans_12
def _localize(
self, pointclouds: Pointclouds, live_frame: RGBDImages, prev_frame: RGBDImages
):
r"""Compute the poses for `live_frame`. If `prev_frame` is not None, computes the relative
transformation between `live_frame` and `prev_frame` using the selected odometry provider.
If `prev_frame` is None, use the pose from `live_frame`.
Args:
pointclouds (gradslam.Pointclouds): Input batch of pointcloud global maps
live_frame (gradslam.RGBDImages): Input batch of live frames (at time step :math:`t`). Must have sequence
length of 1.
prev_frame (gradslam.RGBDImages or None): Input batch of previous frames (at time step :math:`t-1`).
Must have sequence length of 1. If None, will (skip calling odometry provider and) use the pose
from `live_frame`. Default: None
Returns:
poses (torch.Tensor): Poses for the live_frame batch
Shape:
poses: :math:`(B, 1, 4, 4)`
"""
if not isinstance(pointclouds, Pointclouds):
raise TypeError(
"Expected pointclouds to be of type gradslam.Pointclouds. Got {0}.".format(
type(pointclouds)
)
)
if not isinstance(live_frame, RGBDImages):
raise TypeError(
"Expected live_frame to be of type gradslam.RGBDImages. Got {0}.".format(
type(live_frame)
)
)
if not isinstance(prev_frame, (RGBDImages, type(None))):
raise TypeError(
"Expected prev_frame to be of type gradslam.RGBDImages or None. Got {0}.".format(
type(prev_frame)
)
)
if prev_frame is not None:
if self.odom == "gt":
warnings.warn(
"`prev_frame` is not used when using `odom='gt'` (should be None)"
)
elif not prev_frame.has_poses:
raise ValueError("`prev_frame` should have poses, but did not.")
if prev_frame is None and pointclouds.has_points and self.odom != "gt":
msg = "`prev_frame` was None despite `{}` odometry method. Using `live_frame` poses.".format(
self.odom
)
warnings.warn(msg)
if prev_frame is None or self.odom == "gt":
if not live_frame.has_poses:
raise ValueError(
"`live_frame` must have poses when `prev_frame` is None or `odom='gt'`."
)
return live_frame.poses
if self.odom in ["icp", "gradicp"]:
live_frame.poses = prev_frame.poses
frames_pc = downsample_rgbdimages(live_frame, self.dsratio)
pc2im_bnhw = find_active_map_points(pointclouds, prev_frame)
maps_pc = downsample_pointclouds(pointclouds, pc2im_bnhw, self.dsratio)
transform = self.odomprov.provide(maps_pc, frames_pc)
return compose_transformations(
transform.squeeze(1), prev_frame.poses.squeeze(1)
).unsqueeze(1)