gradslam.odometry¶
gradslam.odometry.base¶
gradslam.odometry.gradicp¶
- class GradICPOdometryProvider(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)[source]¶
An odometry provider that uses the (differentiable) gradICP technique presented in the gradSLAM paper. Computes the relative transformation between a pair of gradslam.Pointclouds objects using GradICP which uses gradLM (\(\nabla LM\)) solver (See gradLM section of the gradSLAM paper). The iterate and damping coefficient are updated by:
\[\begin{split}lambda_1 = Q_\lambda(r_0, r_1) & = \lambda_{min} + \frac{\lambda_{max} - \lambda_{min}}{1 + e^{-B (r_1 - r_0)}} \\ Q_x(r_0, r_1) & = x_0 + \frac{\delta x_0}{\sqrt[nu]{1 + e^{-B2*(r_1 - r_0)}}}`\end{split}\]- provide(maps_pointclouds: gradslam.structures.pointclouds.Pointclouds, frames_pointclouds: gradslam.structures.pointclouds.Pointclouds) → torch.Tensor[source]¶
Uses gradICP to compute the relative homogenous transformation that, when applied to frames_pointclouds, would cause the points to align with points of maps_pointclouds.
- Parameters
maps_pointclouds (gradslam.Pointclouds) – Object containing batch of map pointclouds of batch size \((B)\)
frames_pointclouds (gradslam.Pointclouds) – Object containing batch of live frame pointclouds of batch size \((B)\)
- Returns
The relative transformation that would align maps_pointclouds with frames_pointclouds
- Return type
- Shape:
Output: \((B, 1, 4, 4)\)
gradslam.odometry.groundtruth¶
- class GroundTruthOdometryProvider(*params)[source]¶
Ground truth odometry provider. Computes the relative transformation between a pair of gradslam.RGBDImages objects. Both objects must contain poses attributes.
- provide(rgbdimages1: gradslam.structures.rgbdimages.RGBDImages, rgbdimages2: gradslam.structures.rgbdimages.RGBDImages) → torch.Tensor[source]¶
Computes the relative homogenous transformation between poses of rgbdimages2 and rgbdimages1. The relative transformation is computed as \(T = (T_1)^{-1} \cdot T_2\).
- Parameters
rgbdimages1 (gradslam.RGBDImages) – Object containing batch of reference poses of shape \((B, 1, 4, 4)\)
rgbdimages2 (gradslam.RGBDImages) – Object containing batch of destination poses of shape \((B, 1, 4, 4)\)
- Returns
The relative transformation between the poses of rgbdimages1 and rgbdimages2 (\(T = (T_1)^{-1} \cdot T_2\)).
- Return type
- Shape:
Output: \((B, 1, 4, 4)\)
gradslam.odometry.icp¶
- class ICPOdometryProvider(numiters: int = 20, damp: float = 1e-08, dist_thresh: Optional[Union[float, int]] = None)[source]¶
ICP odometry provider using a point-to-plane error metric. Computes the relative transformation between a pair of gradslam.Pointclouds objects using ICP (Iterative Closest Point). Uses LM (Levenberg-Marquardt) solver.
- provide(maps_pointclouds: gradslam.structures.pointclouds.Pointclouds, frames_pointclouds: gradslam.structures.pointclouds.Pointclouds) → torch.Tensor[source]¶
Uses ICP to compute the relative homogenous transformation that, when applied to frames_pointclouds, would cause the points to align with points of maps_pointclouds.
- Parameters
maps_pointclouds (gradslam.Pointclouds) – Object containing batch of map pointclouds of batch size \((B)\)
frames_pointclouds (gradslam.Pointclouds) – Object containing batch of live frame pointclouds of batch size \((B)\)
- Returns
The relative transformation that would align maps_pointclouds with frames_pointclouds
- Return type
- Shape:
Output: \((B, 1, 4, 4)\)
gradslam.odometry.icputils¶
- solve_linear_system(A: torch.Tensor, b: torch.Tensor, damp: Union[float, torch.Tensor] = 1e-08)[source]¶
Solves the normal equations of a linear system Ax = b, given the constraint matrix A and the coefficient vector b. Note that this solves the normal equations, not the linear system. That is, solves \(A^T A x = A^T b\), not \(Ax = b\).
- Parameters
A (torch.Tensor) – The constraint matrix of the linear system.
b (torch.Tensor) – The coefficient vector of the linear system.
damp (float or torch.Tensor) – Damping coefficient to optionally condition the linear system (in practice, a damping coefficient of \(\rho\) means that we are solving a modified linear system that adds a tiny \(\rho\) to each diagonal element of the constraint matrix \(A\), so that the linear system becomes \((A^TA + \rho I)x = b\), where \(I\) is the identity matrix of shape \((\text{num_of_variables}, \text{num_of_variables})\). Default: 1e-8
- Returns
Solution vector of the normal equations of the linear system
- Return type
- Shape:
A: \((\text{num_of_equations}, \text{num_of_variables})\)
b: \((\text{num_of_equations}, 1)\)
Output: \((\text{num_of_variables}, 1)\)
- gauss_newton_solve(src_pc: torch.Tensor, tgt_pc: torch.Tensor, tgt_normals: torch.Tensor, dist_thresh: Optional[Union[float, int]] = None)[source]¶
Computes Gauss Newton step by forming linear equation. Points from src_pc which have a distance greater than dist_thresh to the closest point in tgt_pc will be filtered.
- Parameters
src_pc (torch.Tensor) – Source pointcloud (the pointcloud that needs warping).
tgt_pc (torch.Tensor) – Target pointcloud (the pointcloud to which the source pointcloud must be warped to).
tgt_normals (torch.Tensor) – Per-point normal vectors for each point in the target pointcloud.
dist_thresh (float or int or None) – Distance threshold for removing src_pc points distant from tgt_pc. Default: None
- Returns
tuple containing:
A (torch.Tensor): linear system equation
b (torch.Tensor): linear system residual
- chamfer_indices (torch.Tensor): Index of the closest point in tgt_pc for each point in src_pc
that was not filtered out.
- Return type
- Shape:
src_pc: \((1, N_s, 3)\)
tgt_pc: \((1, N_t, 3)\)
tgt_normals: \((1, N_t, 3)\)
A: \((N_sf, 6)\) where \(N_sf \leq N_s\)
b: \((N_sf, 1)\) where \(N_sf \leq N_s\)
chamfer_indices: \((1, N_sf)\) where \(N_sf \leq N_s\)
- point_to_plane_ICP(src_pc: torch.Tensor, tgt_pc: torch.Tensor, tgt_normals: torch.Tensor, initial_transform: Optional[torch.Tensor] = None, numiters: int = 20, damp: float = 1e-08, dist_thresh: Optional[Union[float, int]] = None)[source]¶
Computes a rigid transformation between tgt_pc (target pointcloud) and src_pc (source pointcloud) using a point-to-plane error metric and the LM (Levenberg–Marquardt) solver.
- Parameters
src_pc (torch.Tensor) – Source pointcloud (the pointcloud that needs warping).
tgt_pc (torch.Tensor) – Target pointcloud (the pointcloud to which the source pointcloud must be warped to).
tgt_normals (torch.Tensor) – Per-point normal vectors for each point in the target pointcloud.
initial_transform (torch.Tensor or None) – The initial estimate of the transformation between ‘src_pc’ and ‘tgt_pc’. If None, will use the identity matrix as the initial transform. Default: None
numiters (int) – Number of iterations to run the optimization for. Default: 20
damp (float) – Damping coefficient for nonlinear least-squares. Default: 1e-8
dist_thresh (float or int or None) – Distance threshold for removing src_pc points distant from tgt_pc. Default: None
- Returns
tuple containing:
transform (torch.Tensor): linear system residual
chamfer_indices (torch.Tensor): Index of the closest point in tgt_pc for each point in src_pc that was not filtered out.
- Return type
- Shape:
src_pc: \((1, N_s, 3)\)
tgt_pc: \((1, N_t, 3)\)
tgt_normals: \((1, N_t, 3)\)
initial_transform: \((4, 4)\)
transform: \((4, 4)\)
chamfer_indices: \((1, N_sf)\) where \(N_sf \leq N_s\)
- point_to_plane_gradICP(src_pc: torch.Tensor, tgt_pc: torch.Tensor, tgt_normals: torch.Tensor, initial_transform: Optional[torch.Tensor] = None, 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)[source]¶
Computes a rigid transformation between tgt_pc (target pointcloud) and src_pc (source pointcloud) using a point-to-plane error metric and gradLM (\(\nabla LM\)) solver (See gradLM section of the gradSLAM paper). The iterate and damping coefficient are updated by:
\[\begin{split}lambda_1 = Q_\lambda(r_0, r_1) & = \lambda_{min} + \frac{\lambda_{max} - \lambda_{min}}{1 + e^{-B (r_1 - r_0)}} \\ Q_x(r_0, r_1) & = x_0 + \frac{\delta x_0}{\sqrt[nu]{1 + e^{-B2*(r_1 - r_0)}}}`\end{split}\]- Parameters
src_pc (torch.Tensor) – Source pointcloud (the pointcloud that needs warping).
tgt_pc (torch.Tensor) – Target pointcloud (the pointcloud to which the source pointcloud must be warped to).
tgt_normals (torch.Tensor) – Per-point normal vectors for each point in the target pointcloud.
initial_transform (torch.Tensor or None) – The initial estimate of the transformation between ‘src_pc’ and ‘tgt_pc’. If None, will use the identity matrix as the initial transform. Default: None
numiters (int) – Number of iterations to run the optimization for. Default: 20
damp (float) – Damping coefficient for nonlinear least-squares. Default: 1e-8
dist_thresh (float or int or None) – Distance threshold for removing src_pc points distant from tgt_pc. Default: None
lambda_max (float or int) – Maximum value the damping function can assume (lambda_min will be \(\frac{1}{\text{lambda_max}}\))
- Returns
tuple containing:
transform (torch.Tensor): linear system residual
chamfer_indices (torch.Tensor): Index of the closest point in tgt_pc for each point in src_pc that was not filtered out.
- Return type
- Shape:
src_pc: \((1, N_s, 3)\)
tgt_pc: \((1, N_t, 3)\)
tgt_normals: \((1, N_t, 3)\)
initial_transform: \((4, 4)\)
transform: \((4, 4)\)
chamfer_indices: \((1, N_sf)\) where \(N_sf \leq N_s\)
- downsample_pointclouds(pointclouds: gradslam.structures.pointclouds.Pointclouds, pc2im_bnhw: torch.Tensor, ds_ratio: int) → gradslam.structures.pointclouds.Pointclouds[source]¶
Downsamples active points of pointclouds (points that project inside the live frame) and removes non-active points.
- Parameters
pointclouds (gradslam.Pointclouds) – Pointclouds to downsample
pc2im_bnhw (torch.Tensor) – Active map points lookup table. Each row contains batch index b, point index (in pointclouds) n, and height and width index after projection to live frame h and w respectively.
ds_ratio (int) – Downsampling ratio
- Returns
Downsampled pointclouds
- Return type
gradslam.Pointclouds
- Shape:
pc2im_bnhw: \((\text{num_active_map_points}, 4)\)
- downsample_rgbdimages(rgbdimages: gradslam.structures.rgbdimages.RGBDImages, ds_ratio: int) → gradslam.structures.pointclouds.Pointclouds[source]¶
Downsamples points and normals of RGBDImages and returns a gradslam.Pointclouds object
- Parameters
rgbdimages (gradslam.RGBDImages) – RGBDImages to downsample
ds_ratio (int) – Downsampling ratio
- Returns
Downsampled points and normals
- Return type
gradslam.Pointclouds