Enuxia-Public/enuxia-vacuum
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add Cartographer-style IMU fusion and motion filtering

Browse Source 
Implement pose extrapolation with odometry + IMU blending and scan
insertion throttling for improved localization accuracy:

- ImuTracker: gravity estimation and gyroscope integration
- PoseExtrapolator: odometry-primary pose prediction with IMU blending
- MotionFilter: distance/rotation/time-based scan throttling
- ImuMeasurement: 6-axis sensor type with raw i16 conversion
- LocalizerConfig: optional motion filtering configuration
- Localizer: add_imu() and localize_with_timestamp() methods
This commit is contained in:
codetiger
2026-01-01 18:26:33 +05:30
parent e66268db0f
commit 5eff1eef55
11 changed files with 2140 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-slam/src/config/mod.rs
+36 -1
View File
@@ -16,6 +16,9 @@
//! // Convert to runtime configs
//! let map_config = config.to_map_config();
//! let matcher_config = config.correlative_matcher_config();
//!
//! // Create localizer config with motion filtering (if configured)
//! let localizer_config = config.to_localizer_config();
//! ```
//!
//! ## Configuration Sections
@@ -24,7 +27,7 @@
//! |---------|-------------|
//! | [`GridSection`] | Grid dimensions, resolution, origin |
//! | [`SensorSection`] | Robot geometry, LiDAR, cliff, bumper |
//! | [`SlamSection`] | Matcher, loop closure, pose graph settings |
//! | [`SlamSection`] | Matcher, loop closure, pose graph, motion filtering |
//! | [`PersistenceSection`] | Output format and directory |
//!
//! ## Example YAML
@@ -45,6 +48,38 @@
//! correlative:
//! enabled: true
//! multi_resolution: true
//!
//! # Optional: IMU fusion for improved pose estimation
//! pose_extrapolator:
//! pose_queue_duration: 0.5 # seconds
//! odom_queue_duration: 0.5 # seconds
//! imu_gravity_time_constant: 10.0 # seconds
//! imu_rotation_weight: 0.3 # 0.0=odom only, 1.0=IMU only
//!
//! # Optional: Scan insertion throttling
//! motion_filter:
//! max_time_seconds: 5.0 # insert at least every 5s
//! max_distance_meters: 0.2 # insert if moved 20cm
//! max_angle_radians: 0.035 # insert if rotated ~2°
//! ```
//!
//! ## Motion Filtering
//!
//! Enable Cartographer-style motion filtering for improved accuracy:
//!
//! ```rust,ignore
//! use vastu_slam::config::VastuConfig;
//!
//! let config = VastuConfig::load_default()?;
//!
//! // Check if motion filtering is enabled
//! if config.has_motion_filtering() {
//! let extrapolator = config.pose_extrapolator_config();
//! let filter = config.motion_filter_config();
//! }
//!
//! // Create complete localizer config
//! let localizer_config = config.to_localizer_config();
//! ```
mod defaults;
vastu-slam/src/config/slam.rs
+15
View File
@@ -2,6 +2,7 @@
use serde::{Deserialize, Serialize};
use crate::core::{MotionFilterConfig, PoseExtrapolatorConfig};
use crate::grid::LogOddsConfig;
use crate::matching::{
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
@@ -29,4 +30,18 @@ pub struct SlamSection {
/// Background optimizer configuration
#[serde(default)]
pub background_optimizer: BackgroundOptimizerConfig,
/// Pose extrapolator configuration for IMU fusion.
///
/// When set, enables Cartographer-style pose extrapolation with
/// odometry (primary) and IMU (secondary) fusion.
#[serde(default)]
pub pose_extrapolator: Option<PoseExtrapolatorConfig>,
/// Motion filter configuration for scan insertion throttling.
///
/// When set, only processes scans when the robot has moved enough
/// (distance, rotation, or time since last scan).
#[serde(default)]
pub motion_filter: Option<MotionFilterConfig>,
}
vastu-slam/src/config/vastu.rs
+85
View File
@@ -4,10 +4,12 @@ use std::path::Path;
use serde::{Deserialize, Serialize};
use crate::core::{MotionFilterConfig, PoseExtrapolatorConfig};
use crate::grid::MapConfig;
use crate::matching::{
BackgroundOptimizerConfig, CorrelativeMatcherConfig, LoopClosureConfig, PoseGraphConfig,
};
use crate::modes::LocalizerConfig;
use super::error::ConfigLoadError;
use super::grid::GridSection;
@@ -91,6 +93,37 @@ impl VastuConfig {
pub fn robot_radius(&self) -> f32 {
self.sensor.robot.radius
}
/// Get the pose extrapolator config (for IMU fusion).
///
/// Returns `None` if IMU fusion is not configured.
pub fn pose_extrapolator_config(&self) -> Option<PoseExtrapolatorConfig> {
self.slam.pose_extrapolator.clone()
}
/// Get the motion filter config (for scan throttling).
///
/// Returns `None` if motion filtering is not configured.
pub fn motion_filter_config(&self) -> Option<MotionFilterConfig> {
self.slam.motion_filter.clone()
}
/// Check if motion filtering (IMU fusion + scan throttling) is enabled.
pub fn has_motion_filtering(&self) -> bool {
self.slam.pose_extrapolator.is_some() || self.slam.motion_filter.is_some()
}
/// Create a LocalizerConfig from the SLAM settings.
///
/// This combines correlative matcher settings with optional motion filtering.
pub fn to_localizer_config(&self) -> LocalizerConfig {
LocalizerConfig {
matcher: self.slam.correlative.clone(),
pose_extrapolator: self.slam.pose_extrapolator.clone(),
motion_filter: self.slam.motion_filter.clone(),
..Default::default()
}
}
}
#[cfg(test)]
@@ -119,4 +152,56 @@ mod tests {
assert_eq!(map_config.grid.resolution, 0.025);
assert_eq!(map_config.sensor.robot_radius, 0.17);
}
#[test]
fn test_motion_filtering_disabled_by_default() {
let config = VastuConfig::default();
assert!(!config.has_motion_filtering());
assert!(config.pose_extrapolator_config().is_none());
assert!(config.motion_filter_config().is_none());
}
#[test]
fn test_motion_filtering_from_yaml() {
let yaml = r#"
slam:
pose_extrapolator:
pose_queue_duration: 0.5
imu_gravity_time_constant: 10.0
imu_rotation_weight: 0.3
motion_filter:
max_time_seconds: 5.0
max_distance_meters: 0.2
max_angle_radians: 0.035
"#;
let config = VastuConfig::from_yaml(yaml).unwrap();
assert!(config.has_motion_filtering());
let extrapolator = config.pose_extrapolator_config().unwrap();
assert!((extrapolator.imu_rotation_weight - 0.3).abs() < 0.01);
let filter = config.motion_filter_config().unwrap();
assert!((filter.max_distance_meters - 0.2).abs() < 0.01);
}
#[test]
fn test_to_localizer_config() {
let yaml = r#"
slam:
correlative:
enabled: true
linear_search_window: 0.5
pose_extrapolator:
imu_rotation_weight: 0.4
motion_filter:
max_distance_meters: 0.15
"#;
let config = VastuConfig::from_yaml(yaml).unwrap();
let localizer_config = config.to_localizer_config();
assert!(localizer_config.matcher.enabled);
assert!(localizer_config.pose_extrapolator.is_some());
assert!(localizer_config.motion_filter.is_some());
assert!((localizer_config.motion_filter.unwrap().max_distance_meters - 0.15).abs() < 0.01);
}
}
vastu-slam/src/core/imu_tracker.rs
+454
View File
@@ -0,0 +1,454 @@
//! IMU tracking with gravity estimation.
//!
//! This module implements Cartographer-style IMU tracking using an
//! exponential filter for gravity estimation and gyroscope integration
//! for orientation tracking.
//!
//! # Algorithm
//!
//! The ImuTracker uses two main components:
//!
//! 1. **Gravity Estimation**: Exponential smoothing filter on accelerometer
//! readings to estimate the gravity direction. This allows detecting
//! when the robot is tilted.
//!
//! 2. **Orientation Integration**: Gyroscope readings are integrated over
//! time to track the robot's yaw angle.
//!
//! # Cartographer Approach
//!
//! This follows Google Cartographer's approach:
//! - Uses exponential filter instead of Kalman filter (simpler, works well)
//! - Time constant balances noise rejection vs responsiveness
//! - For 2D SLAM, primarily tracks yaw (rotation around gravity axis)
//!
//! # Example
//!
//! ```rust,ignore
//! use vastu_slam::core::{ImuTracker, ImuTrackerConfig, ImuMeasurement};
//!
//! let config = ImuTrackerConfig::default();
//! let mut tracker = ImuTracker::new(config);
//!
//! // Feed IMU measurements at ~110 Hz
//! tracker.add_imu(&imu_measurement);
//!
//! // Get current yaw estimate
//! let yaw = tracker.yaw();
//! ```
use serde::{Deserialize, Serialize};
use super::ImuMeasurement;
/// Configuration for the IMU tracker.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct ImuTrackerConfig {
/// Time constant for gravity estimation filter (seconds).
///
/// Higher values make the filter slower to respond but more stable.
/// Lower values make it faster but noisier.
///
/// Default: 10.0 seconds (Cartographer default)
pub gravity_time_constant: f32,
/// Initial gravity vector assumption [x, y, z] in m/s².
///
/// For a level robot: [0, 0, 9.81]
/// Default: [0.0, 0.0, 9.81]
pub initial_gravity: [f32; 3],
}
impl Default for ImuTrackerConfig {
fn default() -> Self {
Self {
gravity_time_constant: 10.0,
initial_gravity: [0.0, 0.0, 9.81],
}
}
}
/// IMU tracker for gravity estimation and orientation tracking.
///
/// Uses Cartographer-style exponential smoothing for gravity and
/// gyroscope integration for orientation.
#[derive(Clone, Debug)]
pub struct ImuTracker {
/// Configuration
config: ImuTrackerConfig,
/// Estimated gravity vector (not normalized, in m/s²)
gravity: [f32; 3],
/// Current yaw angle (radians, CCW positive from initial heading)
yaw: f32,
/// Integrated pitch from gyroscope (for tilt compensation)
pitch: f32,
/// Integrated roll from gyroscope (for tilt compensation)
roll: f32,
/// Last IMU timestamp for delta calculation
last_timestamp_us: Option<u64>,
/// Number of IMU measurements processed
measurement_count: u64,
}
impl ImuTracker {
/// Create a new IMU tracker with the given configuration.
pub fn new(config: ImuTrackerConfig) -> Self {
let gravity = config.initial_gravity;
Self {
config,
gravity,
yaw: 0.0,
pitch: 0.0,
roll: 0.0,
last_timestamp_us: None,
measurement_count: 0,
}
}
/// Process an IMU measurement.
///
/// Updates the gravity estimate using exponential smoothing and
/// integrates the gyroscope for orientation tracking.
pub fn add_imu(&mut self, imu: &ImuMeasurement) {
// Calculate time delta
let dt = if let Some(last_ts) = self.last_timestamp_us {
if imu.timestamp_us <= last_ts {
// Non-monotonic timestamp, skip
return;
}
(imu.timestamp_us - last_ts) as f32 / 1_000_000.0 // microseconds to seconds
} else {
// First measurement, just initialize
self.last_timestamp_us = Some(imu.timestamp_us);
self.gravity = imu.accel();
self.measurement_count += 1;
return;
};
// Sanity check on dt (reject unreasonably large gaps)
if dt > 1.0 {
// Gap too large, reset tracking
self.last_timestamp_us = Some(imu.timestamp_us);
self.gravity = imu.accel();
self.measurement_count += 1;
return;
}
// Update gravity using exponential filter
// alpha = exp(-dt / time_constant)
// gravity = alpha * gravity + (1 - alpha) * accel
let alpha = (-dt / self.config.gravity_time_constant).exp();
let accel = imu.accel();
self.gravity[0] = alpha * self.gravity[0] + (1.0 - alpha) * accel[0];
self.gravity[1] = alpha * self.gravity[1] + (1.0 - alpha) * accel[1];
self.gravity[2] = alpha * self.gravity[2] + (1.0 - alpha) * accel[2];
// Integrate gyroscope for orientation
// Simple Euler integration (sufficient for small dt)
self.roll += imu.gyro_x * dt;
self.pitch += imu.gyro_y * dt;
self.yaw += imu.gyro_z * dt;
// Normalize yaw to [-PI, PI]
self.yaw = normalize_angle(self.yaw);
self.last_timestamp_us = Some(imu.timestamp_us);
self.measurement_count += 1;
}
/// Get the current yaw angle estimate (radians, CCW positive).
///
/// This is the primary output for 2D SLAM - the rotation around
/// the gravity axis.
#[inline]
pub fn yaw(&self) -> f32 {
self.yaw
}
/// Get the current pitch angle estimate (radians).
#[inline]
pub fn pitch(&self) -> f32 {
self.pitch
}
/// Get the current roll angle estimate (radians).
#[inline]
pub fn roll(&self) -> f32 {
self.roll
}
/// Get the estimated gravity vector [x, y, z] in m/s².
///
/// For a level robot, this should be approximately [0, 0, 9.81].
#[inline]
pub fn gravity(&self) -> [f32; 3] {
self.gravity
}
/// Get the normalized gravity direction vector.
///
/// Returns a unit vector pointing in the gravity direction.
pub fn gravity_direction(&self) -> [f32; 3] {
let mag = (self.gravity[0] * self.gravity[0]
+ self.gravity[1] * self.gravity[1]
+ self.gravity[2] * self.gravity[2])
.sqrt();
if mag > 0.001 {
[
self.gravity[0] / mag,
self.gravity[1] / mag,
self.gravity[2] / mag,
]
} else {
[0.0, 0.0, 1.0] // Default to vertical if magnitude is too small
}
}
/// Get the tilt angle (radians) - angle between gravity and vertical.
///
/// Returns 0.0 when the robot is level, increases as it tilts.
pub fn tilt_angle(&self) -> f32 {
let dir = self.gravity_direction();
// Tilt is the angle between gravity direction and Z axis
// cos(theta) = dot(gravity_dir, [0,0,1]) = gravity_dir[2]
dir[2].clamp(-1.0, 1.0).acos()
}
/// Check if the robot is approximately level.
///
/// Returns true if tilt angle is less than the given threshold (radians).
pub fn is_level(&self, threshold_radians: f32) -> bool {
self.tilt_angle() < threshold_radians
}
/// Get the number of IMU measurements processed.
#[inline]
pub fn measurement_count(&self) -> u64 {
self.measurement_count
}
/// Reset the tracker to initial state.
pub fn reset(&mut self) {
self.gravity = self.config.initial_gravity;
self.yaw = 0.0;
self.pitch = 0.0;
self.roll = 0.0;
self.last_timestamp_us = None;
self.measurement_count = 0;
}
/// Reset only the yaw angle (useful when aligning with odometry).
pub fn reset_yaw(&mut self, new_yaw: f32) {
self.yaw = normalize_angle(new_yaw);
}
/// Advance the orientation estimate without a new IMU measurement.
///
/// Uses the last known angular velocity to extrapolate forward.
/// Useful for predicting pose between IMU samples.
pub fn advance(&mut self, dt_seconds: f32, angular_velocity_z: f32) {
self.yaw += angular_velocity_z * dt_seconds;
self.yaw = normalize_angle(self.yaw);
}
/// Get the last timestamp processed.
pub fn last_timestamp_us(&self) -> Option<u64> {
self.last_timestamp_us
}
}
/// Normalize angle to [-PI, PI] range.
#[inline]
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle;
while a > std::f32::consts::PI {
a -= 2.0 * std::f32::consts::PI;
}
while a < -std::f32::consts::PI {
a += 2.0 * std::f32::consts::PI;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_imu_tracker_creation() {
let tracker = ImuTracker::new(ImuTrackerConfig::default());
assert_eq!(tracker.yaw(), 0.0);
assert_eq!(tracker.measurement_count(), 0);
}
#[test]
fn test_gravity_estimation_level() {
let mut tracker = ImuTracker::new(ImuTrackerConfig::default());
// Feed level readings (gravity in Z)
for i in 0..100 {
let imu = ImuMeasurement::new(
i * 10000, // 10ms intervals
[0.0, 0.0, 9.81],
[0.0, 0.0, 0.0],
);
tracker.add_imu(&imu);
}
let gravity = tracker.gravity();
assert!((gravity[0]).abs() < 0.1);
assert!((gravity[1]).abs() < 0.1);
assert!((gravity[2] - 9.81).abs() < 0.1);
assert!(tracker.is_level(0.1)); // Less than 0.1 rad tilt
}
#[test]
fn test_yaw_integration() {
let mut tracker = ImuTracker::new(ImuTrackerConfig::default());
// Rotate at 1 rad/s for 1 second (100 samples at 10ms)
for i in 0..100 {
let imu = ImuMeasurement::new(
i * 10000, // 10ms intervals
[0.0, 0.0, 9.81],
[0.0, 0.0, 1.0], // 1 rad/s CCW
);
tracker.add_imu(&imu);
}
// Should have rotated about 1 radian (minus first sample)
let expected_yaw = 0.99; // 99 * 0.01s * 1 rad/s
assert!(
(tracker.yaw() - expected_yaw).abs() < 0.1,
"yaw = {}, expected ~{}",
tracker.yaw(),
expected_yaw
);
}
#[test]
fn test_yaw_integration_negative() {
let mut tracker = ImuTracker::new(ImuTrackerConfig::default());
// Rotate at -0.5 rad/s (CW) for 1 second
for i in 0..100 {
let imu = ImuMeasurement::new(
i * 10000,
[0.0, 0.0, 9.81],
[0.0, 0.0, -0.5], // -0.5 rad/s CW
);
tracker.add_imu(&imu);
}
let expected_yaw = -0.495; // 99 * 0.01s * -0.5 rad/s
assert!(
(tracker.yaw() - expected_yaw).abs() < 0.1,
"yaw = {}, expected ~{}",
tracker.yaw(),
expected_yaw
);
}
#[test]
fn test_reset_yaw() {
let mut tracker = ImuTracker::new(ImuTrackerConfig::default());
// Integrate some yaw
for i in 0..50 {
let imu = ImuMeasurement::new(i * 10000, [0.0, 0.0, 9.81], [0.0, 0.0, 1.0]);
tracker.add_imu(&imu);
}
assert!(tracker.yaw().abs() > 0.1);
// Reset yaw
tracker.reset_yaw(0.0);
assert_eq!(tracker.yaw(), 0.0);
}
#[test]
fn test_advance() {
let mut tracker = ImuTracker::new(ImuTrackerConfig::default());
// Initialize with one sample
let imu = ImuMeasurement::new(0, [0.0, 0.0, 9.81], [0.0, 0.0, 0.0]);
tracker.add_imu(&imu);
// Advance by 0.5 seconds at 1 rad/s
tracker.advance(0.5, 1.0);
assert!((tracker.yaw() - 0.5).abs() < 0.01);
}
#[test]
fn test_tilt_detection() {
let mut tracker = ImuTracker::new(ImuTrackerConfig {
gravity_time_constant: 0.1, // Fast response for test
initial_gravity: [0.0, 0.0, 9.81],
});
// Feed tilted readings (gravity in Y direction = 90 degree tilt)
for i in 0..100 {
let imu = ImuMeasurement::new(i * 10000, [0.0, 9.81, 0.0], [0.0, 0.0, 0.0]);
tracker.add_imu(&imu);
}
// Should detect significant tilt
assert!(tracker.tilt_angle() > 1.0); // More than 1 radian tilt
assert!(!tracker.is_level(0.5));
}
#[test]
fn test_normalize_angle() {
assert!((normalize_angle(0.0) - 0.0).abs() < 1e-6);
assert!((normalize_angle(std::f32::consts::PI) - std::f32::consts::PI).abs() < 1e-6);
assert!(
(normalize_angle(2.0 * std::f32::consts::PI) - 0.0).abs() < 1e-6,
"2PI should normalize to 0"
);
assert!(
(normalize_angle(-2.0 * std::f32::consts::PI) - 0.0).abs() < 1e-6,
"-2PI should normalize to 0"
);
assert!(
(normalize_angle(3.0 * std::f32::consts::PI) - std::f32::consts::PI).abs() < 1e-6,
"3PI should normalize to PI"
);
}
#[test]
fn test_non_monotonic_timestamp() {
let mut tracker = ImuTracker::new(ImuTrackerConfig::default());
tracker.add_imu(&ImuMeasurement::new(
100000,
[0.0, 0.0, 9.81],
[0.0, 0.0, 1.0],
));
tracker.add_imu(&ImuMeasurement::new(
200000,
[0.0, 0.0, 9.81],
[0.0, 0.0, 1.0],
));
let yaw_before = tracker.yaw();
let count_before = tracker.measurement_count();
// Non-monotonic timestamp should be ignored
tracker.add_imu(&ImuMeasurement::new(
150000,
[0.0, 0.0, 9.81],
[0.0, 0.0, 10.0],
));
assert_eq!(tracker.yaw(), yaw_before);
assert_eq!(tracker.measurement_count(), count_before);
}
}
vastu-slam/src/core/mod.rs
+52 -2
View File
@@ -25,14 +25,20 @@
//! - [`LidarScan`]: 360° range measurements in polar coordinates
//! - [`CliffSensors`]: Four IR floor drop-off detectors
//! - [`BumperSensors`]: Two mechanical collision sensors
//! - [`ImuMeasurement`]: 6-axis IMU readings (accelerometer + gyroscope)
//! - [`SensorObservation`]: Combined sensor snapshot at a timestamp
//!
//! ### Motion Filtering (Cartographer-style)
//! - [`ImuTracker`]: Gravity estimation and orientation tracking from IMU
//! - [`PoseExtrapolator`]: Pose prediction with odometry + IMU fusion
//! - [`MotionFilter`]: Scan insertion throttling based on motion
//!
//! ### SIMD Operations
//! - [`PointCloud`]: Structure-of-Arrays point storage for vectorized operations
//! - [`RotationMatrix4`]: Pre-computed rotation for batch transforms
//! - [`GridCoordBatch`]: Batch of grid coordinates for SIMD processing
//!
//! ## Example
//! ## Example: Basic Types
//!
//! ```rust,ignore
//! use vastu_slam::core::{Pose2D, WorldPoint, LidarScan};
@@ -50,6 +56,40 @@
//! let points = PointCloud::from_scan(&scan);
//! let world_points = transform_points(&points, &pose);
//! ```
//!
//! ## Example: Motion Filtering with IMU
//!
//! ```rust,ignore
//! use vastu_slam::core::{
//! ImuMeasurement, ImuTracker, ImuTrackerConfig,
//! PoseExtrapolator, PoseExtrapolatorConfig,
//! MotionFilter, MotionFilterConfig,
//! Pose2D,
//! };
//!
//! // Create components with default configs
//! let mut imu_tracker = ImuTracker::new(ImuTrackerConfig::default());
//! let mut extrapolator = PoseExtrapolator::new(PoseExtrapolatorConfig::default());
//! let mut filter = MotionFilter::new(MotionFilterConfig::default());
//!
//! // Feed IMU at high rate (~110 Hz)
//! let imu = ImuMeasurement::from_raw(timestamp_us, [0, 0, 100], [0, 0, 16384]);
//! imu_tracker.add_imu(&imu);
//! extrapolator.add_imu(&imu);
//!
//! // Feed odometry
//! let odom_delta = Pose2D::new(0.01, 0.0, 0.001); // 1cm forward, 0.06° rotation
//! extrapolator.add_odometry(odom_delta, timestamp_us);
//!
//! // Get extrapolated pose
//! let predicted = extrapolator.extrapolate_pose(timestamp_us + 50_000);
//!
//! // Check if we should process a scan
//! if filter.should_insert(predicted, timestamp_us) {
//! // Process lidar scan
//! filter.accept(predicted, timestamp_us);
//! }
//! ```
mod cell;
mod motion;
@@ -57,11 +97,21 @@ mod point;
mod pose;
mod sensors;
// Motion filtering modules (Cartographer-style)
mod imu_tracker;
mod motion_filter;
mod pose_extrapolator;
pub mod simd;
pub use cell::{Cell, CellType};
pub use motion::{MotionModel, Odometry};
pub use point::{GridCoord, WorldPoint};
pub use pose::{Pose2D, interpolate_pose, normalize_angle};
pub use sensors::{BumperSensors, CliffSensors, LidarScan, SensorObservation};
pub use sensors::{BumperSensors, CliffSensors, ImuMeasurement, LidarScan, SensorObservation};
pub use simd::{GridCoordBatch, PointCloud, RotationMatrix4};
// Motion filtering exports
pub use imu_tracker::{ImuTracker, ImuTrackerConfig};
pub use motion_filter::{MotionFilter, MotionFilterConfig};
pub use pose_extrapolator::{PoseExtrapolator, PoseExtrapolatorConfig, TimedOdometry, TimedPose};
vastu-slam/src/core/motion_filter.rs
+450
View File
@@ -0,0 +1,450 @@
//! Motion filter for scan insertion throttling.
//!
//! This module implements Cartographer-style motion filtering that controls
//! when lidar scans are inserted into the map. By requiring minimum motion
//! between insertions, we reduce computational load and prevent redundant
//! map updates.
//!
//! # Insertion Criteria
//!
//! A scan is inserted when ANY of these conditions are met:
//!
//! 1. **Time**: Maximum time since last insertion exceeded
//! 2. **Distance**: Robot has moved beyond distance threshold
//! 3. **Rotation**: Robot has rotated beyond angle threshold
//!
//! # Cartographer Defaults
//!
//! The default values are based on Cartographer's 2D SLAM configuration:
//! - Max time: 5.0 seconds (insert at least every 5s)
//! - Max distance: 0.2 meters (20cm)
//! - Max angle: 0.035 radians (~2°)
//!
//! # Example
//!
//! ```rust,ignore
//! use vastu_slam::core::{MotionFilter, MotionFilterConfig, Pose2D};
//!
//! let config = MotionFilterConfig::default();
//! let mut filter = MotionFilter::new(config);
//!
//! // Check if scan should be inserted
//! if filter.should_insert(current_pose, timestamp_us) {
//! // Process and insert the scan
//! map.insert_scan(&scan);
//!
//! // Mark as accepted
//! filter.accept(current_pose, timestamp_us);
//! }
//! ```
use serde::{Deserialize, Serialize};
use super::Pose2D;
/// Configuration for the motion filter.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct MotionFilterConfig {
/// Maximum time between scan insertions (seconds).
///
/// If this much time has passed since the last insertion,
/// insert a new scan regardless of motion.
///
/// Default: 5.0 seconds
#[serde(default = "default_max_time_seconds")]
pub max_time_seconds: f32,
/// Minimum distance to trigger scan insertion (meters).
///
/// If the robot has moved this far since the last insertion,
/// insert a new scan.
///
/// Default: 0.2 meters (20cm)
#[serde(default = "default_max_distance_meters")]
pub max_distance_meters: f32,
/// Minimum rotation to trigger scan insertion (radians).
///
/// If the robot has rotated this much since the last insertion,
/// insert a new scan.
///
/// Default: 0.035 radians (~2°)
#[serde(default = "default_max_angle_radians")]
pub max_angle_radians: f32,
}
fn default_max_time_seconds() -> f32 {
5.0
}
fn default_max_distance_meters() -> f32 {
0.2
}
fn default_max_angle_radians() -> f32 {
0.035
}
impl Default for MotionFilterConfig {
fn default() -> Self {
Self {
max_time_seconds: 5.0,
max_distance_meters: 0.2,
max_angle_radians: 0.035, // ~2 degrees
}
}
}
impl MotionFilterConfig {
/// Create a config for high-frequency insertion (more scans).
///
/// Useful for detailed mapping or when computational resources allow.
pub fn high_frequency() -> Self {
Self {
max_time_seconds: 2.0,
max_distance_meters: 0.1,
max_angle_radians: 0.02,
}
}
/// Create a config for low-frequency insertion (fewer scans).
///
/// Useful for resource-constrained systems or large environments.
pub fn low_frequency() -> Self {
Self {
max_time_seconds: 10.0,
max_distance_meters: 0.5,
max_angle_radians: 0.1,
}
}
}
/// Motion filter for controlling scan insertion rate.
///
/// Tracks robot motion and decides when to insert scans based on
/// distance, rotation, and time thresholds.
#[derive(Clone, Debug)]
pub struct MotionFilter {
/// Configuration
config: MotionFilterConfig,
/// Last accepted pose
last_pose: Option<Pose2D>,
/// Last accepted timestamp (microseconds)
last_timestamp_us: Option<u64>,
/// Number of scans accepted
accepted_count: u64,
/// Number of scans rejected
rejected_count: u64,
}
impl MotionFilter {
/// Create a new motion filter with the given configuration.
pub fn new(config: MotionFilterConfig) -> Self {
Self {
config,
last_pose: None,
last_timestamp_us: None,
accepted_count: 0,
rejected_count: 0,
}
}
/// Check if a scan should be inserted.
///
/// Returns `true` if any of the following conditions are met:
/// - This is the first scan
/// - Time since last insertion exceeds `max_time_seconds`
/// - Distance moved exceeds `max_distance_meters`
/// - Rotation exceeds `max_angle_radians`
pub fn should_insert(&self, pose: Pose2D, timestamp_us: u64) -> bool {
// Always accept first scan
let (last_pose, last_ts) = match (self.last_pose, self.last_timestamp_us) {
(Some(p), Some(t)) => (p, t),
_ => return true,
};
// Check time threshold
let dt_seconds = (timestamp_us.saturating_sub(last_ts)) as f32 / 1_000_000.0;
if dt_seconds >= self.config.max_time_seconds {
return true;
}
// Check distance threshold
let dx = pose.x - last_pose.x;
let dy = pose.y - last_pose.y;
let distance = (dx * dx + dy * dy).sqrt();
if distance >= self.config.max_distance_meters {
return true;
}
// Check angle threshold
let dtheta = normalize_angle(pose.theta - last_pose.theta).abs();
if dtheta >= self.config.max_angle_radians {
return true;
}
false
}
/// Mark a pose as accepted (scan was inserted).
///
/// Call this after successfully inserting a scan to update
/// the filter's state.
pub fn accept(&mut self, pose: Pose2D, timestamp_us: u64) {
self.last_pose = Some(pose);
self.last_timestamp_us = Some(timestamp_us);
self.accepted_count += 1;
}
/// Mark a scan as rejected (not inserted).
///
/// Call this when `should_insert` returns false to track statistics.
pub fn reject(&mut self) {
self.rejected_count += 1;
}
/// Get the number of accepted scans.
#[inline]
pub fn accepted_count(&self) -> u64 {
self.accepted_count
}
/// Get the number of rejected scans.
#[inline]
pub fn rejected_count(&self) -> u64 {
self.rejected_count
}
/// Get the total number of scans considered.
#[inline]
pub fn total_count(&self) -> u64 {
self.accepted_count + self.rejected_count
}
/// Get the acceptance rate (0.0 - 1.0).
pub fn acceptance_rate(&self) -> f32 {
let total = self.total_count();
if total == 0 {
1.0
} else {
self.accepted_count as f32 / total as f32
}
}
/// Get the last accepted pose.
#[inline]
pub fn last_pose(&self) -> Option<Pose2D> {
self.last_pose
}
/// Get the last accepted timestamp.
#[inline]
pub fn last_timestamp_us(&self) -> Option<u64> {
self.last_timestamp_us
}
/// Get time since last insertion (seconds).
///
/// Returns `None` if no scan has been accepted yet.
pub fn time_since_last(&self, current_timestamp_us: u64) -> Option<f32> {
self.last_timestamp_us
.map(|last_ts| (current_timestamp_us.saturating_sub(last_ts)) as f32 / 1_000_000.0)
}
/// Get distance since last insertion (meters).
///
/// Returns `None` if no scan has been accepted yet.
pub fn distance_since_last(&self, current_pose: Pose2D) -> Option<f32> {
self.last_pose.map(|last_pose| {
let dx = current_pose.x - last_pose.x;
let dy = current_pose.y - last_pose.y;
(dx * dx + dy * dy).sqrt()
})
}
/// Get rotation since last insertion (radians, absolute).
///
/// Returns `None` if no scan has been accepted yet.
pub fn rotation_since_last(&self, current_pose: Pose2D) -> Option<f32> {
self.last_pose
.map(|last_pose| normalize_angle(current_pose.theta - last_pose.theta).abs())
}
/// Reset the filter state.
pub fn reset(&mut self) {
self.last_pose = None;
self.last_timestamp_us = None;
self.accepted_count = 0;
self.rejected_count = 0;
}
/// Reset only the statistics, keeping the last pose/timestamp.
pub fn reset_stats(&mut self) {
self.accepted_count = 0;
self.rejected_count = 0;
}
}
/// Normalize angle to [-PI, PI] range.
#[inline]
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle;
while a > std::f32::consts::PI {
a -= 2.0 * std::f32::consts::PI;
}
while a < -std::f32::consts::PI {
a += 2.0 * std::f32::consts::PI;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_motion_filter_creation() {
let filter = MotionFilter::new(MotionFilterConfig::default());
assert_eq!(filter.accepted_count(), 0);
assert_eq!(filter.rejected_count(), 0);
assert!(filter.last_pose().is_none());
}
#[test]
fn test_first_scan_always_accepted() {
let filter = MotionFilter::new(MotionFilterConfig::default());
let pose = Pose2D::new(0.0, 0.0, 0.0);
assert!(filter.should_insert(pose, 0));
}
#[test]
fn test_no_motion_rejected() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
let pose = Pose2D::new(0.0, 0.0, 0.0);
// Accept first scan
filter.accept(pose, 0);
// Same pose, short time later - should be rejected
assert!(!filter.should_insert(pose, 100000)); // 0.1 seconds later
}
#[test]
fn test_distance_threshold() {
let config = MotionFilterConfig {
max_distance_meters: 0.2,
max_time_seconds: 1000.0, // Disable time trigger
max_angle_radians: 1000.0, // Disable angle trigger
};
let mut filter = MotionFilter::new(config);
let pose1 = Pose2D::new(0.0, 0.0, 0.0);
filter.accept(pose1, 0);
// Small movement - rejected
let pose2 = Pose2D::new(0.1, 0.0, 0.0);
assert!(!filter.should_insert(pose2, 100000));
// Large movement - accepted
let pose3 = Pose2D::new(0.25, 0.0, 0.0);
assert!(filter.should_insert(pose3, 200000));
}
#[test]
fn test_angle_threshold() {
let config = MotionFilterConfig {
max_distance_meters: 1000.0, // Disable distance trigger
max_time_seconds: 1000.0, // Disable time trigger
max_angle_radians: 0.1,
};
let mut filter = MotionFilter::new(config);
let pose1 = Pose2D::new(0.0, 0.0, 0.0);
filter.accept(pose1, 0);
// Small rotation - rejected
let pose2 = Pose2D::new(0.0, 0.0, 0.05);
assert!(!filter.should_insert(pose2, 100000));
// Large rotation - accepted
let pose3 = Pose2D::new(0.0, 0.0, 0.15);
assert!(filter.should_insert(pose3, 200000));
}
#[test]
fn test_time_threshold() {
let config = MotionFilterConfig {
max_distance_meters: 1000.0, // Disable distance trigger
max_time_seconds: 1.0,
max_angle_radians: 1000.0, // Disable angle trigger
};
let mut filter = MotionFilter::new(config);
let pose = Pose2D::new(0.0, 0.0, 0.0);
filter.accept(pose, 0);
// Short time - rejected
assert!(!filter.should_insert(pose, 500000)); // 0.5 seconds
// Long time - accepted
assert!(filter.should_insert(pose, 1500000)); // 1.5 seconds
}
#[test]
fn test_statistics() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
let pose = Pose2D::new(0.0, 0.0, 0.0);
filter.accept(pose, 0);
filter.reject();
filter.reject();
filter.accept(Pose2D::new(1.0, 0.0, 0.0), 1000000);
assert_eq!(filter.accepted_count(), 2);
assert_eq!(filter.rejected_count(), 2);
assert_eq!(filter.total_count(), 4);
assert!((filter.acceptance_rate() - 0.5).abs() < 0.01);
}
#[test]
fn test_distance_since_last() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
let pose1 = Pose2D::new(0.0, 0.0, 0.0);
filter.accept(pose1, 0);
let pose2 = Pose2D::new(3.0, 4.0, 0.0);
let dist = filter.distance_since_last(pose2).unwrap();
assert!((dist - 5.0).abs() < 0.01); // 3-4-5 triangle
}
#[test]
fn test_reset() {
let mut filter = MotionFilter::new(MotionFilterConfig::default());
filter.accept(Pose2D::new(1.0, 2.0, 0.5), 1000000);
filter.reject();
filter.reset();
assert!(filter.last_pose().is_none());
assert_eq!(filter.accepted_count(), 0);
assert_eq!(filter.rejected_count(), 0);
}
#[test]
fn test_high_frequency_config() {
let config = MotionFilterConfig::high_frequency();
assert!(config.max_distance_meters < 0.2);
assert!(config.max_time_seconds < 5.0);
}
#[test]
fn test_low_frequency_config() {
let config = MotionFilterConfig::low_frequency();
assert!(config.max_distance_meters > 0.2);
assert!(config.max_time_seconds > 5.0);
}
}
vastu-slam/src/core/pose_extrapolator.rs
+552
View File
@@ -0,0 +1,552 @@
//! Pose extrapolation with odometry and IMU fusion.
//!
//! This module implements Cartographer-style pose extrapolation, combining
//! wheel odometry (primary) with IMU data (secondary) for improved pose
//! estimation.
//!
//! # Algorithm
//!
//! The PoseExtrapolator maintains:
//!
//! 1. **Odometry Queue**: Recent odometry measurements for velocity estimation
//! 2. **Pose Queue**: Recent scan-matched poses for drift correction
//! 3. **IMU Tracker**: Gyroscope-based orientation tracking
//!
//! # Priority Scheme
//!
//! - **Translation**: 100% from wheel odometry (most accurate for ground robots)
//! - **Rotation**: Weighted blend of odometry and IMU
//! - Default: 70% odometry + 30% IMU
//! - Configurable via `imu_rotation_weight`
//!
//! # Example
//!
//! ```rust,ignore
//! use vastu_slam::core::{PoseExtrapolator, PoseExtrapolatorConfig, ImuMeasurement, Pose2D};
//!
//! let config = PoseExtrapolatorConfig::default();
//! let mut extrapolator = PoseExtrapolator::new(config);
//!
//! // Feed sensor data at high rate
//! extrapolator.add_odometry(odom_delta, timestamp_us);
//! extrapolator.add_imu(&imu);
//!
//! // Extrapolate pose when needed (e.g., at lidar scan time)
//! let predicted_pose = extrapolator.extrapolate_pose(scan_timestamp_us);
//!
//! // After scan matching, add corrected pose
//! extrapolator.add_pose(matched_pose, timestamp_us);
//! ```
use std::collections::VecDeque;
use serde::{Deserialize, Serialize};
use super::imu_tracker::{ImuTracker, ImuTrackerConfig};
use super::{ImuMeasurement, Pose2D};
/// Configuration for the pose extrapolator.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct PoseExtrapolatorConfig {
/// Duration to keep poses in the queue for velocity estimation (seconds).
///
/// Longer durations give smoother velocity estimates but may be less
/// responsive to changes.
///
/// Default: 0.5 seconds
#[serde(default = "default_pose_queue_duration")]
pub pose_queue_duration: f32,
/// Duration to keep odometry in the queue (seconds).
///
/// Default: 0.5 seconds
#[serde(default = "default_odom_queue_duration")]
pub odom_queue_duration: f32,
/// IMU gravity time constant (seconds).
///
/// Passed to the internal ImuTracker.
///
/// Default: 10.0 seconds
#[serde(default = "default_imu_gravity_time_constant")]
pub imu_gravity_time_constant: f32,
/// Weight for IMU rotation vs odometry rotation [0.0 - 1.0].
///
/// - 0.0: Use only odometry for rotation
/// - 1.0: Use only IMU for rotation
/// - 0.3: 70% odometry + 30% IMU (default, odometry primary)
///
/// Default: 0.3
#[serde(default = "default_imu_rotation_weight")]
pub imu_rotation_weight: f32,
}
fn default_pose_queue_duration() -> f32 {
0.5
}
fn default_odom_queue_duration() -> f32 {
0.5
}
fn default_imu_gravity_time_constant() -> f32 {
10.0
}
fn default_imu_rotation_weight() -> f32 {
0.3
}
impl Default for PoseExtrapolatorConfig {
fn default() -> Self {
Self {
pose_queue_duration: 0.5,
odom_queue_duration: 0.5,
imu_gravity_time_constant: 10.0,
imu_rotation_weight: 0.3,
}
}
}
/// A pose with timestamp.
#[derive(Clone, Copy, Debug)]
pub struct TimedPose {
/// The pose in world frame.
pub pose: Pose2D,
/// Timestamp in microseconds when this pose was recorded.
pub timestamp_us: u64,
}
/// An odometry delta with timestamp.
#[derive(Clone, Copy, Debug)]
pub struct TimedOdometry {
/// Pose delta in robot frame (dx forward, dy left, dtheta CCW)
pub delta: Pose2D,
/// Timestamp when this odometry was measured
pub timestamp_us: u64,
}
/// Pose extrapolator with odometry and IMU fusion.
///
/// Combines wheel odometry (primary for translation) with IMU
/// (secondary for rotation) using Cartographer-style algorithms.
#[derive(Clone, Debug)]
pub struct PoseExtrapolator {
/// Configuration
config: PoseExtrapolatorConfig,
/// IMU tracker for orientation
imu_tracker: ImuTracker,
/// Recent poses from scan matching (for velocity estimation)
pose_queue: VecDeque<TimedPose>,
/// Recent odometry deltas
odom_queue: VecDeque<TimedOdometry>,
/// Current pose estimate
current_pose: Pose2D,
/// Last pose timestamp
last_pose_timestamp_us: Option<u64>,
/// Cumulative odometry since last pose update
cumulative_odom: Pose2D,
/// Last odometry timestamp
last_odom_timestamp_us: Option<u64>,
/// Estimated linear velocity (m/s) in world frame [vx, vy]
linear_velocity: [f32; 2],
/// Estimated angular velocity (rad/s)
angular_velocity: f32,
/// IMU yaw at last pose update (for relative tracking)
imu_yaw_at_last_pose: f32,
}
impl PoseExtrapolator {
/// Create a new pose extrapolator with the given configuration.
pub fn new(config: PoseExtrapolatorConfig) -> Self {
let imu_config = ImuTrackerConfig {
gravity_time_constant: config.imu_gravity_time_constant,
initial_gravity: [0.0, 0.0, 9.81],
};
Self {
config,
imu_tracker: ImuTracker::new(imu_config),
pose_queue: VecDeque::new(),
odom_queue: VecDeque::new(),
current_pose: Pose2D::default(),
last_pose_timestamp_us: None,
cumulative_odom: Pose2D::default(),
last_odom_timestamp_us: None,
linear_velocity: [0.0, 0.0],
angular_velocity: 0.0,
imu_yaw_at_last_pose: 0.0,
}
}
/// Add an IMU measurement (secondary for rotation).
pub fn add_imu(&mut self, imu: &ImuMeasurement) {
self.imu_tracker.add_imu(imu);
}
/// Add an odometry delta (primary for translation).
///
/// The delta should be in robot frame (dx forward, dy left, dtheta CCW).
pub fn add_odometry(&mut self, delta: Pose2D, timestamp_us: u64) {
// Store in queue
self.odom_queue.push_back(TimedOdometry {
delta,
timestamp_us,
});
// Trim old entries
let cutoff_us =
timestamp_us.saturating_sub((self.config.odom_queue_duration * 1_000_000.0) as u64);
while self
.odom_queue
.front()
.is_some_and(|o| o.timestamp_us < cutoff_us)
{
self.odom_queue.pop_front();
}
// Accumulate odometry
self.cumulative_odom = self.cumulative_odom.compose(&delta);
self.last_odom_timestamp_us = Some(timestamp_us);
// Update velocity estimate from odometry
self.update_velocity_from_odometry();
}
/// Add a pose from scan matching (for velocity estimation and drift correction).
///
/// Call this after successful scan matching to update the extrapolator
/// with the corrected pose.
pub fn add_pose(&mut self, pose: Pose2D, timestamp_us: u64) {
// Store in queue
self.pose_queue.push_back(TimedPose { pose, timestamp_us });
// Trim old entries
let cutoff_us =
timestamp_us.saturating_sub((self.config.pose_queue_duration * 1_000_000.0) as u64);
while self
.pose_queue
.front()
.is_some_and(|p| p.timestamp_us < cutoff_us)
{
self.pose_queue.pop_front();
}
// Update current pose
self.current_pose = pose;
self.last_pose_timestamp_us = Some(timestamp_us);
// Reset cumulative odometry
self.cumulative_odom = Pose2D::default();
// Record IMU yaw at this pose
self.imu_yaw_at_last_pose = self.imu_tracker.yaw();
// Update velocity estimate from pose history
self.update_velocity_from_poses();
}
/// Extrapolate the pose to the given timestamp.
///
/// Uses odometry for translation and a weighted blend of odometry
/// and IMU for rotation.
pub fn extrapolate_pose(&self, timestamp_us: u64) -> Pose2D {
// Start with current pose
let mut pose = self.current_pose;
// Apply cumulative odometry (translation from odometry, rotation blended)
let odom_theta = self.cumulative_odom.theta;
// Get IMU rotation delta since last pose
let imu_theta_delta = self.imu_tracker.yaw() - self.imu_yaw_at_last_pose;
// Blend odometry and IMU rotation
let blended_theta = (1.0 - self.config.imu_rotation_weight) * odom_theta
+ self.config.imu_rotation_weight * imu_theta_delta;
// Create blended delta
let blended_delta = Pose2D::new(
self.cumulative_odom.x,
self.cumulative_odom.y,
blended_theta,
);
// Apply delta to pose
pose = pose.compose(&blended_delta);
// Extrapolate beyond last odometry using velocity
if let Some(last_odom_ts) = self.last_odom_timestamp_us
&& timestamp_us > last_odom_ts
{
let dt = (timestamp_us - last_odom_ts) as f32 / 1_000_000.0;
if dt < 1.0 {
// Reasonable extrapolation window
let dx = self.linear_velocity[0] * dt;
let dy = self.linear_velocity[1] * dt;
let dtheta = self.angular_velocity * dt;
pose = pose.compose(&Pose2D::new(dx, dy, dtheta));
}
}
pose
}
/// Get the current pose estimate (without extrapolation).
#[inline]
pub fn current_pose(&self) -> Pose2D {
self.current_pose
}
/// Get the estimated linear velocity [vx, vy] in m/s (world frame).
#[inline]
pub fn linear_velocity(&self) -> [f32; 2] {
self.linear_velocity
}
/// Get the estimated angular velocity in rad/s.
#[inline]
pub fn angular_velocity(&self) -> f32 {
self.angular_velocity
}
/// Get a reference to the internal IMU tracker.
#[inline]
pub fn imu_tracker(&self) -> &ImuTracker {
&self.imu_tracker
}
/// Get a mutable reference to the internal IMU tracker.
#[inline]
pub fn imu_tracker_mut(&mut self) -> &mut ImuTracker {
&mut self.imu_tracker
}
/// Set the initial pose.
///
/// Call this before starting to set the initial robot position.
pub fn set_initial_pose(&mut self, pose: Pose2D, timestamp_us: u64) {
self.current_pose = pose;
self.last_pose_timestamp_us = Some(timestamp_us);
self.cumulative_odom = Pose2D::default();
self.imu_yaw_at_last_pose = self.imu_tracker.yaw();
self.pose_queue.clear();
self.odom_queue.clear();
}
/// Reset the extrapolator to initial state.
pub fn reset(&mut self) {
self.imu_tracker.reset();
self.pose_queue.clear();
self.odom_queue.clear();
self.current_pose = Pose2D::default();
self.last_pose_timestamp_us = None;
self.cumulative_odom = Pose2D::default();
self.last_odom_timestamp_us = None;
self.linear_velocity = [0.0, 0.0];
self.angular_velocity = 0.0;
self.imu_yaw_at_last_pose = 0.0;
}
/// Update velocity estimate from odometry queue.
fn update_velocity_from_odometry(&mut self) {
if self.odom_queue.len() < 2 {
return;
}
let first = self.odom_queue.front().unwrap();
let last = self.odom_queue.back().unwrap();
let dt = (last.timestamp_us.saturating_sub(first.timestamp_us)) as f32 / 1_000_000.0;
if dt < 0.01 {
return; // Not enough time span
}
// Sum all deltas in the queue
let mut total_delta = Pose2D::default();
for odom in &self.odom_queue {
total_delta = total_delta.compose(&odom.delta);
}
// Compute velocity (in robot frame, then rotate to world frame)
let vx_robot = total_delta.x / dt;
let vy_robot = total_delta.y / dt;
self.angular_velocity = total_delta.theta / dt;
// Rotate to world frame using current heading
let cos_theta = self.current_pose.theta.cos();
let sin_theta = self.current_pose.theta.sin();
self.linear_velocity[0] = vx_robot * cos_theta - vy_robot * sin_theta;
self.linear_velocity[1] = vx_robot * sin_theta + vy_robot * cos_theta;
}
/// Update velocity estimate from pose queue.
fn update_velocity_from_poses(&mut self) {
if self.pose_queue.len() < 2 {
return;
}
let first = self.pose_queue.front().unwrap();
let last = self.pose_queue.back().unwrap();
let dt = (last.timestamp_us.saturating_sub(first.timestamp_us)) as f32 / 1_000_000.0;
if dt < 0.01 {
return; // Not enough time span
}
// Compute velocity from pose difference
let dx = last.pose.x - first.pose.x;
let dy = last.pose.y - first.pose.y;
let dtheta = normalize_angle(last.pose.theta - first.pose.theta);
self.linear_velocity[0] = dx / dt;
self.linear_velocity[1] = dy / dt;
self.angular_velocity = dtheta / dt;
}
}
/// Normalize angle to [-PI, PI] range.
#[inline]
fn normalize_angle(angle: f32) -> f32 {
let mut a = angle;
while a > std::f32::consts::PI {
a -= 2.0 * std::f32::consts::PI;
}
while a < -std::f32::consts::PI {
a += 2.0 * std::f32::consts::PI;
}
a
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_pose_extrapolator_creation() {
let config = PoseExtrapolatorConfig::default();
let extrapolator = PoseExtrapolator::new(config);
assert_eq!(extrapolator.current_pose(), Pose2D::default());
assert_eq!(extrapolator.linear_velocity(), [0.0, 0.0]);
assert_eq!(extrapolator.angular_velocity(), 0.0);
}
#[test]
fn test_set_initial_pose() {
let mut extrapolator = PoseExtrapolator::new(PoseExtrapolatorConfig::default());
let initial = Pose2D::new(1.0, 2.0, 0.5);
extrapolator.set_initial_pose(initial, 1000000);
assert_eq!(extrapolator.current_pose().x, 1.0);
assert_eq!(extrapolator.current_pose().y, 2.0);
assert!((extrapolator.current_pose().theta - 0.5).abs() < 1e-6);
}
#[test]
fn test_odometry_accumulation() {
let mut extrapolator = PoseExtrapolator::new(PoseExtrapolatorConfig::default());
extrapolator.set_initial_pose(Pose2D::default(), 0);
// Add odometry: move forward 1m
extrapolator.add_odometry(Pose2D::new(0.5, 0.0, 0.0), 100000);
extrapolator.add_odometry(Pose2D::new(0.5, 0.0, 0.0), 200000);
let predicted = extrapolator.extrapolate_pose(200000);
assert!((predicted.x - 1.0).abs() < 0.1, "x = {}", predicted.x);
assert!((predicted.y - 0.0).abs() < 0.1, "y = {}", predicted.y);
}
#[test]
fn test_pose_update() {
let mut extrapolator = PoseExtrapolator::new(PoseExtrapolatorConfig::default());
extrapolator.set_initial_pose(Pose2D::default(), 0);
// Add some odometry
extrapolator.add_odometry(Pose2D::new(1.0, 0.0, 0.1), 100000);
// Add corrected pose from scan matching
let matched_pose = Pose2D::new(0.95, 0.05, 0.08); // Slightly different
extrapolator.add_pose(matched_pose, 100000);
// Current pose should be the matched pose
assert_eq!(extrapolator.current_pose().x, 0.95);
assert_eq!(extrapolator.current_pose().y, 0.05);
}
#[test]
fn test_imu_rotation_blending() {
let config = PoseExtrapolatorConfig {
imu_rotation_weight: 0.5, // 50% IMU, 50% odometry
..Default::default()
};
let mut extrapolator = PoseExtrapolator::new(config);
extrapolator.set_initial_pose(Pose2D::default(), 0);
// Add IMU readings with rotation
for i in 0..10 {
let imu = ImuMeasurement::new(
i * 10000, // 10ms intervals
[0.0, 0.0, 9.81],
[0.0, 0.0, 1.0], // 1 rad/s yaw
);
extrapolator.add_imu(&imu);
}
// Add odometry with different rotation
extrapolator.add_odometry(Pose2D::new(0.1, 0.0, 0.2), 100000); // 0.2 rad from odom
let predicted = extrapolator.extrapolate_pose(100000);
// IMU would give ~0.09 rad (9 samples * 0.01s * 1 rad/s)
// Odom gives 0.2 rad
// Blended: 0.5 * 0.2 + 0.5 * 0.09 ≈ 0.145
// This is approximate due to timing
assert!(predicted.theta.abs() > 0.05, "theta = {}", predicted.theta);
assert!(predicted.theta.abs() < 0.3, "theta = {}", predicted.theta);
}
#[test]
fn test_velocity_estimation() {
let mut extrapolator = PoseExtrapolator::new(PoseExtrapolatorConfig::default());
extrapolator.set_initial_pose(Pose2D::default(), 0);
// Add several odometry samples (moving at 1 m/s forward)
for i in 1..=10 {
// 0.1m every 100ms = 1 m/s
extrapolator.add_odometry(Pose2D::new(0.1, 0.0, 0.0), i * 100000);
}
let vel = extrapolator.linear_velocity();
// Should be approximately 1 m/s in x (allowing some tolerance for edge effects)
assert!(vel[0] > 0.8, "vx = {}", vel[0]);
assert!(vel[0] < 1.5, "vx = {}", vel[0]);
assert!(vel[1].abs() < 0.1, "vy = {}", vel[1]);
}
#[test]
fn test_reset() {
let mut extrapolator = PoseExtrapolator::new(PoseExtrapolatorConfig::default());
extrapolator.set_initial_pose(Pose2D::new(5.0, 5.0, 1.0), 1000000);
extrapolator.add_odometry(Pose2D::new(1.0, 0.0, 0.1), 2000000);
extrapolator.reset();
assert_eq!(extrapolator.current_pose(), Pose2D::default());
assert_eq!(extrapolator.linear_velocity(), [0.0, 0.0]);
}
}
vastu-slam/src/core/sensors.rs
+215
View File
@@ -355,6 +355,125 @@ impl BumperSensors {
}
}
/// IMU measurement from inertial measurement unit (6-axis).
///
/// All values are in SI units (m/s², rad/s). The coordinate frame follows
/// ROS REP-103: X forward, Y left, Z up.
///
/// # Conversion from SangamIO raw i16 values
///
/// SangamIO streams raw i16 values that need conversion:
/// - gyro: `raw_i16 / 1000.0` → rad/s
/// - accel: `raw_i16 / 16384.0 * 9.81` → m/s²
///
/// Use [`ImuMeasurement::from_raw`] for automatic conversion.
///
/// # Usage for 2D SLAM
///
/// For 2D ground robots, primarily use:
/// - `gyro_z`: Yaw rate (rotation around vertical axis)
/// - `accel_x`, `accel_y`: For gravity estimation (tilt detection)
///
/// # Example
///
/// ```rust,ignore
/// use vastu_slam::core::ImuMeasurement;
///
/// // From SangamIO raw values
/// let imu = ImuMeasurement::from_raw(
/// timestamp_us,
/// [gyro_x_raw, gyro_y_raw, gyro_z_raw],
/// [accel_x_raw, accel_y_raw, accel_z_raw],
/// );
///
/// // Yaw rate for rotation estimation
/// let yaw_rate = imu.gyro_z; // rad/s, CCW positive
/// ```
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct ImuMeasurement {
/// Timestamp in microseconds since epoch
pub timestamp_us: u64,
/// Linear acceleration X in m/s² (forward)
pub accel_x: f32,
/// Linear acceleration Y in m/s² (left)
pub accel_y: f32,
/// Linear acceleration Z in m/s² (up, includes gravity: ~+9.81 when level)
pub accel_z: f32,
/// Angular velocity X in rad/s (roll rate, rotation around forward axis)
pub gyro_x: f32,
/// Angular velocity Y in rad/s (pitch rate, rotation around left axis)
pub gyro_y: f32,
/// Angular velocity Z in rad/s (yaw rate, rotation around up axis, CCW positive)
/// This is the PRIMARY value for 2D SLAM rotation estimation.
pub gyro_z: f32,
}
/// Scale factor for converting raw gyroscope i16 to rad/s
const GYRO_SCALE: f32 = 1000.0;
/// Scale factor for converting raw accelerometer i16 to g units
const ACCEL_SCALE: f32 = 16384.0;
/// Standard gravity in m/s²
const G: f32 = 9.81;
impl ImuMeasurement {
/// Create a new IMU measurement with SI units
pub fn new(timestamp_us: u64, accel: [f32; 3], gyro: [f32; 3]) -> Self {
Self {
timestamp_us,
accel_x: accel[0],
accel_y: accel[1],
accel_z: accel[2],
gyro_x: gyro[0],
gyro_y: gyro[1],
gyro_z: gyro[2],
}
}
/// Create from SangamIO raw i16 values
///
/// Converts raw sensor values to SI units:
/// - gyro: raw / 1000.0 → rad/s
/// - accel: raw / 16384.0 * 9.81 → m/s²
pub fn from_raw(timestamp_us: u64, gyro: [i16; 3], accel: [i16; 3]) -> Self {
Self {
timestamp_us,
gyro_x: gyro[0] as f32 / GYRO_SCALE,
gyro_y: gyro[1] as f32 / GYRO_SCALE,
gyro_z: gyro[2] as f32 / GYRO_SCALE,
accel_x: accel[0] as f32 / ACCEL_SCALE * G,
accel_y: accel[1] as f32 / ACCEL_SCALE * G,
accel_z: accel[2] as f32 / ACCEL_SCALE * G,
}
}
/// Get acceleration as array [x, y, z] in m/s²
#[inline]
pub fn accel(&self) -> [f32; 3] {
[self.accel_x, self.accel_y, self.accel_z]
}
/// Get angular velocity as array [x, y, z] in rad/s
#[inline]
pub fn gyro(&self) -> [f32; 3] {
[self.gyro_x, self.gyro_y, self.gyro_z]
}
/// Get acceleration magnitude in m/s²
#[inline]
pub fn accel_magnitude(&self) -> f32 {
(self.accel_x * self.accel_x + self.accel_y * self.accel_y + self.accel_z * self.accel_z)
.sqrt()
}
/// Get angular velocity magnitude in rad/s
#[inline]
pub fn gyro_magnitude(&self) -> f32 {
(self.gyro_x * self.gyro_x + self.gyro_y * self.gyro_y + self.gyro_z * self.gyro_z).sqrt()
}
}
/// Combined sensor observation for a single timestep.
///
/// Bundles all sensor readings taken at approximately the same time,
@@ -365,6 +484,7 @@ impl BumperSensors {
///
/// Not all sensors report at the same rate:
/// - **Lidar**: ~5 Hz (every ~200ms) - may be `None` on most ticks
/// - **IMU**: ~110 Hz - optional, for motion filtering
/// - **Cliff/Bumper**: ~110 Hz - always available
///
/// # Processing Order
@@ -383,6 +503,8 @@ pub struct SensorObservation {
pub cliffs: CliffSensors,
/// Bumper sensor states (event-driven, only true on collision)
pub bumpers: BumperSensors,
/// IMU measurement (optional, ~110 Hz, for motion filtering)
pub imu: Option<ImuMeasurement>,
/// Timestamp in microseconds since epoch (for temporal ordering)
pub timestamp_us: u64,
}
@@ -395,6 +517,7 @@ impl SensorObservation {
lidar: None,
cliffs: CliffSensors::SAFE,
bumpers: BumperSensors::CLEAR,
imu: None,
timestamp_us,
}
}
@@ -406,6 +529,36 @@ impl SensorObservation {
lidar: Some(lidar),
cliffs: CliffSensors::SAFE,
bumpers: BumperSensors::CLEAR,
imu: None,
timestamp_us,
}
}
/// Create an observation with lidar and IMU
pub fn with_lidar_and_imu(
pose: Pose2D,
lidar: LidarScan,
imu: ImuMeasurement,
timestamp_us: u64,
) -> Self {
Self {
pose,
lidar: Some(lidar),
cliffs: CliffSensors::SAFE,
bumpers: BumperSensors::CLEAR,
imu: Some(imu),
timestamp_us,
}
}
/// Create an observation with IMU only (no lidar)
pub fn with_imu(pose: Pose2D, imu: ImuMeasurement, timestamp_us: u64) -> Self {
Self {
pose,
lidar: None,
cliffs: CliffSensors::SAFE,
bumpers: BumperSensors::CLEAR,
imu: Some(imu),
timestamp_us,
}
}
@@ -414,6 +567,11 @@ impl SensorObservation {
pub fn any_triggered(&self) -> bool {
self.cliffs.any_triggered() || self.bumpers.any_triggered()
}
/// Check if IMU data is available
pub fn has_imu(&self) -> bool {
self.imu.is_some()
}
}
#[cfg(test)]
@@ -512,4 +670,61 @@ mod tests {
assert!((world_points[0].x - 0.0).abs() < 1e-5);
assert!((world_points[0].y - 1.0).abs() < 1e-5);
}
#[test]
fn test_imu_measurement_from_raw() {
// Test conversion from raw i16 values
let imu = ImuMeasurement::from_raw(
1000000, // 1 second timestamp
[1000, 0, -500], // gyro raw: 1 rad/s X, 0 Y, -0.5 rad/s Z
[0, 0, 16384], // accel raw: 0, 0, 1g (level)
);
// Check gyro conversion: raw / 1000.0 = rad/s
assert!((imu.gyro_x - 1.0).abs() < 1e-6);
assert!((imu.gyro_y - 0.0).abs() < 1e-6);
assert!((imu.gyro_z - (-0.5)).abs() < 1e-6);
// Check accel conversion: raw / 16384.0 * 9.81 = m/s²
assert!((imu.accel_x - 0.0).abs() < 1e-6);
assert!((imu.accel_y - 0.0).abs() < 1e-6);
assert!((imu.accel_z - 9.81).abs() < 0.01);
}
#[test]
fn test_imu_measurement_new() {
let imu = ImuMeasurement::new(
500000,
[0.0, 0.0, 9.81], // accel
[0.1, 0.0, -0.2], // gyro
);
assert_eq!(imu.timestamp_us, 500000);
assert_eq!(imu.accel(), [0.0, 0.0, 9.81]);
assert_eq!(imu.gyro(), [0.1, 0.0, -0.2]);
}
#[test]
fn test_imu_measurement_magnitudes() {
let imu = ImuMeasurement::new(
0,
[3.0, 4.0, 0.0], // accel: magnitude = 5.0
[0.0, 0.0, 1.0], // gyro: magnitude = 1.0
);
assert!((imu.accel_magnitude() - 5.0).abs() < 1e-6);
assert!((imu.gyro_magnitude() - 1.0).abs() < 1e-6);
}
#[test]
fn test_sensor_observation_with_imu() {
let pose = Pose2D::new(1.0, 2.0, 0.5);
let imu = ImuMeasurement::default();
let obs = SensorObservation::with_imu(pose, imu, 123456);
assert!(obs.has_imu());
assert!(obs.lidar.is_none());
assert_eq!(obs.timestamp_us, 123456);
}
}
vastu-slam/src/lib.rs
+26 -2
View File
@@ -18,6 +18,8 @@
//! - **Scan Matching**: Correlative and branch-and-bound scan-to-map alignment
//! - **Loop Closure**: LiDAR-IRIS descriptors for place recognition
//! - **Submap Architecture**: Cartographer-style reversible map updates
//! - **IMU Fusion**: Cartographer-style pose extrapolation with odometry + IMU blending
//! - **Motion Filtering**: Scan insertion throttling based on distance/rotation/time
//!
//! ## Quick Start
//!
@@ -36,6 +38,27 @@
//! println!("Updated {} cells", result.cells_updated);
//! ```
//!
//! ## Localization with IMU Fusion
//!
//! ```rust,ignore
//! use vastu_slam::modes::{Localizer, LocalizerConfig};
//! use vastu_slam::core::ImuMeasurement;
//!
//! // Create localizer with motion filtering enabled
//! let config = LocalizerConfig::with_motion_filtering();
//! let mut localizer = Localizer::new(map, config);
//! localizer.set_initial_pose(Pose2D::new(0.0, 0.0, 0.0));
//!
//! // Feed IMU at high rate (~110 Hz)
//! localizer.add_imu(&ImuMeasurement::from_raw(timestamp_us, gyro, accel));
//!
//! // Localize with motion filtering
//! let result = localizer.localize_with_timestamp(&scan, Some(odom_delta), timestamp_us);
//! if !result.skipped && result.converged {
//! println!("Localized at {:?}", result.pose);
//! }
//! ```
//!
//! ## Coordinate System
//!
//! Uses ROS REP-103 convention:
@@ -72,8 +95,9 @@ pub mod evaluation;
// Re-export commonly used types
pub use core::{
BumperSensors, Cell, CellType, CliffSensors, GridCoord, LidarScan, MotionModel, Odometry,
Pose2D, SensorObservation, WorldPoint,
BumperSensors, Cell, CellType, CliffSensors, GridCoord, ImuMeasurement, ImuTracker,
ImuTrackerConfig, LidarScan, MotionFilter, MotionFilterConfig, MotionModel, Odometry, Pose2D,
PoseExtrapolator, PoseExtrapolatorConfig, SensorObservation, WorldPoint,
};
pub use grid::{CellCounts, ConfigError, GridConfig, GridStorage, MapConfig, SensorConfig};
vastu-slam/src/modes/localization.rs
+213 -2
View File
@@ -41,7 +41,10 @@
use serde::{Deserialize, Serialize};
use crate::core::{LidarScan, Pose2D};
use crate::core::{
ImuMeasurement, LidarScan, MotionFilter, MotionFilterConfig, Pose2D, PoseExtrapolator,
PoseExtrapolatorConfig,
};
use crate::grid::GridStorage;
use crate::matching::{CorrelativeMatcher, CorrelativeMatcherConfig, ScanMatchResult};
@@ -91,6 +94,20 @@ pub struct LocalizerConfig {
/// Default: 0.5 rad (~28.6°)
#[serde(default = "default_max_angular_deviation")]
pub max_angular_deviation: f32,
/// Optional pose extrapolator configuration for IMU fusion.
///
/// When set, enables Cartographer-style pose extrapolation with
/// odometry (primary) and IMU (secondary) fusion.
#[serde(default)]
pub pose_extrapolator: Option<PoseExtrapolatorConfig>,
/// Optional motion filter configuration for scan insertion throttling.
///
/// When set, only processes scans when the robot has moved enough
/// (distance, rotation, or time since last scan).
#[serde(default)]
pub motion_filter: Option<MotionFilterConfig>,
}
fn default_min_score() -> f32 {
@@ -110,6 +127,23 @@ impl Default for LocalizerConfig {
min_score: default_min_score(),
max_linear_deviation: default_max_linear_deviation(),
max_angular_deviation: default_max_angular_deviation(),
pose_extrapolator: None,
motion_filter: None,
}
}
}
impl LocalizerConfig {
/// Create a config with motion filtering enabled (Cartographer-style).
///
/// Uses default values for pose extrapolation and motion filtering:
/// - IMU rotation weight: 0.3 (70% odometry, 30% IMU)
/// - Motion filter: insert when moved 20cm, rotated 2°, or 5s elapsed
pub fn with_motion_filtering() -> Self {
Self {
pose_extrapolator: Some(PoseExtrapolatorConfig::default()),
motion_filter: Some(MotionFilterConfig::default()),
..Default::default()
}
}
}
@@ -126,6 +160,7 @@ impl Default for LocalizerConfig {
///
/// - `converged=true`: Match accepted, `pose` is scan-corrected
/// - `converged=false`: Match rejected, `pose` is odometry fallback
/// - `skipped=true`: Scan was skipped by motion filter (not enough motion)
#[derive(Clone, Debug)]
pub struct LocalizationResult {
/// Estimated robot pose in world frame.
@@ -138,6 +173,7 @@ pub struct LocalizationResult {
///
/// Higher values indicate better alignment with the map.
/// Typical good matches: 0.6 - 0.9
/// Returns 0.0 if skipped.
pub score: f32,
/// Whether the localization converged successfully.
@@ -154,10 +190,31 @@ pub struct LocalizationResult {
/// which may indicate a false match or robot kidnapping.
pub within_bounds: bool,
/// Whether the scan was skipped by the motion filter.
///
/// True if the robot hasn't moved enough since the last scan
/// to warrant processing this scan.
pub skipped: bool,
/// Raw scan match result for detailed diagnostics.
/// Contains a default value if skipped.
pub match_result: ScanMatchResult,
}
impl LocalizationResult {
/// Create a result indicating the scan was skipped due to motion filter.
pub fn skipped(pose: Pose2D) -> Self {
Self {
pose,
score: 0.0,
converged: false,
within_bounds: true,
skipped: true,
match_result: ScanMatchResult::failed(pose),
}
}
}
/// Localizer for known maps.
///
/// Uses scan matching to estimate robot pose without modifying the map.
@@ -208,6 +265,12 @@ pub struct Localizer {
/// Whether we have a valid initial pose.
initialized: bool,
/// Optional pose extrapolator for IMU fusion (Cartographer-style).
pose_extrapolator: Option<PoseExtrapolator>,
/// Optional motion filter for scan insertion throttling.
motion_filter: Option<MotionFilter>,
}
impl Localizer {
@@ -215,12 +278,25 @@ impl Localizer {
pub fn new(map: GridStorage, config: LocalizerConfig) -> Self {
let matcher = CorrelativeMatcher::new(config.matcher.clone());
// Create optional motion filtering components
let pose_extrapolator = config
.pose_extrapolator
.as_ref()
.map(|c| PoseExtrapolator::new(c.clone()));
let motion_filter = config
.motion_filter
.as_ref()
.map(|c| MotionFilter::new(c.clone()));
Self {
map,
matcher,
config,
pose: Pose2D::default(),
initialized: false,
pose_extrapolator,
motion_filter,
}
}
@@ -228,6 +304,22 @@ impl Localizer {
pub fn set_initial_pose(&mut self, pose: Pose2D) {
self.pose = pose;
self.initialized = true;
// Initialize pose extrapolator if present
if let Some(ref mut extrapolator) = self.pose_extrapolator {
extrapolator.set_initial_pose(pose, 0);
}
}
/// Set the initial pose estimate with timestamp
pub fn set_initial_pose_with_timestamp(&mut self, pose: Pose2D, timestamp_us: u64) {
self.pose = pose;
self.initialized = true;
// Initialize pose extrapolator if present
if let Some(ref mut extrapolator) = self.pose_extrapolator {
extrapolator.set_initial_pose(pose, timestamp_us);
}
}
/// Get current pose estimate
@@ -245,7 +337,37 @@ impl Localizer {
&self.map
}
/// Localize using a lidar scan
/// Check if motion filtering is enabled
pub fn has_motion_filtering(&self) -> bool {
self.pose_extrapolator.is_some() || self.motion_filter.is_some()
}
/// Get a reference to the pose extrapolator (if enabled)
pub fn pose_extrapolator(&self) -> Option<&PoseExtrapolator> {
self.pose_extrapolator.as_ref()
}
/// Get a mutable reference to the pose extrapolator (if enabled)
pub fn pose_extrapolator_mut(&mut self) -> Option<&mut PoseExtrapolator> {
self.pose_extrapolator.as_mut()
}
/// Get a reference to the motion filter (if enabled)
pub fn motion_filter(&self) -> Option<&MotionFilter> {
self.motion_filter.as_ref()
}
/// Feed an IMU measurement to the pose extrapolator.
///
/// Call this at high rate (~110 Hz) to keep the IMU tracker updated.
/// Does nothing if pose extrapolator is not configured.
pub fn add_imu(&mut self, imu: &ImuMeasurement) {
if let Some(ref mut extrapolator) = self.pose_extrapolator {
extrapolator.add_imu(imu);
}
}
/// Localize using a lidar scan (basic version, backward compatible)
///
/// # Arguments
/// * `scan` - The lidar scan to match
@@ -285,6 +407,95 @@ impl Localizer {
score: match_result.score,
converged,
within_bounds,
skipped: false,
match_result,
}
}
/// Localize using a lidar scan with IMU fusion and motion filtering.
///
/// This is the full-featured version that uses:
/// - Pose extrapolation with IMU fusion for better prediction
/// - Motion filtering to skip redundant scans
///
/// # Arguments
/// * `scan` - The lidar scan to match
/// * `odom_delta` - Optional odometry delta since last localization
/// * `timestamp_us` - Timestamp in microseconds
///
/// # Returns
/// Localization result with pose estimate and confidence.
/// Returns `skipped=true` if motion filter rejected the scan.
///
/// # Note
/// Call `add_imu()` at high rate (~110 Hz) before calling this method
/// to keep the IMU tracker updated.
pub fn localize_with_timestamp(
&mut self,
scan: &LidarScan,
odom_delta: Option<Pose2D>,
timestamp_us: u64,
) -> LocalizationResult {
// Feed odometry to pose extrapolator if available
if let Some(ref mut extrapolator) = self.pose_extrapolator
&& let Some(delta) = odom_delta
{
extrapolator.add_odometry(delta, timestamp_us);
}
// Get predicted pose (uses filtered motion if extrapolator is available)
let predicted_pose = if let Some(ref extrapolator) = self.pose_extrapolator {
extrapolator.extrapolate_pose(timestamp_us)
} else if let Some(delta) = odom_delta {
self.pose.compose(&delta)
} else {
self.pose
};
// Check motion filter (should we process this scan?)
if let Some(ref filter) = self.motion_filter
&& !filter.should_insert(predicted_pose, timestamp_us)
{
// Update pose with prediction even if skipped
self.pose = predicted_pose;
return LocalizationResult::skipped(predicted_pose);
}
// Match scan against map
let match_result = self.matcher.match_scan(scan, predicted_pose, &self.map);
// Check if match is within acceptable bounds
let within_bounds = self.check_deviation(&predicted_pose, &match_result.pose);
// Determine if we accept this match
let converged =
match_result.converged && match_result.score >= self.config.min_score && within_bounds;
// Update pose if converged
if converged {
self.pose = match_result.pose;
self.initialized = true;
// Update pose extrapolator with matched pose
if let Some(ref mut extrapolator) = self.pose_extrapolator {
extrapolator.add_pose(match_result.pose, timestamp_us);
}
} else if odom_delta.is_some() {
// Fall back to prediction
self.pose = predicted_pose;
}
// Update motion filter
if let Some(ref mut filter) = self.motion_filter {
filter.accept(self.pose, timestamp_us);
}
LocalizationResult {
pose: self.pose,
score: match_result.score,
converged,
within_bounds,
skipped: false,
match_result,
}
}
vastu-slam/src/modes/mod.rs
+42
View File
@@ -40,6 +40,48 @@
//! }
//! ```
//!
//! # Localization with IMU Fusion
//!
//! For improved accuracy, enable Cartographer-style motion filtering with IMU:
//!
//! ```ignore
//! use vastu_slam::modes::{Localizer, LocalizerConfig};
//! use vastu_slam::core::ImuMeasurement;
//!
//! // Enable motion filtering (includes pose extrapolator + motion filter)
//! let config = LocalizerConfig::with_motion_filtering();
//! let mut localizer = Localizer::new(map, config);
//! localizer.set_initial_pose_with_timestamp(Pose2D::new(0.0, 0.0, 0.0), 0);
//!
//! // Feed IMU at high rate (~110 Hz) - call this frequently
//! localizer.add_imu(&ImuMeasurement::from_raw(timestamp_us, gyro, accel));
//!
//! // Localize with timestamp for motion filtering
//! let result = localizer.localize_with_timestamp(&scan, Some(odom_delta), timestamp_us);
//!
//! if result.skipped {
//! // Motion filter rejected scan (not enough motion)
//! println!("Scan skipped, pose: {:?}", result.pose);
//! } else if result.converged {
//! println!("Localized at: {:?}", result.pose);
//! }
//! ```
//!
//! ## Motion Filtering Algorithm
//!
//! The motion filter uses Cartographer-style thresholds:
//! - **Distance**: Insert scan if moved > 0.2m (default)
//! - **Rotation**: Insert scan if rotated > 2° (default)
//! - **Time**: Insert scan if > 5 seconds since last (default)
//!
//! ## IMU Fusion Algorithm
//!
//! The pose extrapolator combines odometry and IMU:
//! - **Translation**: 100% from wheel odometry (most accurate for ground robots)
//! - **Rotation**: Weighted blend (default: 70% odometry + 30% IMU)
//!
//! This prioritizes wheel encoders while using IMU to smooth rotation estimates.
//!
//! # Choosing a Mode
//!
//! **Use Localization Mode when:**