PaperlayerView exact revision on arXiv

Source-linked AI summary

Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning

Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, Roland Siegwart

arXiv:1611.03631v2cs.RO

TL;DR

MAVs need fast local planning in unexplored environments, but trajectory optimization requires obstacle-distance information. Voxblox incrementally builds ESDFs from TSDFs on growing maps while retaining TSDF-based visualization. It reports faster TSDF construction than Octomap, more accurate TSDF-derived ESDFs than occupancy-derived ones, and real-time onboard MAV replanning.

  • Problem

    MAV local planning in unexplored environments requires rapidly updated obstacle distances and gradients while mapping and planning operate in real time.

  • Method

    Voxblox incrementally builds ESDFs directly from TSDFs on dynamically growing voxel-hashed maps and produces meshes for human-readable visualization.

  • Results

    Voxblox builds TSDFs faster than Octomaps, produces more accurate ESDFs from TSDFs than occupancy maps, and supports real-time onboard online replanning.

  • Takeaways & Limitations

    The complete voxblox system provides a real-time onboard mapping and trajectory-optimization replanning pipeline for MAV navigation in previously unknown environments.

Abstract

from arXiv · show

Micro Aerial Vehicles (MAVs) that operate in unstructured, unexplored environments require fast and flexible local planning, which can replan when new parts of the map are explored. Trajectory optimization methods fulfill these needs, but require obstacle distance information, which can be given by Euclidean Signed Distance Fields (ESDFs). We propose a method to incrementally build ESDFs from Truncated Signed Distance Fields (TSDFs), a common implicit surface representation used in computer graphics and vision. TSDFs are fast to build and smooth out sensor noise over many observations, and are designed to produce surface meshes. Meshes allow human operators to get a better assessment of the robot's environment, and set high-level mission goals. We show that we can build TSDFs faster than Octomaps, and that it is more accurate to build ESDFs out of TSDFs than occupancy maps. Our complete system, called voxblox, will be available as open source and runs in real-time on a single CPU core. We validate our approach on-board an MAV, by using our system with a trajectory optimization local planner, entirely on-board and in real-time.

I. INTRODUCTION

Voxblox targets real-time local planning for MAVs in unexplored environments by incrementally building ESDFs from TSDFs on dynamically growing maps. The system also supports human-readable meshes and is validated through onboard online replanning.

  • Motivation: MAV local planning must operate in real time while mapping and planning through changing or newly explored environments.Fast, lightweight algorithms are needed because MAVs have limited payload and power budgets and fast dynamics.
  • Motivation: Trajectory optimization planners require obstacle distances and distance gradients throughout the map for local replanning.Existing distance maps are commonly computed from occupancy maps, often in batch.
  • Validation: Voxblox is presented as a complete real-time system for integrating TSDF and ESDF maps with an onboard trajectory-optimization planner.The planning experiment uses a TSDF mesh and an ESDF slice while the vehicle plans toward a point behind an obstacle.
  • Contribution: Voxblox incrementally builds ESDFs online from TSDFs on dynamically growing maps while producing meshes suitable for visualization.TSDFs are fast to construct, filter sensor noise, and provide human-readable surfaces with sub-voxel resolution.
  • Contributions: The work analyzes TSDF construction and ESDF errors, including safety margins, and validates online replanning entirely onboard an MAV.The contributions include reconstruction-speed analysis, surface-accuracy analysis, and analytical and experimental ESDF error analysis.

II. RELATED WORK

Related work spans occupancy-based planning maps, incremental ESDF construction, and real-time TSDF reconstruction. Voxblox combines these directions for CPU-only onboard planning with dynamically growing maps and planning-oriented voxel sizes.

  • Occupancy and ESDF maps: Trajectory optimization planners need ESDF distances and collision gradients beyond the occupancy information supplied by common occupancy maps.Octomap stores occupancy probabilities in a hierarchical octree, while ESDFs provide distances over the robot’s workspace.
  • Incremental construction: Prior ESDF research largely accelerated batch occupancy transforms with GPUs, whereas this work targets minimal computation cost on CPU-only platforms.Incremental ESDF construction and dynamically growing maps are identified as essential for onboard MAV local planning.
  • Incremental construction: Voxblox extends incremental occupancy-based ESDF construction by maintaining ESDFs directly from TSDF distance information.This avoids converting TSDFs to occupancy data before ESDF construction.
  • TSDF reconstruction: TSDF systems commonly emphasize high-resolution real-time mesh reconstruction, but voxblox instead uses larger voxels for fast, memory-efficient onboard planning.Prior systems include KinectFusion and extensions using moving volumes, octrees, or voxel hashing.
  • Comparison with prior ESDF-TSDF planning: An earlier ESDF-TSDF planning system converted a complete TSDF to occupancy and computed an ESDF in batch for a fixed-size volume.Voxblox instead maintains the ESDF incrementally from the TSDF and supports map growth without knowing its size beforehand.

III. SYSTEM

