GPU-accelerated particle filter for SLAM and MCL localization
npm install @novadynamics/pswarm/usr/local/cuda)
node-addon-api: For Node.js native addon support
node-gyp: For building native addons
bash
Add NVIDIA package repositories
wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin
sudo mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600
wget https://developer.download.nvidia.com/compute/cuda/12.0.0/local_installers/cuda-repo-ubuntu2004-12-0-local_12.0.0-525.60.13-1_amd64.deb
sudo dpkg -i cuda-repo-ubuntu2004-12-0-local_12.0.0-525.60.13-1_amd64.deb
sudo cp /var/cuda-repo-ubuntu2004-12-0-local/cuda-*-keyring.gpg /usr/share/keyrings/
sudo apt-get update
sudo apt-get -y install cuda
Verify CUDA installation
/usr/local/cuda/bin/nvcc --version
`
Ensure CUDA is installed at /usr/local/cuda. If installed elsewhere, create a symlink:
`bash
sudo ln -s /usr/local/cuda-12.0 /usr/local/cuda
`
$3
`bash
npm install
`
$3
`bash
npx node-gyp configure
npx node-gyp build
`
Usage
$3
Build a map of an unknown environment:
`bash
With visualization
node example_mapping.js
Without visualization (faster)
node example_mapping.js --no-viz
`
This will:
1. Load sensor data from bigboi_munged.json
2. Run particle SLAM to build a map
3. Save the final map to node_baked_map.bin
4. Display profiling results
$3
Localize the robot in a known map:
`bash
With visualization
node example_mcl.js
Without visualization (faster)
node example_mcl.js --no-viz
`
This will:
1. Load a pre-built map from node_baked_map.bin
2. Load sensor data from one_loop_munged.json
3. Localize the robot using MCL
4. Display position estimates and profiling results
API Reference
$3
`javascript
const { ParticleSlam } = require('./index');
// Create instance
const slam = new ParticleSlam(
num_particles, // Number of particles (e.g., 5000-10000)
max_trajectory_length, // Maximum trajectory history per particle
max_chunk_length, // Maximum visual measurement chunks to buffer
cell_size_m, // Map cell size in meters (default: 0.1)
pos_weight, // Weight for positive observations (default: 0.7)
neg_weight, // Weight for negative observations (default: 0.4)
alpha_prior, // Alpha prior for Beta distribution (default: 1.0)
beta_prior, // Beta prior for Beta distribution (default: 1.5)
measurement_saturation // Max observation counter value (default: 200)
);
`
$3
#### Initialization
`javascript
// Initialize CUDA memory and RNG
slam.init(random_seed = 1234);
`
#### Motion Update
`javascript
// Apply dead reckoning motion step
slam.apply_step(
{ x: dx, y: dy, z: dtheta }, // Motion delta in robot frame
timestamp, // Timestamp in seconds
pos_std = 1.6e-3, // Position noise std dev
yaw_std = 1e-3 // Yaw noise std dev
);
`
#### Measurement Update
`javascript
// Ingest visual measurement (60x60 occupancy grid)
const chunk = {
timestamp: ts,
cells: [[{ num_pos: 0, num_neg: 0 }, ...], ...] // 60x60 array
};
const chunk_idx = slam.ingest_visual_measurement(chunk);
// For SLAM: accumulate map from particle trajectories
slam.accumulate_map_from_trajectories(chunk_idx);
// For MCL: accumulate map from global map
slam.accumulate_map_from_map(chunk_idx);
// Evaluate and resample particles
slam.evaluate_and_resample(chunk_idx);
`
#### State Estimation
`javascript
// Calculate mean and covariance
const measurement = slam.calculate_measurement();
console.log(measurement.mean); // { x, y, z }
console.log(measurement.covariance); // { m: [9 elements] } (row-major 3x3)
console.log(measurement.is_gaussian); // boolean
`
#### Data Retrieval
`javascript
// Download current particle states
const particles = slam.download_current_particle_states();
// Returns: [{ state: { x, y, z }, timestamp }, ...]
// Download particle scores
const scores = slam.download_scores();
// Returns: [score1, score2, ...]
// Download chunk states for a particle
const trajectory = slam.download_chunk_states_for_particle(
particle_idx,
num_chunks
);
// Returns: [{ x, y, z }, ...]
`
#### Map Operations
`javascript
// For MCL: Set global map
slam.set_global_map(map);
// For MCL: Uniformly initialize particles across map
slam.uniform_initialize_particles();
// For MCL: Remove particles outside map bounds
slam.prune_particles_outside_map();
// Bake final map from best particle
const final_map = slam.bake_best_particle_map();
// For MCL: Bake global map from best particle
const updated_map = slam.bake_global_map_best_particle();
`
$3
`javascript
// Load map from file
const { load_map_from_file } = require('./index');
const map = load_map_from_file('map.bin');
// Save map to file
const { save_map_to_file } = require('./index');
save_map_to_file(map, 'map.bin');
`
$3
`javascript
{
width: 800, // Map width in cells
height: 600, // Map height in cells
min_x: -40.0, // Minimum X coordinate (meters)
min_y: -30.0, // Minimum Y coordinate (meters)
max_x: 40.0, // Maximum X coordinate (meters)
max_y: 30.0, // Maximum Y coordinate (meters)
cell_size: 0.1, // Cell size in meters
cells: [ // Flattened array of cells (height * width)
{ num_pos: 10, num_neg: 5 },
// ...
]
}
`
Terminal Visualizer
The included ASCII terminal visualizer provides real-time feedback:
`javascript
const { TermVisualizer } = require('./lib/term_visualizer');
const visualizer = new TermVisualizer({
width: 120, // Terminal width in characters
height: 50, // Terminal height in characters
metersPerChar: 0.2, // Meters per character cell
maxParticles: 200 // Max particles to render
});
visualizer.init();
visualizer.setMap(map); // Optional: set background map
visualizer.render(particles, mean, isGaussian, {
title: 'SLAM Visualization',
info: ['Position: (1.23, 4.56)', 'Particles: 5000']
});
`
Legend:
- ββββββββ - Blue arrows show particle positions and orientations
- β (Green) - Mean pose when distribution is Gaussian
- β (Yellow) - Mean pose when distribution is non-Gaussian
- ββββ - Map occupancy (black = occupied, white = free)
File Structure
`
.
βββ ParticleSlam.h # Main particle filter header
βββ ParticleSlam.cu # CUDA implementation
βββ src/
β βββ pswarm_wrapper.cpp # Node.js addon wrapper
βββ lib/
β βββ term_visualizer.js # ASCII terminal visualizer
βββ example_mapping.js # SLAM example
βββ example_mcl.js # MCL example
βββ binding.gyp # Build configuration
βββ index.js # Node.js entry point
βββ index.d.ts # TypeScript definitions
βββ README.md # This file
`
Input Data Format
The examples expect JSON files with the following format:
`json
[
{
"type": "dr_step",
"ts": 1234567.89,
"value": {
"x": [x, y, theta] // Robot state (odometry)
}
},
{
"type": "map_measurement",
"ts": 1234568.12,
"value": {
"cells": [ // 60x60 grid
[
{"num_pos": 5, "num_neg": 2},
null, // No observation
// ...
],
// ...
]
}
}
]
`
Performance
Typical performance on an NVIDIA RTX 3080 with 10,000 particles:
| Operation | Average Time |
|-----------|-------------|
| apply_step | 0.5-1.0 ms |
| ingest_visual_measurement | 0.3-0.5 ms |
| accumulate_map_from_trajectories | 2-5 ms |
| evaluate_and_resample | 3-8 ms |
| calculate_measurement | 0.5-1.0 ms |
| download_current_particle_states | 0.5-1.0 ms |
Tips for Performance:
- Use --no-viz flag to disable visualization for maximum speed
- Adjust NUM_PARTICLES based on your GPU capability
- Larger max_trajectory_length increases memory usage
- Profile with built-in timing to identify bottlenecks
Troubleshooting
$3
Error: nvcc: command not found
- Ensure CUDA Toolkit is installed at /usr/local/cuda
- Verify nvcc is accessible: /usr/local/cuda/bin/nvcc --version
- If CUDA is installed elsewhere, create a symlink:
`bash
sudo ln -s /path/to/cuda /usr/local/cuda
`
Error: undefined symbol: _ZN6pswarm...
- The build system automatically sets rpath to /usr/local/cuda/lib64
- If issues persist, manually add to library path:
`bash
export LD_LIBRARY_PATH=/usr/local/cuda/lib64:$LD_LIBRARY_PATH
`
Error: Building on Windows
- Windows is not supported. Use Linux or WSL2 (Windows Subsystem for Linux 2)
$3
Error: CUDA error: out of memory
- Reduce NUM_PARTICLES or MAX_TRAJECTORY_LENGTH
- Close other GPU-intensive applications
Visualization flickering
- The alternate screen buffer should eliminate flicker
- If issues persist, use --no-viz flag
Slow performance
- Profile with built-in timing to identify bottlenecks
- Ensure CUDA drivers are up to date
- Check GPU utilization with nvidia-smi
Algorithm Details
$3
Each particle maintains:
- Current state: (x, y, ΞΈ) in world frame
- Trajectory history of states at measurement times
- Individual occupancy map built from observations
$3
Dead reckoning with Gaussian noise:
`
x' = x + Ξx + noise(Ο_pos)
y' = y + Ξy + noise(Ο_pos)
ΞΈ' = ΞΈ + ΞΞΈ + noise(Ο_yaw)
`
$3
60Γ60 occupancy grid centered on robot:
- Each cell contains positive/negative observation counts
- Beta-Bernoulli model for occupancy probability:
- Ξ± = Ξ±_prior + pos_weight Γ num_pos
- Ξ² = Ξ²_prior + neg_weight Γ num_neg
- P(occupied) = Ξ± / (Ξ± + Ξ²)`