A C++ library with Python bindings for calculating distances and collision probabilities between ellipsoids. This package is designed for efficiency and is suitable for robotics and computer graphics applications.
- Ellipsoid Representation: Represents ellipsoids using their center and inverse shape matrix.
- Distance Calculation: Computes the minimum distance between two ellipsoids.
- Gradient Calculation: Calculates the gradient of the distance function.
- Collision Probability: Approximates the probability of collision between two ellipsoids, considering the uncertainty of one of the ellipsoids.
- Python Bindings: Provides a simple Python interface for the C++ library using nanobind.
- Examples: Includes several Python examples demonstrating the library's usage with
datovizfor visualization. - Benchmarks: Contains C++ and Python benchmarks for performance evaluation.
- C++17 compiler
- CMake (>= 3.15)
- Eigen3 (>= 3.4)
- nanobind
- Python (>= 3.8)
- colcon (for building)
- datoviz (for examples)
- scipy (for examples)
- numpy (for examples)
This package is designed to be built with colcon. See the meta-package gira3d-map-ops for instructions.
This will build the C++ library and the Python bindings. The resulting files will be in the install directory.
The examples directory contains several Python scripts that demonstrate how to use the ellipsoid_utils library. These examples use datoviz for 3D visualization.
single_ellipsoid_pair.py: Visualizes the distance calculation between a single pair of ellipsoids.
single_ellipsoid_pair-1.mov
random_ellipsoid_pairs.py: Generates and visualizes multiple random pairs of ellipsoids and the distances between them.
random_ellipsoid_pairs-1.mov
closest_ellipsoid.py: For a set of red ellipsoids, finds the closest green ellipsoid to each red one.
closest_ellipsoid-1.mov
single_ellipsoid_heatmap.py: Creates a heatmap of distances, gradients, and collision probability from a fixed ellipsoid to a grid of points.
single_ellipsoid_heatmap-1.mov
multi_ellipsoid_heatmap.py: Creates a heatmap of distances from a set of ellipsoids to a grid of points.
multi_ellipsoid_heatmap-1.mov
The benchmarks directory contains scripts for evaluating the performance of the distance and collision probability calculations.
generate_random_ellipsoid_pairs.py: A Python script to generate a specified number of random ellipsoid pairs and save them to a file.ellipsoid_benchmark.cpp: A C++ program that reads the generated ellipsoid pairs and benchmarks the construction, distance/gradient, and collision probability calculations.
-
Generate ellipsoid pairs:
python benchmarks/generate_random_ellipsoid_pairs.py 10000 --output ellipsoid_pairs.txt
This will create a file named
ellipsoid_pairs.txtwith 10,000 random ellipsoid pairs. -
Run the C++ benchmark: After building the project, an executable named
ellipsoid_benchmarkwill be available in theinstall/ellipsoid_utils/directory../install/ellipsoid_utils/ellipsoid_benchmark ellipsoid_pairs.txt
The benchmark will output statistics (count, total time, average time, min, max, and standard deviation) for the different calculations in microseconds.
This project is licensed under the BSD 3-Clause License. See the LICENSE file for details.
A technical introduction to the theory behind this work is provided in our paper, available here.
@inproceedings{Goel_Distance_and_Collision_2025,
author = {Goel, Kshitij and Tabib, Wennie},
title = {{Distance and Collision Probability Estimation from Gaussian Surface Models}},
journal = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
year = {2025}
}
This work was supported in part by an Uber Presidential Fellowship. This material is based upon work supported by, or in part by, the Army Research Laboratory and the Army Research Office under contract/grant number W911NF-25-2- 0153