Road-network Gaussian Mixture Model localization in GPS-denied environments.
Research project for the National Geo-spatial Intelligence Agency (NGA), conducted at Ohio State University.
GPS denial — from jamming, spoofing, or the urban-canyon effect — is a critical operational challenge for autonomous navigation. This project develops a probabilistic geo-localization system that estimates a vehicle's absolute position on a road network using only visual odometry (monocular or stereo camera) — no GPS, no IMU beyond the camera, and no prior traversal of the route.
The key insight is that a vehicle's trajectory is constrained to lie on a known road network. A Gaussian Mixture Model (GMM) is maintained over all feasible road positions simultaneously; at each timestep an odometry observation updates the mixture and low-probability components are pruned using a KL-divergence threshold. The result converges, typically within a few hundred metres of travel, to a tight unimodal distribution centred on the true road position.
Operational context: Sponsored by the National Geo-spatial Intelligence Agency (NGA), the work targets autonomous ground vehicles operating in GPS-contested urban environments and is evaluated on the publicly available KITTI odometry dataset.
flowchart TD
subgraph Offline
A["OSM Binary .bin<br/>C++ parser"] --> B["MapBuilder.py<br/>build_osm_map"]
B --> C["MapGraph.pickle<br/>road network graph"]
end
subgraph Online
D["Odometry observations<br/>.obs / .txt"] --> E["map_localization.py<br/>do_inference"]
C --> E
E --> F{MPI mode?}
F -- Yes --> G["Distributed GMM<br/>per MPI rank"]
F -- No --> H[Single-process GMM]
G --> I["Position estimate<br/>+ confidence"]
H --> I
end
subgraph Evaluation
I --> J["Error vs. GPS ground truth<br/>per-frame .csv / .p"]
J --> K["Statistics: median error<br/>90th-pctile, Recall@Xm"]
end
sequenceDiagram
participant Obs as Odometry Source
participant Inf as do_inference()
participant GMM as MapGMMDistribution
participant MPI as MPI Exchange
participant Out as Output
Obs->>Inf: observation vector (v, dθ, …) at time t
Inf->>GMM: predict() — propagate each component via MapState2RigidTurnDynamics
Inf->>GMM: update() — multiply by observation likelihood
Inf->>GMM: simplify() — KL-prune low-weight components
Inf->>MPI: exchange boundary-road distributions across ranks (if MPI)
MPI-->>GMM: merge incoming cross-rank components
GMM-->>Inf: MAP estimate (road, arc-length)
Inf->>Out: (lat, lon) + error vs. ground truth
Results on KITTI odometry sequences 00 – 10, urban driving, stereo odometry (libviso2):
| Method | Median Error (m) | 90th percentile (m) | Recall @ 20 m |
|---|---|---|---|
| GPS observation (oracle upper bound) | 2.1 | 6.4 | 0.97 |
| Stereo odometry + GMM (2-kind dynamics) | 8.3 | 22.1 | 0.88 |
| Mono odometry + GMM (2-kind dynamics) | 14.6 | 41.0 | 0.71 |
| Stereo odometry + GMM (1-kind dynamics) | 10.2 | 28.5 | 0.81 |
The GMM filter consistently converges to single-road resolution within 200–400 metres of travel from a uniform prior over the full road network.
gps-denied-geospatial-positioning/
│
├── GMM/ # Core inference engine
│ ├── map_localization.py # Top-level driver — inference loop + MPI
│ ├── MapGraph.py # Road-network graph + MapGMMDistribution
│ ├── MapBuilder.py # Map construction from OSM binary data
│ ├── Statistics.py # Gaussian mixture model primitives
│ ├── Utils.py # Numerical utilities (logsumexp, circle fit, …)
│ ├── bindingstest.py # Smoke test for the C++ osm_objects bindings
│ │
│ ├── configs/ # Experiment configuration files (.dcfg)
│ │ ├── 00-gps.dcfg # Sequence 00, GPS observations
│ │ ├── 00-stereo.dcfg # Sequence 00, stereo odometry
│ │ ├── 00-mono.dcfg # Sequence 00, mono odometry
│ │ ├── *-1kind.dcfg # 1-kind dynamics variants
│ │ └── noise/ # Noise-robustness sweep configs
│ │
│ ├── bindings.cpp # pybind11 Python bindings for osm_objects
│ ├── osm.h / osm.cpp # OSM binary-format parser (C++)
│ ├── objects.h / objects.cpp # OSM node / way / relation types (C++)
│ ├── coordinates.h/cpp # Coordinate-system transforms (C++)
│ ├── mapwidget.h/cpp # Qt map-rendering widget (C++)
│ ├── mapviewer.h/cpp/.ui # Interactive Qt map viewer application
│ └── CMakeLists.txt # CMake build for C++ tools + pybind11 module
│
├── GeoLoc/
│ └── trajLoc.py # Trajectory-contour localization (research stub)
│
├── src/ # Supporting algorithms and utilities
│ ├── osmnetx.py # OSM graph reading via NetworkX
│ ├── traceMatching.py # Trajectory-to-road matching
│ ├── genTrajectoryFeat.py # Spline-based trajectory feature generation
│ ├── esriShapefile.py # ESRI shapefile I/O (pyshp)
│ ├── roadSearch.py # Road network path search
│ ├── matchEdge.py # Edge-to-edge matching algorithms
│ └── … # Additional graph / analysis utilities
│
├── data/
│ ├── map_trajectories/ # Ground-truth GPS trajectories (KITTI sequences)
│ └── odometry/
│ ├── libviso2_mono/ # Monocular odometry (libviso2)
│ └── libviso2_stereo/ # Stereo odometry (libviso2)
│
├── publications/ # Research papers and result archives
├── pyproject.toml # Project metadata and uv/pip dependencies
├── .python-version # Pinned Python version (2.7)
├── LICENSE # MIT License
└── README.md
| Tool | Purpose |
|---|---|
| Python 2.7 | Runtime (code uses Python 2 syntax throughout) |
| CMake ≥ 3.5 | Build the C++ OSM parser and pybind11 module |
| MPI (OpenMPI / MPICH) | Distributed inference (optional for single-node use) |
| Qt 5 | Interactive map viewer (optional) |
uv is a fast Python package manager that respects .python-version and pyproject.toml:
# Install uv (once)
curl -LsSf https://astral.sh/uv/install.sh | sh
# Clone the repository
git clone https://github.com/ashish-code/gps-denied-geospatial-positioning.git
cd gps-denied-geospatial-positioning
# Create a virtual environment pinned to Python 2.7 and install dependencies
uv python install 2.7 # download CPython 2.7 if not already present
uv sync # creates .venv and installs all dependencies
# Activate the environment
source .venv/bin/activateTo install optional extras:
uv sync --extra osm # adds pyshp + pysal for shapefile / OSM utilities
uv sync --extra viewer # adds PyQt5 for the interactive map viewer
uv sync --extra dev # adds pytest + ruffgit clone https://github.com/ashish-code/gps-denied-geospatial-positioning.git
cd gps-denied-geospatial-positioning
python2.7 -m pip install numpy scipy networkx matplotlib mpi4pycd GMM
mkdir build && cd build
cmake ..
make -j$(nproc)
# Copy or symlink the generated osm_objects*.so back to GMM/The road-network graph is built once from an OSM binary file produced by the
mapviewer C++ tool and saved as a pickle:
# Inside GMM/
from MapBuilder import build_osm_map
import cPickle as pickle
import osm_objects # C++ pybind11 extension
osm = osm_objects.Objects(True)
osm.loadFromFile('../../data/00.bin')
mapgraph = build_osm_map(osm)
with open('../../data/00.p', 'wb') as f:
pickle.dump(mapgraph, f, protocol=2)cd GMM
python map_localization.py configs/00-stereo.dcfgProgress is printed to stdout; per-frame estimates are written to the output directory specified in the config file.
cd GMM
mpiexec -n 8 python map_localization.py configs/00-stereo.dcfgThe road network is partitioned across 8 ranks; boundary distributions are exchanged at each inference step.
Config files use Python's ConfigParser [section] key=value format.
Example (configs/00-stereo.dcfg):
[sequence_data]
data_dir = ../../data
map = 00.p ; road-network pickle
dataname = 00stereo
odometry = %(data_dir)s/odometry/libviso2_stereo/00.txt
gps_data = %(data_dir)s/map_trajectories/00.txt
dt = 0.1 ; 100 ms time step
data_skip = 10 ; use every 10th frame
dynamics_params = params/stereo_dynamics_params.p
data_source = stereo
sequence_id = 00
snr = 0Available data_source values: GPS, mono, stereo.
Config variants ending in -1kind use the simplified MapState1RigidDynamics
motion model (speed only, no angular rate).
- Road-constrained state space: Vehicle positions are represented as arc-length coordinates on individual road segments rather than raw (x, y), dramatically reducing the effective state space.
- Gaussian Mixture Model filter: Multiple competing position hypotheses are
propagated simultaneously; the mixture collapses naturally as evidence
accumulates.
Statistics.GaussianMixtureModel.simplify()prunes components via a variational KL upper-bound to keep computation tractable. - Rigid constant-turn dynamics:
MapState2RigidTurnDynamicspropagates speed and angular rate as latent states, enabling accurate prediction on curved roads without explicit curvature estimation from the camera. - Distributed inference:
partition_graph_by_sortdivides the road graph by geographic position across MPI ranks.determine_local_communicationspre-computes the sparse inter-rank exchange pattern so that each inference step only communicates across road boundaries actually traversed. - Geometry primitives: Road segments are modelled as lines or circular arcs
(fitted by
Utils.fit_circle_with_endpoints), allowing accurate arc-length integration and smooth probability-mass transfer at intersections.
- Gupta, A. (2016). GPS-Denied Geospatial Positioning Using Road-Network Priors. Ohio State University / NGA.
- Geiger, A. et al. (2012). Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite. CVPR.
- Viorela Ila, Juan M. Porta, Juan Andrade-Cetto. (2010). Information-Based Compact Pose SLAM. IEEE T-RO.
- Krajník, T. et al. (2014). Long-term topological localisation for service robots in dynamic environments. IROS.
This project was conducted as academic research under sponsorship of the National Geo-spatial Intelligence Agency at Ohio State University. No classified data is used or included. All odometry data is derived from the publicly available KITTI dataset.
MIT License — see LICENSE for details.