The system integrates sensor data into a TSDF, then incrementally propagates updated voxels into an ESDF and mesh. Voxel hashing supports map growth, while weighting and merging strategies target speed and accuracy at large voxel sizes.

  • System architecture: Voxblox has two stages: incorporating sensor data into a TSDF and propagating updated TSDF voxels to update the ESDF.Updated TSDF voxels can also drive incremental mesh reconstruction for visualization.
  • Map representation: Independent TSDF and ESDF voxel layers use hashed fixed-size blocks, enabling O(1) insertions and look-ups as the map grows.The hashed structure provides flexible growth and faster access than Octomap’s octree structure.
  • System architecture: The ESDF and mesh can be reconstructed incrementally on demand from updated TSDF voxels for planning or visualization.The system diagram depicts TSDF, ESDF, and mesh layers interacting with sensor data through integrators.
  • TSDF integration: TSDF weighting and merging choices strongly affect integration speed and reconstruction accuracy, especially with large voxels.The method evaluates weighting strategies and grouped integration to improve speed and accuracy at large voxel sizes.
  • Weighting: The proposed weighting combines a simplified RGB-D noise model with linear drop-off behind the surface boundary.The truncation distance is δ = 4v and ϵ = v, where v is the voxel size; the design reduces influence from voxels behind thin surfaces.

B. Merging

For large voxels, grouped raycasting reduces TSDF integration work by aggregating points that map to the same voxel before raycasting. This retains the measurements while avoiding repeated raycasts.

  • Motivation: Large voxels can receive thousands of rays from one scan, motivating integration that raycasts only once per end voxel.The approach exploits this concentration of measurements for a significant speedup.
  • Integration strategies: Raycasting updates voxels along camera rays, whereas projection mapping is faster but causes strong aliasing effects for larger voxels.Projection mapping computes voxel distances from depth-image values rather than traversing rays from the camera.
  • Grouped raycasting: Grouped raycasting groups sensor points by destination voxel, averages their positions and colors, and raycasts once on each weighted mean.All measurements remain incorporated through their combined weights and distances.

V. CONSTRUCTING ESDF FROM TSDF

The method incrementally updates an ESDF from a TSDF on a dynamically growing map. It propagates changes through raise and lower wavefronts while preserving TSDF distances near surfaces.

  • The approach extends incremental ESDF construction from occupancy maps to use TSDF distance values and dynamically changing map sizes.
  • ESDF voxels within a fixed band around the surface inherit values from co-located TSDF voxels and cannot be modified by propagation.The band contains TSDF voxels satisfying |vT.d| < γ.
  • Raise and lower wavefronts propagate distance increases and decreases through 26-connected neighboring voxels.Raise invalidates affected voxels and their children; lower continues while neighboring distances can decrease.
  • The implementation raises all voxels before lowering them, leaves unknown voxels unchanged, and stores parent directions rather than full parent indices.
  • Newly observed voxels add their neighbors to the lower queue so their ESDF values become valid as the map grows.
  • A bucketed priority queue tracks updates using priority |d|, with FIFO and priority-queue variants compared experimentally.

B. Sources of Error in ESDF

The analysis separates ESDF error into projective TSDF distance error and quasi-Euclidean distance error. It uses geometric assumptions and repeated observations to quantify these errors and recommend planning margins.

  • The final ESDF error has two sources: projective distance calculations from the TSDF and quasi-Euclidean distance calculations.
  • Projective distance matches or overestimates Euclidean distance to the nearest surface, with residual error depending on measured distance d and incidence angle θ.The analysis assumes locally planar objects.
  • The projective-error analysis assumes θ is uniformly distributed from π/20 to π/2, corresponding to an MAV at least 1 meter from a surface with a 5-meter maximum ray length.
  • For multiple independent observations, the projective-distance error decreases: at p = 0.95 it is 0.5δ for three observations and trends below 0.25δ as observations increase.
  • If only one voxel at the surface frontier defines the fixed band, the recommended compensation is to increase safety distance by half a voxel.
  • Quasi-Euclidean distance has zero angular error at multiples of 45° and maximum error at φ = 22.5°.
  • The method recommends inflating the robot bounding box by 8.25% to compensate for the quasi-Euclidean distance approximation.Empirical results reportedly show the overall ESDF error is small enough to justify the speed-up over full Euclidean distance.

VI. EXPERIMENTAL RESULTS

The experiments validate the algorithms on two real-world datasets with different sensing modalities and structure ground truth. All experiments use one thread on a quad-core 2.5 GHz i7 CPU.

  • The evaluation uses the cow dataset with an RGB-D sensor and EuRoC with a stereo camera, both validated against structure ground truth.
  • The cow dataset uses a Microsoft Kinect, Vicon pose data, and three merged Leica TPS MS50 scans as ground truth.
  • EuRoC is a public 3D reconstruction benchmark using narrow-baseline grayscale stereo, Vicon-fused IMU pose data, and Leica TPS MS50 structure ground truth.The experiments use the V1 01 easy dataset.
  • All experiments run on a quad-core i7 CPU at 2.5 GHz with only one thread used.

