Technical Summary: UNSEEN – Uncertainty-aware Navigation via Sparse Estimation in Unknown Environments

1. Problem Statement

Autonomous navigation in unknown environments remains a critical challenge for resource-constrained Unmanned Aerial Vehicles (UAVs), particularly when relying solely on visual sensors. Existing approaches often suffer from three primary limitations:

1. Loose Coupling: Most pipelines decouple perception (SLAM), mapping, and planning, failing to propagate estimation uncertainty consistently across the stack.
2. Brittleness: Vision-only systems degrade significantly under motion blur, low texture, and illumination changes because they neglect the tight coupling between commanded motion and perception quality.
3. Assumptions: Many methods rely on strong assumptions, such as prior maps, near-perfect localization, or noise-agnostic models, or they require expensive multi-modal sensor suites (e.g., LiDAR, IMU, RGB-D) that increase system complexity and cost.

The specific problem addressed is Exploratory Point-to-Point motion: navigating a robot from a known start pose to a goal pose through an environment that is initially unknown, using only a front-mounted camera, while ensuring safety and maintaining state estimation accuracy.

2. Methodology

The paper proposes UNSEEN, a unified, end-to-end framework that explicitly couples localization, mapping, and planning using sparse map representations and consistent uncertainty propagation. The system operates at 6 Hz and consists of four tightly integrated modules:

A. UNSEEN-SLAM (Localization & Uncertainty Extraction)

Unlike traditional smoothing-based V-SLAM systems (e.g., ORB-SLAM3) that discard uncertainty after optimization, UNSEEN-SLAM extracts state covariances from the pose graph.

• Mechanism: It employs a pose-graph optimization with Tikhonov regularization, incorporating marginal covariances of both poses and landmarks as residuals.
• Covariance Computation: To maintain efficiency, the system exploits the block-diagonal structure of the information matrix. It computes the Schur complement to efficiently derive marginal covariances for the state vector without incurring the full $O(N^3)$ complexity of a dense inversion.
• Output: It provides sparse maps and robot poses with associated uncertainties, which are propagated downstream.

B. UNSEEN-MAP (Probabilistic Sparse Mapping)

This module converts the sparse, uncertain landmarks from the SLAM module into a probabilistic occupancy representation suitable for planning.

• Representation: It utilizes an Octree structure (based on Octomap) but introduces a novel update rule.
• Uncertainty Integration: Instead of treating landmarks as deterministic points, the system models them as truncated Gaussian distributions. When updating a voxel, it integrates the probability density function (PDF) of the landmark's covariance ellipsoid over the voxel volume.
• Result: Voxels with high-confidence landmarks are strongly marked as occupied, while uncertain sections contribute weakly, effectively modeling the spatial extent of estimation uncertainty.

C. UNSEEN-PLAN (Uncertainty-Aware Path Planning)

This module generates safe paths using a Receding Horizon approach in SE(3).

• Global Planning: It uses RRT* to generate a path toward the goal, assuming unknown space is free (optimistic) but scoring nodes based on a visibility metric that favors pointing the camera toward textured (occupied) regions to maximize information gain.
• FoV Truncation & Visibility: The global path is truncated at the boundary of known free space. A visibility check ensures that every point on the local path maintains a Field of View (FoV) containing at least some occupied voxels, preventing the robot from entering untextured, unobservable regions.
• Convex Decomposition: The discrete path is converted into convex free-space polytopes to serve as constraints for trajectory optimization.

D. UNSEEN-OPT (Probabilistic Trajectory Optimization)

The final trajectory is generated by solving a constrained Maximum Likelihood Estimation (MLE) problem on the Lie Group SE(3).

• Objective: The cost function maximizes the likelihood of observing landmarks in the visible submap. It steers the camera toward landmarks with low uncertainty rather than simply maximizing the number of visible features.
• Formulation: The trajectory is parameterized using B-splines for both translation and rotation (via the Lie algebra $\mathfrak{so}(3)$).
• Constraints: The optimizer enforces hard constraints on dynamics (velocity, acceleration), obstacle avoidance (via convex polytopes), and platform-specific constraints (e.g., differential flatness for UAVs).

3. Key Contributions

The paper outlines five specific contributions:

1. Novel V-SLAM: A system that extracts state covariances from a pose graph and propagates them upstream and downstream to improve estimation conditioning and safety.
2. Probabilistic Sparse Mapping: A mapping framework using truncated Gaussian distributions to update occupancy, enabling uncertainty-aware planning.
3. Uncertainty-Aware Planning: A receding horizon algorithm that ensures safety and estimation robustness by combining global exploration with local smoothing and visibility constraints.
4. SE(3) Trajectory Optimization: A novel constrained MLE problem on SE(3) that maximizes visual navigation effectiveness under uncertainty using a new visibility model and B-spline representation.
5. Unified Stack: A tightly integrated autonomy stack enabling vision-only, safe, and reliable navigation for resource-constrained platforms.

4. Experimental Results

The authors evaluated UNSEEN through simulations and extensive real-world experiments in an indoor arena with varying levels of texture difficulty (Easy, Medium, Hard).

• SLAM Performance: Compared to ORB-SLAM3, UNSEEN-SLAM reduced the Absolute Translational Error (ATE) by 9.8% and the Relative Translational Error (RTE) by 2%. The asynchronous covariance extraction did not hinder tracking performance.
• Planning Efficiency: In the "Cube Benchmark" simulation (navigating around a textured cube in a textureless room), UNSEEN-PLAN successfully handled maps with up to 40,000 points in under 2 seconds. In contrast, the state-of-the-art method APACE failed to converge within 22 seconds on the same map size.
• Perception Awareness: In a comparison between Perception-Aware (PAW) and Perception-Agnostic (PAG) planning, the PAW approach reduced the Root Mean Square Error (RMSE) of the trajectory from 1.997m to 0.674m, demonstrating that planning for feature visibility significantly reduces drift.
• Real-World Success: In 10 real-world flights through unknown environments with limited texture, UNSEEN achieved a 100% task success rate (reaching the goal without collisions) while satisfying motion constraints.

5. Significance and Claims

The paper claims that UNSEEN is the first framework to achieve online, uncertainty-aware, and perception-aware planning and trajectory optimization in unknown environments using only a front-facing camera for sparse estimation.

The significance of the work lies in its ability to:

• Bridge the Gap: It effectively bridges the gap between sparse V-SLAM estimation and occupancy-based planning, preserving uncertainty information that is typically discarded.
• Enable Resource-Constrained Autonomy: It demonstrates that high-performance, safe navigation is possible without heavy multi-modal sensor suites, relying instead on a lightweight, vision-only approach.
• Improve Robustness: By explicitly factoring uncertainty into the planning loop, the system avoids "greedy" exploration that leads to dead-ends or loss of tracking in low-texture environments, ensuring that the robot only traverses areas where it can maintain reliable state estimation.

The authors conclude that while the method is robust, future work will focus on extending the approach to challenging outdoor scenarios, incorporating a wider class of robotic platforms, and addressing distributed navigation problems.