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)