SLAM (Simultaneous Localization and Mapping) enables a robot to build a map of its environment while tracking its own pose within that map — without any external reference. This project implements a complete visual-inertial SLAM pipeline that tightly couples a 100 Hz IMU with 10 Hz stereo camera observations via the Extended Kalman Filter.
The IMU provides high-rate motion predictions via dead-reckoning on SE(3), while stereo images supply sparse visual features that are triangulated into 3D landmarks and used to correct accumulated drift. The system estimates a full 6-DOF trajectory and a sparse 3D landmark map jointly, tested on the dataset01.npy benchmark sequence.
Propagates the robot's SE(3) pose using angular velocity ωk and linear velocity vk from the IMU. The Rodrigues formula computes the SO(3) exponential map for rotation updates, while a 6×6 Jacobian propagates covariance through the prediction step.
Initializes 3D landmark positions from stereo pixel pairs (uL, vL, uR, vR) using the normalized Direct Linear Transform (DLT). Outliers are rejected via Median Absolute Deviation, and multi-frame aggregation improves depth estimates. Only landmarks observed in ≥ 150 frames are retained.
Jointly updates the 6-DOF pose and all selected landmark positions using visual reprojection errors. A sparse block-diagonal covariance (scipy.sparse) ensures efficiency. Chi-square gating (χ² < 9.488, 95% CI) and a 30 px pixel threshold form a two-stage outlier rejection pipeline before applying the Kalman update.
Characteristic system properties from dataset01.npy.
| Parameter | Value | Description |
|---|---|---|
min_obs | 150 | Min frames to select a landmark |
chi2_threshold | 9.488 | χ² test threshold (95% CI, 4 DOF) |
pixel_threshold | 30.0 px | Max reprojection residual |
| EKF state dim | 6 | Pose error state [δφ, δp] |
| Dataset | dataset01.npy | IMU + stereo features stream |