FastDEM Elevation Mapping Engine

FastDEM is a lightweight, ultra-fast elevation mapping engine designed for mobile robots. It incrementally fuses LiDAR or RGB-D point clouds into local or global elevation maps.

Key characteristics:

• Performance: Achieves 100+ Hz (approx. 10ms per scan) on embedded hardware like Jetson Orin.
• Lightweight: Runs entirely on CPU and does not require PCL, OpenCV, or CUDA; it relies primarily on Eigen.
• Versatile: Supports multiple estimators (Kalman Filter and P² Quantile), sensor-aware physics models, and various post-processing tasks like raycasting, uncertainty fusion, and inpainting.
• Deployment: Can be used as a standalone C++ library or via ROS1/ROS2 adapters.

nanoPCL is a lightweight, high-performance alternative to PCL (Point Cloud Library) designed for robotics and vision. It focuses on minimal dependencies (Eigen only) and fast compilation.

Key Capabilities:

• Core Layout: SIMD-friendly Structure-of-Arrays (SoA) for reduced memory footprint (approx. 16 bytes/pt for XYZ+I).
• Filters: VoxelGrid, CropBox, and Outlier Removal (Radius/Statistical).
• Registration: ICP, Point-to-Plane, GICP, and VGICP (OpenMP accelerated).
• Search: VoxelHash (O(1) complexity) and KdTree (via nanoflann wrapper).
• Integrations: Built-in adapters for ROS 1, ROS 2, PCL, and Rerun for visualization.

The FastDEM repository includes several example programs categorized by difficulty and functionality:

• 01_hello_mapping (Basic): Demonstrates map setup, integration with explicit-transform, and accessing elevation data.
• 02_config_loading (Basic): Demonstrates how to load YAML configurations using LOCAL or GLOBAL presets.
• 03_estimator_comparison (Intermediate): Compares the performance/behavior of Kalman and P2 Quantile estimators.
• 04_transform_provider (Intermediate): Shows integration with Calibration and Odometry interfaces.

To build the FastDEM example programs, you must enable the FASTDEM_BUILD_EXAMPLES flag during the CMake configuration step. It is recommended to use Release build type for performance testing.

Follow these steps:

1. Navigate to the fastdem directory.
2. Create and enter a build directory.
3. Run cmake with the example flag enabled.
4. Compile using make with parallel jobs.

cd FastDEM/fastdem
mkdir -p build && cd build
cmake .. -DFASTDEM_BUILD_EXAMPLES=ON -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)

Once the build process is complete, the executables are located within the build/examples/ subdirectory. You can run them directly from the build directory.

./build/examples/01_hello_mapping/01_hello_mapping
./build/examples/02_config_loading/02_config_loading
./build/examples/03_estimator_comparison/03_estimator_comparison
./build/examples/04_transform_provider/04_transform_provider

To add a new performance test to the suite, follow these steps:

1. Include benchmark_common.hpp in your source file.
2. Use benchmark::run() to perform the actual measurement.
3. Use benchmark::printResult() to output the results to the console.
4. Follow the naming convention: benchmark_<operation>.cpp.
5. Add an entry for your new benchmark to the benchmarks/README.md file.

To build the nanoPCL benchmarks, you must enable the NANOPCL_BUILD_BENCHMARKS flag during the CMake configuration step. If you wish to run the PCL comparison benchmarks, you must also have libpcl-dev installed on your system.

Requirements

• Core benchmarks: No external dependencies required (only CMake).
• PCL comparison benchmarks: Requires libpcl-dev (sudo apt install libpcl-dev).

Build Instructions

mkdir build && cd build
cmake .. -DNANOPCL_BUILD_BENCHMARKS=ON -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)

After building, you can execute benchmarks directly from their build directory locations.

• Run a single benchmark: Execute the specific binary (e.g., ./profiling/benchmark_voxel_grid).
• Run all profiling benchmarks: Use a shell loop to iterate through the profiling/ directory.
• Run PCL comparisons: Execute binaries in the vs_pcl/ directory (requires PCL installed).

# Run individual benchmark
./profiling/benchmark_voxel_grid

# Run all profiling benchmarks
for f in profiling/benchmark_*; do ./$f; done

# Run PCL comparisons (requires PCL)
./vs_pcl/benchmark_icp

The nanoPCL benchmark framework provides a consistent methodology for performance analysis using benchmark_common.hpp. It includes features like unified iteration policies (FAST, MEDIUM, SLOW), automatic outlier removal using the IQR method, and statistical analysis (mean, stddev, median, 95% CI).

To implement a new benchmark, include benchmark_common.hpp, use benchmark::run() to wrap the code under test, and use the provided print functions for formatted output.

#include "benchmark_common.hpp"

int main() {
    benchmark::printHeader("My Benchmark");
    benchmark::PlatformInfo::capture().print();

    auto stats = benchmark::run([&]() {
        return myFunction();
    });

    benchmark::printResult("myFunction", stats);
    benchmark::printFooter("Conclusion here");
    return 0;
}