A. TSDF Construction

TSDF construction choices affect reconstruction accuracy and speed, particularly at larger voxel sizes. The proposed weighting and merging strategy improves structure preservation and enables faster mapping than Octomap-based approaches.

  • Weighting and reconstruction: Weighting matters more at larger voxel sizes because more measurements are merged into each voxel.
  • Weighting and reconstruction: Quadratic weighting outperforms constant weighting as voxel size increases, preserving structure that constant weighting distorts.On the cow dataset, constant weighting misplaced the head and removed the rear legs, while the proposed strategy better preserved structure.
  • Merging and speed: Voxel hashing keeps lookups at O(1), whereas Octomap lookups scale with O(log n) as the tree grows.This data-structure difference explains the speed advantage of the TSDF approach over hierarchical Octomap storage.
  • Merging and speed: Our approach is up to 20 times faster than standard TSDF raycasting and up to 2 times faster than grouped Octomap insertions.The timing comparison uses EuRoC data and a logarithmic time scale.
  • Simulation setup: A simulation mesh and horizontal ESDF slice demonstrate the reconstruction setup using 50 viewpoints and 0.05-meter voxels.The scene contains three planes, a cube, and a sphere, with the ground plane omitted from the rendering.
  • Practical outcome: The resulting TSDF speed makes real-time mapping and planning on-board an MAV feasible on a single CPU core.

B. ESDF Construction

The ESDF evaluation compares occupancy-derived and TSDF-derived constructions under simulated ground truth. TSDF surface bands provide lower error than occupancy, while quasi-Euclidean distance trades a small accuracy loss for substantially lower runtime.

  • Simulation setup: A simulated benchmark evaluates ESDF error and integration time against ground truth for planes, a sphere, and a cube.The 10 × 10 × 10-meter scene uses noiseless RGB-D observations from 50 random free-space viewpoints.
  • Surface-band construction: A one-voxel-wide fixed band around the TSDF surface achieves the lowest ESDF errors.The paper attributes this result to projection error in the ESDF construction process.
  • Surface-band construction: All evaluated TSDF-based methods have significantly lower error than occupancy-derived values.
  • Distance computation: Full Euclidean distance reduces error by 8.23%, 5.18%, and 4.72% at voxel sizes 0.05, 0.10, and 0.20 meters, respectively.The corresponding integration-time increases are 201.0%, 61.3%, and 33.9%.
  • Distance computation: Quasi-Euclidean distance is recommended as a runtime-accuracy trade-off for real-time ESDF construction.

2) Real Data:

Incremental ESDF updates substantially accelerate construction over a full dataset, with queue choice depending on voxel size. At large voxels, ESDF updates continue getting faster while TSDF integration plateaus.

  • Incremental updates: Incremental ESDF construction provides an order-of-magnitude speedup over batch construction across the EuRoC dataset.
  • Queueing strategy: A normal FIFO queue performs best at small voxel sizes, while a single-insert priority queue is faster at large voxel sizes.
  • Relative timings: At voxel size 0.20 meters, TSDF integration time flattens while ESDF update time continues decreasing.The TSDF plateau occurs because the number of sensor points being projected does not vary with voxel size.
  • Recommended configuration: The recommended ESDF configuration uses a single-voxel fixed band, quasi-Euclidean distance, and a priority queue.

VII. MAV PLANNING EXPERIMENTS

The voxblox system incrementally updates TSDF-derived ESDFs for onboard trajectory-optimization replanning as an MAV explores unknown environments. It runs in real time within the platform’s computational budget while using explicit handling of unknown space.

  • System integration: The experiment integrates stereo-inertial sensing, TSDF mapping, ESDF updates, trajectory optimization, and control entirely onboard an MAV.The system uses a forward-facing stereo camera synced to an IMU for mapping input and state estimation.
  • System integration: ESDF updates occur at 4 Hz, with replanning after every map update using 0.20 meter voxels.The planner relies on smooth collision costs and gradients from the Euclidean distance map.
  • Real-time performance: The complete system runs well under the 250 ms time budget, including TSDF construction, ESDF updates, replanning, and random optimization restarts.
  • Unknown-space handling: Unknown voxels within a 5 meter sphere around the robot are marked occupied, while a 1 meter sphere around the start position is marked free.These changes provide the planner with collision information near the vehicle despite its inability to handle unknown space.
  • Planning outcome: The mapping approach navigates an MAV to a waypoint in an unknown environment while continually replanning around obstacles in real time.The experiment demonstrates operation within the computational limits of the aerial platform.
  • Mapping approach: The system combines incrementally built TSDF-derived ESDFs with voxel hashing to support dynamically growing maps for local MAV planning.The approach uses TSDF distance information rather than occupancy-based representations.
  • Evaluation: The method builds TSDFs faster than Octomaps and produces more accurate ESDFs from TSDFs than from occupancy maps.The complete system is validated with onboard online trajectory-optimization replanning.
Loading 1611.03631v2…