ICovariance2to3
The ICovariance2to3 interface is responsible for converting 2D uncertainties (depth and flow) into 3D spatial covariance matrices. This is a key component for probabilistic visual odometry as described in Section III.C of the MAC-VO paper.
Interface
class ICovariance2to3(ABC, ConfigTestableSubclass):
@abstractmethod
def estimate(
self,
frame: StereoData,
kp: torch.Tensor,
depth_est: IStereoDepth.Output,
depth_cov: torch.Tensor | None,
flow_cov: torch.Tensor | None,
) -> torch.Tensor: ...
Methods to Implement
estimate(...) -> torch.Tensor- Core method for covariance conversion
- Parameters:
frame: Current stereo framekp: N×2 tensor of keypoint coordinates (u,v)depth_est: Depth estimation outputdepth_cov: Optional per-point depth uncertaintyflow_cov: Optional per-point flow uncertainty
- Returns N×3×3 covariance matrices in camera coordinates
Implementations
Base Models
-
NoCovariance- Returns identity matrices for all points
- Used for testing or when uncertainty is not needed
- No configuration required
-
DepthCovariance- Uses only depth uncertainty
- Adds regularization for numerical stability
- Configuration:
regularization: Positive float (default: 1e-5)
Advanced Models
-
MatchCovariance- Main covariance model used in MAC-VO
- Combines depth and flow uncertainties
- Uses local depth statistics in uncertainty regions
- Configuration:
kernel_size: Size of local region (must be odd)match_cov_default: Default flow uncertaintymin_flow_cov: Minimum flow uncertaintymin_depth_cov: Minimum depth uncertaintydevice: Target device ("cuda" or "cpu")
-
GaussianMixtureCovariance- Models depth uncertainty as Gaussian mixture
- More sophisticated than weighted variance
- Configuration:
kernel_size: Size of local region (must be odd)match_cov_default: Default flow uncertaintymin_flow_cov: Minimum flow uncertaintymin_depth_cov: Minimum depth uncertainty
Modifiers
-
Modifier_Diagonalize- Wrapper that zeroes off-diagonal elements
- Makes covariance matrices diagonal
- Configuration:
type: Base model typeargs: Base model arguments
-
Modifier_Normalize- Wrapper that normalizes determinants
- Makes average determinant = 1
- Configuration:
type: Base model typeargs: Base model arguments