gradslam.slam

gradslam.slam.icpslam

class ICPSLAM(*, odom: str = 'gradicp', dsratio: int = 4, numiters: int = 20, damp: float = 1e-08, dist_thresh: Optional[Union[float, int]] = None, lambda_max: Union[float, int] = 2.0, B: Union[float, int] = 1.0, B2: Union[float, int] = 1.0, nu: Union[float, int] = 200.0, device: Optional[Union[torch.device, str]] = None)[source]

ICP-SLAM for batched sequences of RGB-D images.

Parameters
  • odom (str) – Odometry method to be used from {‘gt’, ‘icp’, ‘gradicp’}. Default: ‘gradicp’

  • dsratio (int) – Downsampling ratio to apply to input frames before ICP. Only used if odom is ‘icp’ or ‘gradicp’. Default: 4

  • numiters (int) – Number of iterations to run the optimization for. Only used if odom is ‘icp’ or ‘gradicp’. Default: 20

  • damp (float or torch.Tensor) – Damping coefficient for nonlinear least-squares. Only used if odom is ‘icp’ or ‘gradicp’. Default: 1e-8

  • dist_thresh (float or int or None) – Distance threshold for removing src_pc points distant from tgt_pc. Only used if odom is ‘icp’ or ‘gradicp’. Default: None

  • lambda_max (float or int) – Maximum value the damping function can assume (lambda_min will be \(\frac{1}{\text{lambda_max}}\)). Only used if odom is ‘gradicp’.

  • B (float or int) – gradLM falloff control parameter (see GradICPOdometryProvider description). Only used if odom is ‘gradicp’.

  • B2 (float or int) – gradLM control parameter (see GradICPOdometryProvider description). Only used if odom is ‘gradicp’.

  • nu (float or int) – gradLM control parameter (see GradICPOdometryProvider description). Only used if odom is ‘gradicp’.

  • device (torch.device or str or None) – The desired device of internal tensors. If None, sets device to be the CPU. Default: None

Examples:

>>> rgbdimages = RGBDImages(colors, depths, intrinsics, poses)
>>> slam = ICPSLAM(odom='gt')
>>> pointclouds, poses = slam(rgbdimages)
>>> o3d.visualization.draw_geometries([pointclouds.o3d(0)])

>>> rgbdimages = RGBDImages(colors, depths, intrinsics, poses)
>>> slam = ICPSLAM(odom='gt')
>>> pointclouds = Pointclouds()
>>> pointclouds, new_poses = self.step(pointclouds, frames[:, 0], None)
>>> frames.poses[:, :1] = new_poses
>>> pointclouds, new_poses = self.step(pointclouds, frames[:, 1], frames[:, 0])

>>> rgbdimages = RGBDImages(colors, depths, intrinsics, poses)
>>> slam = ICPSLAM(odom='gradicp')
>>> pointclouds = Pointclouds()
>>> pointclouds, new_poses = self.step(pointclouds, frames[:, 0], None)
>>> frames.poses[:, :1] = new_poses
>>> pointclouds, new_poses = self.step(pointclouds, frames[:, 1], frames[:, 0])
forward(frames: gradslam.structures.rgbdimages.RGBDImages)[source]

Builds global map pointclouds from a batch of input RGBDImages with a batch size of \(B\) and sequence length of \(L\).

Parameters

frames (gradslam.RGBDImages) – Input batch of frames with a sequence length of L.

Returns

tuple containing:

  • pointclouds (gradslam.Pointclouds): Pointclouds object containing \(B\) global maps

  • poses (torch.Tensor): Poses computed by the odometry method

Return type

tuple

Shape:
  • poses: \((B, L, 4, 4)\)

step(pointclouds: gradslam.structures.pointclouds.Pointclouds, live_frame: gradslam.structures.rgbdimages.RGBDImages, prev_frame: Optional[gradslam.structures.rgbdimages.RGBDImages] = None, inplace: bool = False)[source]

Updates global map pointclouds with a SLAM step on 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.

Parameters
  • pointclouds (gradslam.Pointclouds) – Input batch of pointcloud global maps

  • live_frame (gradslam.RGBDImages) – Input batch of live frames (at time step \(t\)). Must have sequence length of 1.

  • prev_frame (gradslam.RGBDImages or None) – Input batch of previous frames (at time step \(t-1\)). Must have sequence length of 1. If None, will (skip calling odometry provider and) use the pose from live_frame. Default: None

  • inplace (bool) – Can optionally update the pointclouds and live_frame poses in-place. Default: False

Returns

tuple containing:

  • pointclouds (gradslam.Pointclouds): Updated \(B\) global maps

  • poses (torch.Tensor): Poses for the live_frame batch

Return type

tuple

Shape:
  • poses: \((B, 1, 4, 4)\)

gradslam.slam.pointfusion

class PointFusion(*, odom: str = 'gradicp', dist_th: Union[float, int] = 0.05, angle_th: Union[float, int] = 20, sigma: Union[float, int] = 0.6, dsratio: int = 4, numiters: int = 20, damp: float = 1e-08, dist_thresh: Optional[Union[float, int]] = None, lambda_max: Union[float, int] = 2.0, B: Union[float, int] = 1.0, B2: Union[float, int] = 1.0, nu: Union[float, int] = 200.0, device: Optional[Union[torch.device, str]] = None)[source]

Point-based Fusion (PointFusion for short) SLAM for batched sequences of RGB-D images (See Point-based Fusion paper).

Parameters
  • odom (str) – Odometry method to be used from {‘gt’, ‘icp’, ‘gradicp’}. Default: ‘gradicp’

  • dist_th (float or int) – Distance threshold.

  • dot_th (float or int) – Dot product threshold.

  • sigma (torch.Tensor or float or int) – Width of the gaussian bell. Original paper uses 0.6 emperically.

  • dsratio (int) – Downsampling ratio to apply to input frames before ICP. Only used if odom is ‘icp’ or ‘gradicp’. Default: 4

  • numiters (int) – Number of iterations to run the optimization for. Only used if odom is ‘icp’ or ‘gradicp’. Default: 20

  • damp (float or torch.Tensor) – Damping coefficient for nonlinear least-squares. Only used if odom is ‘icp’ or ‘gradicp’. Default: 1e-8

  • dist_thresh (float or int or None) – Distance threshold for removing src_pc points distant from tgt_pc. Only used if odom is ‘icp’ or ‘gradicp’. Default: None

  • lambda_max (float or int) – Maximum value the damping function can assume (lambda_min will be \(\frac{1}{\text{lambda_max}}\)). Only used if odom is ‘gradicp’.

  • B (float or int) – gradLM falloff control parameter (see GradICPOdometryProvider description). Only used if odom is ‘gradicp’.

  • B2 (float or int) – gradLM control parameter (see GradICPOdometryProvider description). Only used if odom is ‘gradicp’.

  • nu (float or int) – gradLM control parameter (see GradICPOdometryProvider description). Only used if odom is ‘gradicp’.

  • device (torch.device or str or None) – The desired device of internal tensors. If None, sets device to be the CPU. Default: None

Examples:

>>> rgbdimages = RGBDImages(colors, depths, intrinsics, poses)
>>> slam = PointFusion(odom='gt')
>>> pointclouds, poses = slam(rgbdimages)
>>> o3d.visualization.draw_geometries([pointclouds.o3d(0)])

gradslam.slam.fusionutils

update_map_fusion(pointclouds: gradslam.structures.pointclouds.Pointclouds, rgbdimages: gradslam.structures.rgbdimages.RGBDImages, dist_th: Union[float, int], dot_th: Union[float, int], sigma: Union[torch.Tensor, float, int], inplace: bool = False)gradslam.structures.pointclouds.Pointclouds[source]

Updates pointclouds in-place given the live frame RGB-D images using PointFusion. (See Point-based Fusion paper).

Parameters
  • pointclouds (gradslam.Pointclouds) – Pointclouds of global maps. Must have points, colors, normals and features (ccounts).

  • rgbdimages (gradslam.RGBDImages) – Live frames from the latest sequence

  • dist_th (float or int) – Distance threshold.

  • dot_th (float or int) – Dot product threshold.

  • sigma (torch.Tensor or float or int) – Standard deviation of the Gaussian. Original paper uses 0.6 emperically.

  • inplace (bool) – Can optionally update the pointclouds in-place. Default: False

Returns

Updated Pointclouds object containing global maps.

Return type

gradslam.Pointclouds

update_map_aggregate(pointclouds: gradslam.structures.pointclouds.Pointclouds, rgbdimages: gradslam.structures.rgbdimages.RGBDImages, inplace: bool = False)gradslam.structures.pointclouds.Pointclouds[source]

Aggregate points from live frames with global maps by appending the live frame points.

Parameters
  • pointclouds (gradslam.Pointclouds) – Pointclouds of global maps. Must have points, colors, normals and features (ccounts).

  • rgbdimages (gradslam.RGBDImages) – Live frames from the latest sequence

  • inplace (bool) – Can optionally update the pointclouds in-place. Default: False

Returns

Updated Pointclouds object containing global maps.

Return type

gradslam.Pointclouds