Add Cartographer-style evaluation framework with benchmarks and tests
- Add evaluation module with accuracy, quality, and performance metrics - Add ground truth handling and relations metrics for SLAM evaluation - Add criterion benchmarks for scan matching, grid operations, and evaluation - Add integration tests for accuracy, quality, and performance - Add Serialize/Deserialize derives to CellCounts for evaluation output
This commit is contained in:
codetiger
@@ -16,6 +16,19 @@ serde_yaml = "0.9" # YAML config files
|
||||
env_logger = "0.11" # Test logging
|
||||
approx = "0.5" # Approximate comparisons (tests only)
|
||||
image = { version = "0.25", default-features = false, features = ["png", "pnm"] }
|
||||
criterion = "0.5" # Benchmarking
|
||||
|
||||
[[bench]]
|
||||
name = "scan_matching"
|
||||
harness = false
|
||||
|
||||
[[bench]]
|
||||
name = "grid_operations"
|
||||
harness = false
|
||||
|
||||
[[bench]]
|
||||
name = "evaluation"
|
||||
harness = false
|
||||
|
||||
[profile.release]
|
||||
opt-level = 3
|
||||
|
||||
@@ -0,0 +1,121 @@
|
||||
//! Benchmark evaluation metrics performance.
|
||||
|
||||
use criterion::{BenchmarkId, Criterion, black_box, criterion_group, criterion_main};
|
||||
use vastu_slam::Pose2D;
|
||||
use vastu_slam::evaluation::{
|
||||
AbsoluteTrajectoryError, GroundTruthConfig, GroundTruthRelations, RelationsMetrics,
|
||||
RelativePoseError,
|
||||
};
|
||||
|
||||
fn create_trajectory(n: usize, spacing: f32) -> Vec<Pose2D> {
|
||||
(0..n)
|
||||
.map(|i| Pose2D::new(i as f32 * spacing, 0.0, 0.0))
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn add_noise(trajectory: &[Pose2D], trans_std: f32) -> Vec<Pose2D> {
|
||||
trajectory
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, p)| {
|
||||
// Simple deterministic "noise"
|
||||
let noise = (
|
||||
(i as f32 * 0.7).sin() * trans_std,
|
||||
(i as f32 * 1.3).cos() * trans_std,
|
||||
);
|
||||
Pose2D::new(p.x + noise.0, p.y + noise.1, p.theta)
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn bench_ground_truth_generation(c: &mut Criterion) {
|
||||
let mut group = c.benchmark_group("ground_truth_generation");
|
||||
|
||||
for n in [50, 100, 500].iter() {
|
||||
let trajectory = create_trajectory(*n, 0.1);
|
||||
let config = GroundTruthConfig::default();
|
||||
|
||||
group.bench_with_input(BenchmarkId::from_parameter(n), n, |b, _| {
|
||||
b.iter(|| {
|
||||
let gt =
|
||||
GroundTruthRelations::generate(black_box(&trajectory), &[], config.clone());
|
||||
black_box(gt)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
group.finish();
|
||||
}
|
||||
|
||||
fn bench_relations_metrics(c: &mut Criterion) {
|
||||
let mut group = c.benchmark_group("relations_metrics");
|
||||
|
||||
for n in [50, 100, 500].iter() {
|
||||
let ground_trajectory = create_trajectory(*n, 0.1);
|
||||
let config = GroundTruthConfig::default();
|
||||
let ground_truth = GroundTruthRelations::generate(&ground_trajectory, &[], config);
|
||||
|
||||
let test_trajectory = add_noise(&ground_trajectory, 0.001);
|
||||
|
||||
group.bench_with_input(BenchmarkId::from_parameter(n), n, |b, _| {
|
||||
b.iter(|| {
|
||||
let metrics = RelationsMetrics::compute(black_box(&test_trajectory), &ground_truth);
|
||||
black_box(metrics)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
group.finish();
|
||||
}
|
||||
|
||||
fn bench_ate(c: &mut Criterion) {
|
||||
let mut group = c.benchmark_group("ate");
|
||||
|
||||
for n in [50, 100, 500].iter() {
|
||||
let ground_truth = create_trajectory(*n, 0.1);
|
||||
let estimated = add_noise(&ground_truth, 0.001);
|
||||
|
||||
group.bench_with_input(BenchmarkId::from_parameter(n), n, |b, _| {
|
||||
b.iter(|| {
|
||||
let ate = AbsoluteTrajectoryError::compute(
|
||||
black_box(&estimated),
|
||||
black_box(&ground_truth),
|
||||
);
|
||||
black_box(ate)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
group.finish();
|
||||
}
|
||||
|
||||
fn bench_rpe(c: &mut Criterion) {
|
||||
let mut group = c.benchmark_group("rpe");
|
||||
|
||||
for n in [50, 100, 500].iter() {
|
||||
let ground_truth = create_trajectory(*n, 0.1);
|
||||
let estimated = add_noise(&ground_truth, 0.001);
|
||||
|
||||
group.bench_with_input(BenchmarkId::from_parameter(n), n, |b, _| {
|
||||
b.iter(|| {
|
||||
let rpe = RelativePoseError::compute(
|
||||
black_box(&estimated),
|
||||
black_box(&ground_truth),
|
||||
1.0,
|
||||
);
|
||||
black_box(rpe)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
group.finish();
|
||||
}
|
||||
|
||||
criterion_group!(
|
||||
benches,
|
||||
bench_ground_truth_generation,
|
||||
bench_relations_metrics,
|
||||
bench_ate,
|
||||
bench_rpe
|
||||
);
|
||||
criterion_main!(benches);
|
||||
@@ -0,0 +1,149 @@
|
||||
//! Benchmark grid operations performance.
|
||||
|
||||
use criterion::{BenchmarkId, Criterion, black_box, criterion_group, criterion_main};
|
||||
use std::f32::consts::PI;
|
||||
use vastu_slam::{LidarScan, MapConfig, OccupancyGridMap, Pose2D};
|
||||
|
||||
/// Create a room scan for benchmarking.
|
||||
fn room_scan(
|
||||
room_width: f32,
|
||||
room_height: f32,
|
||||
robot_x: f32,
|
||||
robot_y: f32,
|
||||
num_points: usize,
|
||||
) -> LidarScan {
|
||||
let angle_increment = 2.0 * PI / num_points as f32;
|
||||
let max_range = (room_width * room_width + room_height * room_height).sqrt();
|
||||
|
||||
let mut ranges = Vec::with_capacity(num_points);
|
||||
let mut angles = Vec::with_capacity(num_points);
|
||||
|
||||
for i in 0..num_points {
|
||||
let angle = i as f32 * angle_increment - PI;
|
||||
angles.push(angle);
|
||||
|
||||
let cos_a = angle.cos();
|
||||
let sin_a = angle.sin();
|
||||
let mut range = max_range;
|
||||
|
||||
// Simple room ray casting
|
||||
if cos_a > 0.0 {
|
||||
let t = (room_width - robot_x) / cos_a;
|
||||
if t > 0.0 && t < range {
|
||||
let y = robot_y + t * sin_a;
|
||||
if y >= 0.0 && y <= room_height {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
if cos_a < 0.0 {
|
||||
let t = -robot_x / cos_a;
|
||||
if t > 0.0 && t < range {
|
||||
let y = robot_y + t * sin_a;
|
||||
if y >= 0.0 && y <= room_height {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
if sin_a > 0.0 {
|
||||
let t = (room_height - robot_y) / sin_a;
|
||||
if t > 0.0 && t < range {
|
||||
let x = robot_x + t * cos_a;
|
||||
if x >= 0.0 && x <= room_width {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
if sin_a < 0.0 {
|
||||
let t = -robot_y / sin_a;
|
||||
if t > 0.0 && t < range {
|
||||
let x = robot_x + t * cos_a;
|
||||
if x >= 0.0 && x <= room_width {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ranges.push(range.min(max_range));
|
||||
}
|
||||
|
||||
LidarScan::new(ranges, angles, 0.15, max_range)
|
||||
}
|
||||
|
||||
fn bench_map_update(c: &mut Criterion) {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
let scan = room_scan(6.0, 6.0, 3.0, 3.0, 360);
|
||||
let pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
// Warm up
|
||||
for _ in 0..5 {
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
c.bench_function("grid_update_360pts", |b| {
|
||||
b.iter(|| {
|
||||
let result = map.observe_lidar(black_box(&scan), black_box(pose));
|
||||
black_box(result)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
fn bench_map_update_resolutions(c: &mut Criterion) {
|
||||
let mut group = c.benchmark_group("grid_update_resolution");
|
||||
|
||||
for num_points in [180, 360, 720].iter() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
let scan = room_scan(6.0, 6.0, 3.0, 3.0, *num_points);
|
||||
let pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
// Warm up
|
||||
for _ in 0..5 {
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
group.bench_with_input(
|
||||
BenchmarkId::from_parameter(num_points),
|
||||
num_points,
|
||||
|b, _| {
|
||||
b.iter(|| {
|
||||
let result = map.observe_lidar(black_box(&scan), black_box(pose));
|
||||
black_box(result)
|
||||
})
|
||||
},
|
||||
);
|
||||
}
|
||||
|
||||
group.finish();
|
||||
}
|
||||
|
||||
fn bench_coverage_stats(c: &mut Criterion) {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Build up a map
|
||||
for i in 0..20 {
|
||||
let angle = i as f32 * 0.2;
|
||||
let pose = Pose2D::new(3.0, 3.0, angle);
|
||||
let scan = room_scan(6.0, 6.0, 3.0, 3.0, 360);
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
c.bench_function("coverage_stats", |b| {
|
||||
b.iter(|| {
|
||||
let stats = map.coverage_stats();
|
||||
black_box(stats)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
criterion_group!(
|
||||
benches,
|
||||
bench_map_update,
|
||||
bench_map_update_resolutions,
|
||||
bench_coverage_stats
|
||||
);
|
||||
criterion_main!(benches);
|
||||
@@ -0,0 +1,154 @@
|
||||
//! Benchmark scan matching performance.
|
||||
|
||||
use criterion::{BenchmarkId, Criterion, black_box, criterion_group, criterion_main};
|
||||
use std::f32::consts::PI;
|
||||
use vastu_slam::{
|
||||
CorrelativeMatcher, CorrelativeMatcherConfig, LidarScan, MapConfig, OccupancyGridMap, Pose2D,
|
||||
};
|
||||
|
||||
/// Create a room scan for benchmarking.
|
||||
fn room_scan(
|
||||
room_width: f32,
|
||||
room_height: f32,
|
||||
robot_x: f32,
|
||||
robot_y: f32,
|
||||
num_points: usize,
|
||||
) -> LidarScan {
|
||||
let angle_increment = 2.0 * PI / num_points as f32;
|
||||
let max_range = (room_width * room_width + room_height * room_height).sqrt();
|
||||
|
||||
let mut ranges = Vec::with_capacity(num_points);
|
||||
let mut angles = Vec::with_capacity(num_points);
|
||||
|
||||
for i in 0..num_points {
|
||||
let angle = i as f32 * angle_increment - PI;
|
||||
angles.push(angle);
|
||||
|
||||
let cos_a = angle.cos();
|
||||
let sin_a = angle.sin();
|
||||
let mut range = max_range;
|
||||
|
||||
// Right wall
|
||||
if cos_a > 0.0 {
|
||||
let t = (room_width - robot_x) / cos_a;
|
||||
if t > 0.0 && t < range {
|
||||
let y = robot_y + t * sin_a;
|
||||
if y >= 0.0 && y <= room_height {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Left wall
|
||||
if cos_a < 0.0 {
|
||||
let t = -robot_x / cos_a;
|
||||
if t > 0.0 && t < range {
|
||||
let y = robot_y + t * sin_a;
|
||||
if y >= 0.0 && y <= room_height {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Top wall
|
||||
if sin_a > 0.0 {
|
||||
let t = (room_height - robot_y) / sin_a;
|
||||
if t > 0.0 && t < range {
|
||||
let x = robot_x + t * cos_a;
|
||||
if x >= 0.0 && x <= room_width {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Bottom wall
|
||||
if sin_a < 0.0 {
|
||||
let t = -robot_y / sin_a;
|
||||
if t > 0.0 && t < range {
|
||||
let x = robot_x + t * cos_a;
|
||||
if x >= 0.0 && x <= room_width {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ranges.push(range.min(max_range));
|
||||
}
|
||||
|
||||
LidarScan::new(ranges, angles, 0.15, max_range)
|
||||
}
|
||||
|
||||
fn setup_map() -> (OccupancyGridMap, LidarScan) {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Initialize map
|
||||
for i in 0..10 {
|
||||
let angle = i as f32 * 0.1;
|
||||
let pose = Pose2D::new(3.0, 3.0, angle);
|
||||
let scan = room_scan(6.0, 6.0, 3.0, 3.0, 360);
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
let scan = room_scan(6.0, 6.0, 3.1, 3.0, 360);
|
||||
(map, scan)
|
||||
}
|
||||
|
||||
fn bench_scan_matching(c: &mut Criterion) {
|
||||
let (map, scan) = setup_map();
|
||||
let initial_pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
let config = CorrelativeMatcherConfig::default();
|
||||
let matcher = CorrelativeMatcher::new(config);
|
||||
|
||||
c.bench_function("scan_matching_360pts", |b| {
|
||||
b.iter(|| {
|
||||
let result =
|
||||
matcher.match_scan(black_box(&scan), black_box(initial_pose), map.storage());
|
||||
black_box(result)
|
||||
})
|
||||
});
|
||||
}
|
||||
|
||||
fn bench_scan_matching_resolutions(c: &mut Criterion) {
|
||||
let mut group = c.benchmark_group("scan_matching_resolution");
|
||||
|
||||
for num_points in [180, 360, 720].iter() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
for i in 0..10 {
|
||||
let angle = i as f32 * 0.1;
|
||||
let pose = Pose2D::new(3.0, 3.0, angle);
|
||||
let scan = room_scan(6.0, 6.0, 3.0, 3.0, *num_points);
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
let scan = room_scan(6.0, 6.0, 3.1, 3.0, *num_points);
|
||||
let initial_pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
let matcher_config = CorrelativeMatcherConfig::default();
|
||||
let matcher = CorrelativeMatcher::new(matcher_config);
|
||||
|
||||
group.bench_with_input(
|
||||
BenchmarkId::from_parameter(num_points),
|
||||
num_points,
|
||||
|b, _| {
|
||||
b.iter(|| {
|
||||
let result = matcher.match_scan(
|
||||
black_box(&scan),
|
||||
black_box(initial_pose),
|
||||
map.storage(),
|
||||
);
|
||||
black_box(result)
|
||||
})
|
||||
},
|
||||
);
|
||||
}
|
||||
|
||||
group.finish();
|
||||
}
|
||||
|
||||
criterion_group!(
|
||||
benches,
|
||||
bench_scan_matching,
|
||||
bench_scan_matching_resolutions
|
||||
);
|
||||
criterion_main!(benches);
|
||||
@@ -0,0 +1,504 @@
|
||||
//! Absolute and Relative Trajectory Error metrics.
|
||||
//!
|
||||
//! This module provides ATE (Absolute Trajectory Error) and RPE (Relative Pose Error)
|
||||
//! metrics that are compatible with the [evo](https://github.com/MichaelGrupp/evo)
|
||||
//! trajectory evaluation tool.
|
||||
//!
|
||||
//! ## When to Use
|
||||
//!
|
||||
//! - **Pose Relations** (primary): Use for self-evaluation without external ground truth
|
||||
//! - **ATE/RPE** (secondary): Use for cross-validation when external ground truth is available
|
||||
//!
|
||||
//! ## Metrics
|
||||
//!
|
||||
//! - **ATE**: Global accuracy after SE(2) alignment. Measures overall trajectory quality.
|
||||
//! - **RPE**: Local drift over fixed distances. Measures odometry consistency.
|
||||
|
||||
use crate::Pose2D;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Container for all accuracy metrics.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct AccuracyMetrics {
|
||||
/// Absolute Trajectory Error
|
||||
pub ate: AbsoluteTrajectoryError,
|
||||
|
||||
/// Relative Pose Error
|
||||
pub rpe: RelativePoseError,
|
||||
}
|
||||
|
||||
impl AccuracyMetrics {
|
||||
/// Compute all accuracy metrics.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `estimated` - Estimated trajectory from SLAM
|
||||
/// * `ground_truth` - Ground truth trajectory (from simulation or external sensor)
|
||||
/// * `delta_m` - Distance delta for RPE computation (meters)
|
||||
pub fn compute(estimated: &[Pose2D], ground_truth: &[Pose2D], delta_m: f32) -> Self {
|
||||
Self {
|
||||
ate: AbsoluteTrajectoryError::compute(estimated, ground_truth),
|
||||
rpe: RelativePoseError::compute(estimated, ground_truth, delta_m),
|
||||
}
|
||||
}
|
||||
|
||||
/// Print all metrics.
|
||||
pub fn print(&self) {
|
||||
self.ate.print();
|
||||
println!();
|
||||
self.rpe.print();
|
||||
}
|
||||
}
|
||||
|
||||
/// Error statistics for trajectory comparison.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct TrajectoryError {
|
||||
/// Root mean square error
|
||||
pub rmse: f32,
|
||||
|
||||
/// Mean error
|
||||
pub mean: f32,
|
||||
|
||||
/// Standard deviation
|
||||
pub std: f32,
|
||||
|
||||
/// Minimum error
|
||||
pub min: f32,
|
||||
|
||||
/// Maximum error
|
||||
pub max: f32,
|
||||
|
||||
/// Median error
|
||||
pub median: f32,
|
||||
|
||||
/// Number of samples
|
||||
pub count: usize,
|
||||
}
|
||||
|
||||
impl TrajectoryError {
|
||||
/// Compute statistics from a list of errors.
|
||||
pub fn from_errors(errors: &[f32]) -> Self {
|
||||
if errors.is_empty() {
|
||||
return Self::default();
|
||||
}
|
||||
|
||||
let count = errors.len();
|
||||
let n = count as f32;
|
||||
|
||||
let sum: f32 = errors.iter().sum();
|
||||
let mean = sum / n;
|
||||
|
||||
let sum_sq: f32 = errors.iter().map(|e| e * e).sum();
|
||||
let rmse = (sum_sq / n).sqrt();
|
||||
|
||||
let variance = errors.iter().map(|e| (e - mean).powi(2)).sum::<f32>() / n;
|
||||
let std = variance.sqrt();
|
||||
|
||||
let min = errors.iter().cloned().fold(f32::INFINITY, f32::min);
|
||||
let max = errors.iter().cloned().fold(f32::NEG_INFINITY, f32::max);
|
||||
|
||||
let mut sorted = errors.to_vec();
|
||||
sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
|
||||
let median = if count.is_multiple_of(2) {
|
||||
(sorted[count / 2 - 1] + sorted[count / 2]) / 2.0
|
||||
} else {
|
||||
sorted[count / 2]
|
||||
};
|
||||
|
||||
Self {
|
||||
rmse,
|
||||
mean,
|
||||
std,
|
||||
min,
|
||||
max,
|
||||
median,
|
||||
count,
|
||||
}
|
||||
}
|
||||
|
||||
/// Format as a single-line summary.
|
||||
pub fn summary(&self) -> String {
|
||||
format!(
|
||||
"rmse: {:.4}, mean: {:.4}, std: {:.4}, min: {:.4}, max: {:.4}",
|
||||
self.rmse, self.mean, self.std, self.min, self.max
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
/// Absolute Trajectory Error (ATE).
|
||||
///
|
||||
/// Measures the global accuracy of a trajectory by computing the RMSE of position
|
||||
/// errors after SE(2) alignment (translation + rotation).
|
||||
///
|
||||
/// This is the standard metric used by evo's `evo_ape` command.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct AbsoluteTrajectoryError {
|
||||
/// Translational error statistics (meters)
|
||||
pub translation: TrajectoryError,
|
||||
|
||||
/// Rotational error statistics (radians)
|
||||
pub rotation: TrajectoryError,
|
||||
|
||||
/// Whether SE(2) alignment was applied
|
||||
pub aligned: bool,
|
||||
}
|
||||
|
||||
impl AbsoluteTrajectoryError {
|
||||
/// Compute ATE with SE(2) alignment.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `estimated` - Estimated trajectory from SLAM
|
||||
/// * `ground_truth` - Ground truth trajectory
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// ATE metrics after aligning trajectories.
|
||||
pub fn compute(estimated: &[Pose2D], ground_truth: &[Pose2D]) -> Self {
|
||||
Self::compute_with_options(estimated, ground_truth, true)
|
||||
}
|
||||
|
||||
/// Compute ATE with optional alignment.
|
||||
pub fn compute_with_options(
|
||||
estimated: &[Pose2D],
|
||||
ground_truth: &[Pose2D],
|
||||
align: bool,
|
||||
) -> Self {
|
||||
let n = estimated.len().min(ground_truth.len());
|
||||
if n == 0 {
|
||||
return Self::default();
|
||||
}
|
||||
|
||||
// Optionally align trajectories
|
||||
let aligned_estimated = if align {
|
||||
align_trajectory_se2(estimated, ground_truth)
|
||||
} else {
|
||||
estimated.to_vec()
|
||||
};
|
||||
|
||||
// Compute per-pose errors
|
||||
let mut trans_errors = Vec::with_capacity(n);
|
||||
let mut rot_errors = Vec::with_capacity(n);
|
||||
|
||||
for i in 0..n {
|
||||
let est = &aligned_estimated[i];
|
||||
let gt = &ground_truth[i];
|
||||
|
||||
let trans_error = est.distance(gt);
|
||||
let rot_error = (est.theta - gt.theta).abs();
|
||||
|
||||
trans_errors.push(trans_error);
|
||||
rot_errors.push(rot_error);
|
||||
}
|
||||
|
||||
Self {
|
||||
translation: TrajectoryError::from_errors(&trans_errors),
|
||||
rotation: TrajectoryError::from_errors(&rot_errors),
|
||||
aligned: align,
|
||||
}
|
||||
}
|
||||
|
||||
/// Print ATE metrics.
|
||||
pub fn print(&self) {
|
||||
println!("=== Absolute Trajectory Error (ATE) ===");
|
||||
println!("Alignment: {}", if self.aligned { "SE(2)" } else { "None" });
|
||||
println!(
|
||||
"Translation RMSE: {:.6} m (mean: {:.6}, std: {:.6})",
|
||||
self.translation.rmse, self.translation.mean, self.translation.std
|
||||
);
|
||||
println!(
|
||||
"Rotation RMSE: {:.6} rad (mean: {:.6}, std: {:.6})",
|
||||
self.rotation.rmse, self.rotation.mean, self.rotation.std
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
/// Relative Pose Error (RPE).
|
||||
///
|
||||
/// Measures local drift by comparing relative poses over fixed distances.
|
||||
/// This is useful for evaluating odometry quality independently of global drift.
|
||||
///
|
||||
/// This is the standard metric used by evo's `evo_rpe` command.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct RelativePoseError {
|
||||
/// Translational error statistics (meters)
|
||||
pub translation: TrajectoryError,
|
||||
|
||||
/// Rotational error statistics (radians)
|
||||
pub rotation: TrajectoryError,
|
||||
|
||||
/// Distance delta used for computation (meters)
|
||||
pub delta_m: f32,
|
||||
|
||||
/// Number of pairs evaluated
|
||||
pub pairs_evaluated: usize,
|
||||
}
|
||||
|
||||
impl RelativePoseError {
|
||||
/// Compute RPE with a fixed distance delta.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `estimated` - Estimated trajectory from SLAM
|
||||
/// * `ground_truth` - Ground truth trajectory
|
||||
/// * `delta_m` - Distance between pose pairs (meters)
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// RPE metrics.
|
||||
pub fn compute(estimated: &[Pose2D], ground_truth: &[Pose2D], delta_m: f32) -> Self {
|
||||
let n = estimated.len().min(ground_truth.len());
|
||||
if n < 2 || delta_m <= 0.0 {
|
||||
return Self {
|
||||
delta_m,
|
||||
..Default::default()
|
||||
};
|
||||
}
|
||||
|
||||
// Compute cumulative distances in ground truth
|
||||
let mut cumulative_dist = vec![0.0f32; n];
|
||||
for i in 1..n {
|
||||
cumulative_dist[i] =
|
||||
cumulative_dist[i - 1] + ground_truth[i].distance(&ground_truth[i - 1]);
|
||||
}
|
||||
|
||||
let mut trans_errors = Vec::new();
|
||||
let mut rot_errors = Vec::new();
|
||||
|
||||
// For each pose, find the next pose that's delta_m away
|
||||
for i in 0..n {
|
||||
// Binary search for pose at distance delta_m
|
||||
let target_dist = cumulative_dist[i] + delta_m;
|
||||
|
||||
// Find j such that cumulative_dist[j] >= target_dist
|
||||
let j = match cumulative_dist[i..].binary_search_by(|d| {
|
||||
d.partial_cmp(&target_dist)
|
||||
.unwrap_or(std::cmp::Ordering::Less)
|
||||
}) {
|
||||
Ok(idx) => i + idx,
|
||||
Err(idx) => {
|
||||
if i + idx >= n {
|
||||
continue;
|
||||
}
|
||||
i + idx
|
||||
}
|
||||
};
|
||||
|
||||
if j >= n {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Compute relative poses
|
||||
let est_rel = estimated[i].inverse().compose(&estimated[j]);
|
||||
let gt_rel = ground_truth[i].inverse().compose(&ground_truth[j]);
|
||||
|
||||
// Error in relative pose
|
||||
let error_rel = gt_rel.inverse().compose(&est_rel);
|
||||
|
||||
trans_errors.push(error_rel.position().length());
|
||||
rot_errors.push(error_rel.theta.abs());
|
||||
}
|
||||
|
||||
Self {
|
||||
translation: TrajectoryError::from_errors(&trans_errors),
|
||||
rotation: TrajectoryError::from_errors(&rot_errors),
|
||||
delta_m,
|
||||
pairs_evaluated: trans_errors.len(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Print RPE metrics.
|
||||
pub fn print(&self) {
|
||||
println!("=== Relative Pose Error (RPE) ===");
|
||||
println!("Delta: {} m, Pairs: {}", self.delta_m, self.pairs_evaluated);
|
||||
println!(
|
||||
"Translation RMSE: {:.6} m (mean: {:.6}, std: {:.6})",
|
||||
self.translation.rmse, self.translation.mean, self.translation.std
|
||||
);
|
||||
println!(
|
||||
"Rotation RMSE: {:.6} rad (mean: {:.6}, std: {:.6})",
|
||||
self.rotation.rmse, self.rotation.mean, self.rotation.std
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
/// Align a trajectory to a reference using SE(2) transformation.
|
||||
///
|
||||
/// Finds the optimal translation and rotation to minimize position error.
|
||||
/// Uses Horn's method for 2D point cloud registration.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `estimated` - Trajectory to align
|
||||
/// * `reference` - Reference trajectory (ground truth)
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Aligned trajectory.
|
||||
pub fn align_trajectory_se2(estimated: &[Pose2D], reference: &[Pose2D]) -> Vec<Pose2D> {
|
||||
let n = estimated.len().min(reference.len());
|
||||
if n == 0 {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
// Compute centroids
|
||||
let est_centroid = compute_centroid(estimated);
|
||||
let ref_centroid = compute_centroid(reference);
|
||||
|
||||
// Center the point clouds
|
||||
let est_centered: Vec<(f32, f32)> = estimated
|
||||
.iter()
|
||||
.take(n)
|
||||
.map(|p| (p.x - est_centroid.0, p.y - est_centroid.1))
|
||||
.collect();
|
||||
let ref_centered: Vec<(f32, f32)> = reference
|
||||
.iter()
|
||||
.take(n)
|
||||
.map(|p| (p.x - ref_centroid.0, p.y - ref_centroid.1))
|
||||
.collect();
|
||||
|
||||
// Compute optimal rotation using SVD-like approach for 2D
|
||||
// For 2D, we can compute this directly without full SVD
|
||||
let mut sxx = 0.0f32;
|
||||
let mut sxy = 0.0f32;
|
||||
let mut syx = 0.0f32;
|
||||
let mut syy = 0.0f32;
|
||||
|
||||
for i in 0..n {
|
||||
sxx += est_centered[i].0 * ref_centered[i].0;
|
||||
sxy += est_centered[i].0 * ref_centered[i].1;
|
||||
syx += est_centered[i].1 * ref_centered[i].0;
|
||||
syy += est_centered[i].1 * ref_centered[i].1;
|
||||
}
|
||||
|
||||
// Optimal rotation angle
|
||||
let theta = (sxy - syx).atan2(sxx + syy);
|
||||
let cos_t = theta.cos();
|
||||
let sin_t = theta.sin();
|
||||
|
||||
// Compute translation
|
||||
let tx = ref_centroid.0 - (cos_t * est_centroid.0 - sin_t * est_centroid.1);
|
||||
let ty = ref_centroid.1 - (sin_t * est_centroid.0 + cos_t * est_centroid.1);
|
||||
|
||||
// Apply transformation
|
||||
estimated
|
||||
.iter()
|
||||
.map(|p| {
|
||||
Pose2D::new(
|
||||
cos_t * p.x - sin_t * p.y + tx,
|
||||
sin_t * p.x + cos_t * p.y + ty,
|
||||
p.theta + theta,
|
||||
)
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn compute_centroid(poses: &[Pose2D]) -> (f32, f32) {
|
||||
if poses.is_empty() {
|
||||
return (0.0, 0.0);
|
||||
}
|
||||
let n = poses.len() as f32;
|
||||
let sum_x: f32 = poses.iter().map(|p| p.x).sum();
|
||||
let sum_y: f32 = poses.iter().map(|p| p.y).sum();
|
||||
(sum_x / n, sum_y / n)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::f32::consts::FRAC_PI_4;
|
||||
|
||||
fn create_straight_trajectory(n: usize, spacing: f32) -> Vec<Pose2D> {
|
||||
(0..n)
|
||||
.map(|i| Pose2D::new(i as f32 * spacing, 0.0, 0.0))
|
||||
.collect()
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_trajectory_error_stats() {
|
||||
let errors = vec![1.0, 2.0, 3.0, 4.0, 5.0];
|
||||
let stats = TrajectoryError::from_errors(&errors);
|
||||
|
||||
assert_eq!(stats.count, 5);
|
||||
assert!((stats.mean - 3.0).abs() < 1e-5);
|
||||
assert!((stats.min - 1.0).abs() < 1e-5);
|
||||
assert!((stats.max - 5.0).abs() < 1e-5);
|
||||
assert!((stats.median - 3.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_ate_perfect_trajectory() {
|
||||
let trajectory = create_straight_trajectory(10, 0.5);
|
||||
|
||||
let ate = AbsoluteTrajectoryError::compute(&trajectory, &trajectory);
|
||||
|
||||
assert!(ate.translation.rmse < 1e-5);
|
||||
assert!(ate.rotation.rmse < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_ate_with_offset() {
|
||||
let ground_truth = create_straight_trajectory(10, 0.5);
|
||||
|
||||
// Estimated with constant offset
|
||||
let estimated: Vec<Pose2D> = ground_truth
|
||||
.iter()
|
||||
.map(|p| Pose2D::new(p.x + 0.1, p.y + 0.1, p.theta))
|
||||
.collect();
|
||||
|
||||
// Without alignment
|
||||
let ate_no_align =
|
||||
AbsoluteTrajectoryError::compute_with_options(&estimated, &ground_truth, false);
|
||||
assert!((ate_no_align.translation.rmse - 0.1414).abs() < 0.01);
|
||||
|
||||
// With alignment, error should be near zero
|
||||
let ate_aligned = AbsoluteTrajectoryError::compute(&estimated, &ground_truth);
|
||||
assert!(ate_aligned.translation.rmse < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rpe_perfect_trajectory() {
|
||||
let trajectory = create_straight_trajectory(10, 0.5);
|
||||
|
||||
let rpe = RelativePoseError::compute(&trajectory, &trajectory, 1.0);
|
||||
|
||||
assert!(rpe.translation.rmse < 1e-5);
|
||||
assert!(rpe.rotation.rmse < 1e-5);
|
||||
assert!(rpe.pairs_evaluated > 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_align_trajectory() {
|
||||
let reference = create_straight_trajectory(5, 1.0);
|
||||
|
||||
// Estimated with rotation and translation
|
||||
let estimated: Vec<Pose2D> = reference
|
||||
.iter()
|
||||
.map(|p| {
|
||||
let cos_t = FRAC_PI_4.cos();
|
||||
let sin_t = FRAC_PI_4.sin();
|
||||
Pose2D::new(
|
||||
cos_t * p.x - sin_t * p.y + 1.0,
|
||||
sin_t * p.x + cos_t * p.y + 2.0,
|
||||
p.theta + FRAC_PI_4,
|
||||
)
|
||||
})
|
||||
.collect();
|
||||
|
||||
let aligned = align_trajectory_se2(&estimated, &reference);
|
||||
|
||||
// After alignment, should be close to reference
|
||||
for (a, r) in aligned.iter().zip(reference.iter()) {
|
||||
assert!(a.distance(r) < 0.01);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_accuracy_metrics_compute() {
|
||||
let trajectory = create_straight_trajectory(10, 0.5);
|
||||
let metrics = AccuracyMetrics::compute(&trajectory, &trajectory, 1.0);
|
||||
|
||||
assert!(metrics.ate.translation.rmse < 1e-5);
|
||||
assert!(metrics.rpe.translation.rmse < 1e-5);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,329 @@
|
||||
//! Ground truth generation for Cartographer-style evaluation.
|
||||
//!
|
||||
//! This module provides the [`GroundTruthRelations`] type for extracting pose relations
|
||||
//! from an optimized trajectory, equivalent to Cartographer's `autogenerate_ground_truth`.
|
||||
//!
|
||||
//! ## Concept
|
||||
//!
|
||||
//! Instead of requiring external ground truth (GPS, motion capture), we extract
|
||||
//! **relative pose constraints** from an optimized trajectory that has had loop
|
||||
//! closures applied. These relations can then be used to evaluate other trajectories.
|
||||
|
||||
use crate::Pose2D;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Configuration for ground truth generation.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct GroundTruthConfig {
|
||||
/// Minimum distance covered between nodes to create a relation (meters).
|
||||
/// Shorter distances are less meaningful for evaluation.
|
||||
pub min_covered_distance: f32,
|
||||
|
||||
/// Maximum translational error to consider a relation valid (meters).
|
||||
/// Relations with larger errors are treated as outliers.
|
||||
pub outlier_threshold_meters: f32,
|
||||
|
||||
/// Maximum rotational error to consider a relation valid (radians).
|
||||
pub outlier_threshold_radians: f32,
|
||||
|
||||
/// Sample every N nodes to create relations.
|
||||
/// Higher values reduce computation but may miss details.
|
||||
pub node_sampling_interval: usize,
|
||||
}
|
||||
|
||||
impl Default for GroundTruthConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
min_covered_distance: 0.5, // 50cm minimum
|
||||
outlier_threshold_meters: 0.05, // 5cm threshold
|
||||
outlier_threshold_radians: 0.035, // ~2 degrees
|
||||
node_sampling_interval: 1,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// A pose relation constraint between two trajectory nodes.
|
||||
///
|
||||
/// This represents the relative transform from one node to another,
|
||||
/// along with the distance traveled between them (for weighting).
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct PoseRelation {
|
||||
/// Index of the starting node.
|
||||
pub from_node: usize,
|
||||
|
||||
/// Index of the ending node.
|
||||
pub to_node: usize,
|
||||
|
||||
/// Relative pose: transform from `from_node` to `to_node`.
|
||||
/// Apply as: to_pose = from_pose.compose(&relative_pose)
|
||||
pub relative_pose: Pose2D,
|
||||
|
||||
/// Distance covered between nodes (cumulative path length).
|
||||
/// Used for weighting in metric computation.
|
||||
pub covered_distance: f32,
|
||||
}
|
||||
|
||||
impl PoseRelation {
|
||||
/// Create a new pose relation.
|
||||
pub fn new(
|
||||
from_node: usize,
|
||||
to_node: usize,
|
||||
relative_pose: Pose2D,
|
||||
covered_distance: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
from_node,
|
||||
to_node,
|
||||
relative_pose,
|
||||
covered_distance,
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute the relative pose from two absolute poses.
|
||||
pub fn from_poses(
|
||||
from_node: usize,
|
||||
to_node: usize,
|
||||
from_pose: &Pose2D,
|
||||
to_pose: &Pose2D,
|
||||
covered_distance: f32,
|
||||
) -> Self {
|
||||
// relative_pose = from_pose.inverse().compose(to_pose)
|
||||
let relative_pose = from_pose.inverse().compose(to_pose);
|
||||
Self::new(from_node, to_node, relative_pose, covered_distance)
|
||||
}
|
||||
}
|
||||
|
||||
/// Ground truth relations extracted from an optimized trajectory.
|
||||
///
|
||||
/// This is the Cartographer-style ground truth format. It stores relative
|
||||
/// pose constraints between trajectory nodes, which can be compared against
|
||||
/// a test trajectory to compute accuracy metrics.
|
||||
#[derive(Clone, Debug, Serialize, Deserialize)]
|
||||
pub struct GroundTruthRelations {
|
||||
/// The pose relations extracted from the optimized trajectory.
|
||||
pub relations: Vec<PoseRelation>,
|
||||
|
||||
/// Configuration used for generation.
|
||||
pub config: GroundTruthConfig,
|
||||
|
||||
/// Total trajectory length (for reference).
|
||||
pub trajectory_length: f32,
|
||||
|
||||
/// Number of nodes in the original trajectory.
|
||||
pub num_nodes: usize,
|
||||
}
|
||||
|
||||
impl GroundTruthRelations {
|
||||
/// Generate ground truth relations from an optimized trajectory.
|
||||
///
|
||||
/// This is equivalent to Cartographer's `autogenerate_ground_truth`.
|
||||
/// It extracts relative pose constraints between nodes in the trajectory.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `optimized_trajectory` - Poses from an optimized trajectory (with loop closures)
|
||||
/// * `loop_closures` - Pairs of node indices that form loop closures (optional)
|
||||
/// * `config` - Configuration for relation generation
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Ground truth relations that can be used for evaluation.
|
||||
pub fn generate(
|
||||
optimized_trajectory: &[Pose2D],
|
||||
loop_closures: &[(usize, usize)],
|
||||
config: GroundTruthConfig,
|
||||
) -> Self {
|
||||
let num_nodes = optimized_trajectory.len();
|
||||
if num_nodes < 2 {
|
||||
return Self {
|
||||
relations: Vec::new(),
|
||||
config,
|
||||
trajectory_length: 0.0,
|
||||
num_nodes,
|
||||
};
|
||||
}
|
||||
|
||||
// Compute cumulative distances along trajectory
|
||||
let mut cumulative_distances = vec![0.0f32; num_nodes];
|
||||
for i in 1..num_nodes {
|
||||
let dist = optimized_trajectory[i].distance(&optimized_trajectory[i - 1]);
|
||||
cumulative_distances[i] = cumulative_distances[i - 1] + dist;
|
||||
}
|
||||
let trajectory_length = cumulative_distances[num_nodes - 1];
|
||||
|
||||
let mut relations = Vec::new();
|
||||
|
||||
// Generate sequential relations
|
||||
let interval = config.node_sampling_interval.max(1);
|
||||
for i in (0..num_nodes).step_by(interval) {
|
||||
for j in ((i + 1)..num_nodes).step_by(interval) {
|
||||
let covered_distance = cumulative_distances[j] - cumulative_distances[i];
|
||||
|
||||
// Skip if distance is too short
|
||||
if covered_distance < config.min_covered_distance {
|
||||
continue;
|
||||
}
|
||||
|
||||
let relation = PoseRelation::from_poses(
|
||||
i,
|
||||
j,
|
||||
&optimized_trajectory[i],
|
||||
&optimized_trajectory[j],
|
||||
covered_distance,
|
||||
);
|
||||
|
||||
relations.push(relation);
|
||||
}
|
||||
}
|
||||
|
||||
// Add loop closure relations with high weight
|
||||
for &(from, to) in loop_closures {
|
||||
if from < num_nodes && to < num_nodes && from != to {
|
||||
// For loop closures, covered distance is the full loop
|
||||
let covered_distance = if from < to {
|
||||
cumulative_distances[to] - cumulative_distances[from]
|
||||
} else {
|
||||
cumulative_distances[from] - cumulative_distances[to]
|
||||
};
|
||||
|
||||
let relation = PoseRelation::from_poses(
|
||||
from.min(to),
|
||||
from.max(to),
|
||||
&optimized_trajectory[from.min(to)],
|
||||
&optimized_trajectory[from.max(to)],
|
||||
covered_distance,
|
||||
);
|
||||
|
||||
// Avoid duplicates
|
||||
if !relations
|
||||
.iter()
|
||||
.any(|r| r.from_node == relation.from_node && r.to_node == relation.to_node)
|
||||
{
|
||||
relations.push(relation);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Self {
|
||||
relations,
|
||||
config,
|
||||
trajectory_length,
|
||||
num_nodes,
|
||||
}
|
||||
}
|
||||
|
||||
/// Generate ground truth from a trajectory with timestamps.
|
||||
///
|
||||
/// Convenience method that accepts timestamped poses.
|
||||
pub fn generate_timestamped(
|
||||
trajectory: &[(f64, Pose2D)],
|
||||
loop_closures: &[(usize, usize)],
|
||||
config: GroundTruthConfig,
|
||||
) -> Self {
|
||||
let poses: Vec<Pose2D> = trajectory.iter().map(|(_, pose)| *pose).collect();
|
||||
Self::generate(&poses, loop_closures, config)
|
||||
}
|
||||
|
||||
/// Get the number of relations.
|
||||
pub fn len(&self) -> usize {
|
||||
self.relations.len()
|
||||
}
|
||||
|
||||
/// Check if there are no relations.
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.relations.is_empty()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::f32::consts::FRAC_PI_2;
|
||||
|
||||
#[test]
|
||||
fn test_pose_relation_from_poses() {
|
||||
let from_pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let to_pose = Pose2D::new(1.0, 0.0, 0.0);
|
||||
|
||||
let relation = PoseRelation::from_poses(0, 1, &from_pose, &to_pose, 1.0);
|
||||
|
||||
assert_eq!(relation.from_node, 0);
|
||||
assert_eq!(relation.to_node, 1);
|
||||
assert!((relation.relative_pose.x - 1.0).abs() < 1e-5);
|
||||
assert!((relation.relative_pose.y - 0.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_pose_relation_with_rotation() {
|
||||
let from_pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let to_pose = Pose2D::new(1.0, 1.0, FRAC_PI_2);
|
||||
|
||||
let relation = PoseRelation::from_poses(0, 1, &from_pose, &to_pose, 1.414);
|
||||
|
||||
// Relative pose should encode the transform
|
||||
let reconstructed = from_pose.compose(&relation.relative_pose);
|
||||
assert!((reconstructed.x - to_pose.x).abs() < 1e-5);
|
||||
assert!((reconstructed.y - to_pose.y).abs() < 1e-5);
|
||||
assert!((reconstructed.theta - to_pose.theta).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_ground_truth_generation() {
|
||||
// Simple straight-line trajectory
|
||||
let trajectory: Vec<Pose2D> = (0..10)
|
||||
.map(|i| Pose2D::new(i as f32 * 0.5, 0.0, 0.0))
|
||||
.collect();
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
node_sampling_interval: 1,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let ground_truth = GroundTruthRelations::generate(&trajectory, &[], config);
|
||||
|
||||
assert!(ground_truth.len() > 0);
|
||||
assert_eq!(ground_truth.num_nodes, 10);
|
||||
assert!((ground_truth.trajectory_length - 4.5).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_ground_truth_with_loop_closures() {
|
||||
// Square trajectory
|
||||
let trajectory = vec![
|
||||
Pose2D::new(0.0, 0.0, 0.0),
|
||||
Pose2D::new(1.0, 0.0, 0.0),
|
||||
Pose2D::new(1.0, 1.0, FRAC_PI_2),
|
||||
Pose2D::new(0.0, 1.0, std::f32::consts::PI),
|
||||
Pose2D::new(0.0, 0.0, -FRAC_PI_2), // Back to start
|
||||
];
|
||||
|
||||
let loop_closures = vec![(0, 4)]; // Loop closure between first and last
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let ground_truth = GroundTruthRelations::generate(&trajectory, &loop_closures, config);
|
||||
|
||||
// Should have relations including the loop closure
|
||||
assert!(!ground_truth.is_empty());
|
||||
assert!(
|
||||
ground_truth
|
||||
.relations
|
||||
.iter()
|
||||
.any(|r| (r.from_node == 0 && r.to_node == 4)
|
||||
|| (r.from_node == 4 && r.to_node == 0))
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_empty_trajectory() {
|
||||
let config = GroundTruthConfig::default();
|
||||
let ground_truth = GroundTruthRelations::generate(&[], &[], config);
|
||||
|
||||
assert!(ground_truth.is_empty());
|
||||
assert_eq!(ground_truth.trajectory_length, 0.0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
//! Cartographer-style evaluation framework for VastuSLAM.
|
||||
//!
|
||||
//! This module provides tools for evaluating SLAM accuracy, performance, and quality
|
||||
//! following the approach used by Google Cartographer.
|
||||
//!
|
||||
//! ## Overview
|
||||
//!
|
||||
//! The evaluation framework is built on three pillars:
|
||||
//!
|
||||
//! 1. **Accuracy** - Pose relations metric, ATE, RPE
|
||||
//! 2. **Performance** - Latency benchmarks, resource usage
|
||||
//! 3. **Quality** - Map consistency, loop closure precision/recall
|
||||
//!
|
||||
//! ## Cartographer-style Evaluation
|
||||
//!
|
||||
//! The core idea is to evaluate using **relative poses** between trajectory nodes,
|
||||
//! which works without external ground truth (GPS, motion capture).
|
||||
//!
|
||||
//! ### Two-step process:
|
||||
//!
|
||||
//! 1. **Generate ground truth**: Extract pose relations from an optimized trajectory
|
||||
//! (with loop closures applied)
|
||||
//! 2. **Evaluate**: Compare a test trajectory against these ground truth relations
|
||||
//!
|
||||
//! ## Example
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_slam::evaluation::{GroundTruthRelations, RelationsMetrics, GroundTruthConfig};
|
||||
//!
|
||||
//! // Step 1: Generate ground truth from optimized trajectory
|
||||
//! let config = GroundTruthConfig::default();
|
||||
//! let ground_truth = GroundTruthRelations::generate(
|
||||
//! &optimized_poses,
|
||||
//! &loop_closures,
|
||||
//! config,
|
||||
//! );
|
||||
//!
|
||||
//! // Step 2: Evaluate test trajectory
|
||||
//! let metrics = RelationsMetrics::compute(&test_poses, &ground_truth);
|
||||
//! metrics.print();
|
||||
//! // Output:
|
||||
//! // Abs translational error 0.00832 +/- 0.00421 m
|
||||
//! // Abs rotational error 0.15234 +/- 0.08123 deg
|
||||
//! ```
|
||||
|
||||
mod accuracy;
|
||||
mod ground_truth;
|
||||
mod performance;
|
||||
mod quality;
|
||||
mod relations_metrics;
|
||||
|
||||
pub use accuracy::{AbsoluteTrajectoryError, AccuracyMetrics, RelativePoseError, TrajectoryError};
|
||||
pub use ground_truth::{GroundTruthConfig, GroundTruthRelations, PoseRelation};
|
||||
pub use performance::{OperationTiming, PerformanceMetrics, PerformanceTracker};
|
||||
pub use quality::{LoopClosureGroundTruth, LoopClosurePR, MapQualityMetrics};
|
||||
pub use relations_metrics::{RelationError, RelationsMetrics};
|
||||
@@ -0,0 +1,389 @@
|
||||
//! Performance metrics for SLAM evaluation.
|
||||
//!
|
||||
//! This module provides timing and resource tracking for benchmarking SLAM operations.
|
||||
//! Unlike accuracy metrics, performance metrics are not part of Cartographer but are
|
||||
//! essential for maintaining real-time performance.
|
||||
//!
|
||||
//! ## Usage
|
||||
//!
|
||||
//! ```rust,ignore
|
||||
//! use vastu_slam::evaluation::PerformanceTracker;
|
||||
//!
|
||||
//! let mut tracker = PerformanceTracker::new();
|
||||
//!
|
||||
//! // Time an operation
|
||||
//! let start = std::time::Instant::now();
|
||||
//! // ... do scan matching ...
|
||||
//! tracker.record("scan_matching", start.elapsed());
|
||||
//!
|
||||
//! // Get metrics
|
||||
//! let metrics = tracker.metrics();
|
||||
//! metrics.print();
|
||||
//! ```
|
||||
|
||||
use serde::{Deserialize, Serialize};
|
||||
use std::collections::HashMap;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
/// Timing statistics for a single operation.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct OperationTiming {
|
||||
/// Operation name.
|
||||
pub name: String,
|
||||
|
||||
/// Mean duration (milliseconds).
|
||||
pub mean_ms: f32,
|
||||
|
||||
/// Standard deviation (milliseconds).
|
||||
pub std_ms: f32,
|
||||
|
||||
/// Minimum duration (milliseconds).
|
||||
pub min_ms: f32,
|
||||
|
||||
/// Maximum duration (milliseconds).
|
||||
pub max_ms: f32,
|
||||
|
||||
/// Median duration (milliseconds).
|
||||
pub median_ms: f32,
|
||||
|
||||
/// Total number of samples.
|
||||
pub count: usize,
|
||||
|
||||
/// Total time spent (milliseconds).
|
||||
pub total_ms: f32,
|
||||
}
|
||||
|
||||
impl OperationTiming {
|
||||
/// Create from a list of durations.
|
||||
pub fn from_durations(name: &str, durations: &[Duration]) -> Self {
|
||||
if durations.is_empty() {
|
||||
return Self {
|
||||
name: name.to_string(),
|
||||
..Default::default()
|
||||
};
|
||||
}
|
||||
|
||||
let ms_values: Vec<f32> = durations.iter().map(|d| d.as_secs_f32() * 1000.0).collect();
|
||||
let n = ms_values.len() as f32;
|
||||
|
||||
let total_ms = ms_values.iter().sum::<f32>();
|
||||
let mean_ms = total_ms / n;
|
||||
|
||||
let variance = ms_values.iter().map(|v| (v - mean_ms).powi(2)).sum::<f32>() / n;
|
||||
let std_ms = variance.sqrt();
|
||||
|
||||
let min_ms = ms_values.iter().cloned().fold(f32::INFINITY, f32::min);
|
||||
let max_ms = ms_values.iter().cloned().fold(f32::NEG_INFINITY, f32::max);
|
||||
|
||||
let mut sorted = ms_values.clone();
|
||||
sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
|
||||
let median_ms = if sorted.len().is_multiple_of(2) {
|
||||
(sorted[sorted.len() / 2 - 1] + sorted[sorted.len() / 2]) / 2.0
|
||||
} else {
|
||||
sorted[sorted.len() / 2]
|
||||
};
|
||||
|
||||
Self {
|
||||
name: name.to_string(),
|
||||
mean_ms,
|
||||
std_ms,
|
||||
min_ms,
|
||||
max_ms,
|
||||
median_ms,
|
||||
count: durations.len(),
|
||||
total_ms,
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if the operation exceeds a threshold (for regression testing).
|
||||
pub fn exceeds_threshold(&self, threshold_ms: f32) -> bool {
|
||||
self.mean_ms > threshold_ms
|
||||
}
|
||||
|
||||
/// Format as a single line.
|
||||
pub fn summary(&self) -> String {
|
||||
format!(
|
||||
"{}: {:.2} ± {:.2} ms (min: {:.2}, max: {:.2}, n={})",
|
||||
self.name, self.mean_ms, self.std_ms, self.min_ms, self.max_ms, self.count
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
/// Aggregate performance metrics.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct PerformanceMetrics {
|
||||
/// Per-operation timing.
|
||||
pub operations: HashMap<String, OperationTiming>,
|
||||
|
||||
/// Scans processed per second.
|
||||
pub scans_per_second: f32,
|
||||
|
||||
/// Real-time factor (1.0 = real-time, >1.0 = faster than real-time).
|
||||
pub real_time_factor: f32,
|
||||
|
||||
/// Total processing time (milliseconds).
|
||||
pub total_time_ms: f32,
|
||||
|
||||
/// Number of scans processed.
|
||||
pub num_scans: usize,
|
||||
}
|
||||
|
||||
impl PerformanceMetrics {
|
||||
/// Print all metrics.
|
||||
pub fn print(&self) {
|
||||
println!("=== Performance Metrics ===");
|
||||
println!("Scans processed: {}", self.num_scans);
|
||||
println!("Scans per second: {:.2}", self.scans_per_second);
|
||||
println!("Real-time factor: {:.2}x", self.real_time_factor);
|
||||
println!("Total time: {:.2} ms", self.total_time_ms);
|
||||
println!();
|
||||
println!("Operation timings:");
|
||||
|
||||
let mut ops: Vec<_> = self.operations.values().collect();
|
||||
ops.sort_by(|a, b| {
|
||||
b.total_ms
|
||||
.partial_cmp(&a.total_ms)
|
||||
.unwrap_or(std::cmp::Ordering::Equal)
|
||||
});
|
||||
|
||||
for op in ops {
|
||||
println!(" {}", op.summary());
|
||||
}
|
||||
}
|
||||
|
||||
/// Check if any operation exceeds its threshold.
|
||||
pub fn check_thresholds(&self, thresholds: &HashMap<String, f32>) -> Vec<String> {
|
||||
let mut violations = Vec::new();
|
||||
for (name, threshold) in thresholds {
|
||||
if let Some(op) = self.operations.get(name)
|
||||
&& op.exceeds_threshold(*threshold)
|
||||
{
|
||||
violations.push(format!(
|
||||
"{}: {:.2}ms exceeds threshold of {}ms",
|
||||
name, op.mean_ms, threshold
|
||||
));
|
||||
}
|
||||
}
|
||||
violations
|
||||
}
|
||||
}
|
||||
|
||||
/// Performance tracker for recording operation timings.
|
||||
#[derive(Clone, Debug, Default)]
|
||||
pub struct PerformanceTracker {
|
||||
/// Recorded durations for each operation.
|
||||
durations: HashMap<String, Vec<Duration>>,
|
||||
|
||||
/// Start time of tracking.
|
||||
start_time: Option<Instant>,
|
||||
|
||||
/// Number of scans tracked.
|
||||
scan_count: usize,
|
||||
|
||||
/// Expected scan interval (for real-time factor calculation).
|
||||
expected_scan_interval: Duration,
|
||||
}
|
||||
|
||||
impl PerformanceTracker {
|
||||
/// Create a new performance tracker.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
durations: HashMap::new(),
|
||||
start_time: None,
|
||||
scan_count: 0,
|
||||
expected_scan_interval: Duration::from_millis(200), // 5Hz default
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with a custom expected scan interval.
|
||||
pub fn with_scan_interval(interval: Duration) -> Self {
|
||||
Self {
|
||||
expected_scan_interval: interval,
|
||||
..Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
/// Start tracking (call at beginning of processing).
|
||||
pub fn start(&mut self) {
|
||||
self.start_time = Some(Instant::now());
|
||||
}
|
||||
|
||||
/// Record a timed operation.
|
||||
pub fn record(&mut self, operation: &str, duration: Duration) {
|
||||
self.durations
|
||||
.entry(operation.to_string())
|
||||
.or_default()
|
||||
.push(duration);
|
||||
}
|
||||
|
||||
/// Record the start of an operation and return a guard for automatic timing.
|
||||
pub fn time(&mut self, operation: &str) -> TimingGuard {
|
||||
TimingGuard {
|
||||
operation: operation.to_string(),
|
||||
start: Instant::now(),
|
||||
tracker: self as *mut Self,
|
||||
}
|
||||
}
|
||||
|
||||
/// Increment scan count.
|
||||
pub fn record_scan(&mut self) {
|
||||
self.scan_count += 1;
|
||||
}
|
||||
|
||||
/// Get the number of recorded scans.
|
||||
pub fn scan_count(&self) -> usize {
|
||||
self.scan_count
|
||||
}
|
||||
|
||||
/// Compute aggregate metrics.
|
||||
pub fn metrics(&self) -> PerformanceMetrics {
|
||||
let operations: HashMap<String, OperationTiming> = self
|
||||
.durations
|
||||
.iter()
|
||||
.map(|(name, durs)| (name.clone(), OperationTiming::from_durations(name, durs)))
|
||||
.collect();
|
||||
|
||||
let total_time_ms = self
|
||||
.start_time
|
||||
.map(|s| s.elapsed().as_secs_f32() * 1000.0)
|
||||
.unwrap_or(0.0);
|
||||
|
||||
let scans_per_second = if total_time_ms > 0.0 {
|
||||
self.scan_count as f32 / (total_time_ms / 1000.0)
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
// Real-time factor: how fast are we compared to expected scan rate
|
||||
let expected_scans_per_second = 1.0 / self.expected_scan_interval.as_secs_f32();
|
||||
let real_time_factor = if expected_scans_per_second > 0.0 {
|
||||
scans_per_second / expected_scans_per_second
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
PerformanceMetrics {
|
||||
operations,
|
||||
scans_per_second,
|
||||
real_time_factor,
|
||||
total_time_ms,
|
||||
num_scans: self.scan_count,
|
||||
}
|
||||
}
|
||||
|
||||
/// Reset all tracking data.
|
||||
pub fn reset(&mut self) {
|
||||
self.durations.clear();
|
||||
self.start_time = None;
|
||||
self.scan_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// Guard for automatic timing of operations.
|
||||
///
|
||||
/// When dropped, records the elapsed time to the tracker.
|
||||
pub struct TimingGuard {
|
||||
operation: String,
|
||||
start: Instant,
|
||||
tracker: *mut PerformanceTracker,
|
||||
}
|
||||
|
||||
impl Drop for TimingGuard {
|
||||
fn drop(&mut self) {
|
||||
let duration = self.start.elapsed();
|
||||
// SAFETY: The guard is always created with a valid mutable reference
|
||||
// and should not outlive the tracker.
|
||||
unsafe {
|
||||
(*self.tracker).record(&self.operation, duration);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Convenience macro for timing a block.
|
||||
#[macro_export]
|
||||
macro_rules! time_operation {
|
||||
($tracker:expr, $name:expr, $block:block) => {{
|
||||
let _guard = $tracker.time($name);
|
||||
$block
|
||||
}};
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::thread;
|
||||
|
||||
#[test]
|
||||
fn test_operation_timing() {
|
||||
let durations = vec![
|
||||
Duration::from_millis(10),
|
||||
Duration::from_millis(20),
|
||||
Duration::from_millis(30),
|
||||
];
|
||||
|
||||
let timing = OperationTiming::from_durations("test", &durations);
|
||||
|
||||
assert_eq!(timing.count, 3);
|
||||
assert!((timing.mean_ms - 20.0).abs() < 0.1);
|
||||
assert!((timing.min_ms - 10.0).abs() < 0.1);
|
||||
assert!((timing.max_ms - 30.0).abs() < 0.1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_performance_tracker() {
|
||||
let mut tracker = PerformanceTracker::new();
|
||||
tracker.start();
|
||||
|
||||
// Record some operations
|
||||
tracker.record("scan_matching", Duration::from_millis(15));
|
||||
tracker.record("scan_matching", Duration::from_millis(12));
|
||||
tracker.record("map_update", Duration::from_millis(5));
|
||||
|
||||
tracker.record_scan();
|
||||
tracker.record_scan();
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
|
||||
assert_eq!(metrics.num_scans, 2);
|
||||
assert!(metrics.operations.contains_key("scan_matching"));
|
||||
assert!(metrics.operations.contains_key("map_update"));
|
||||
|
||||
let scan_matching = metrics.operations.get("scan_matching").unwrap();
|
||||
assert_eq!(scan_matching.count, 2);
|
||||
assert!((scan_matching.mean_ms - 13.5).abs() < 0.1);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_timing_guard() {
|
||||
let mut tracker = PerformanceTracker::new();
|
||||
|
||||
{
|
||||
let _guard = tracker.time("test_op");
|
||||
thread::sleep(Duration::from_millis(10));
|
||||
}
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
let test_op = metrics.operations.get("test_op").unwrap();
|
||||
|
||||
assert_eq!(test_op.count, 1);
|
||||
assert!(test_op.mean_ms >= 10.0); // At least 10ms
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_threshold_checking() {
|
||||
let mut tracker = PerformanceTracker::new();
|
||||
tracker.record("fast_op", Duration::from_millis(5));
|
||||
tracker.record("slow_op", Duration::from_millis(50));
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
|
||||
let mut thresholds = HashMap::new();
|
||||
thresholds.insert("fast_op".to_string(), 10.0);
|
||||
thresholds.insert("slow_op".to_string(), 20.0);
|
||||
|
||||
let violations = metrics.check_thresholds(&thresholds);
|
||||
|
||||
assert_eq!(violations.len(), 1);
|
||||
assert!(violations[0].contains("slow_op"));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,511 @@
|
||||
//! Map and loop closure quality metrics.
|
||||
//!
|
||||
//! This module provides quality metrics that extend beyond accuracy and performance:
|
||||
//!
|
||||
//! - **Loop Closure Quality**: Precision/Recall for loop closure detection (RTAB-Map style)
|
||||
//! - **Map Quality**: Consistency, coverage, and sharpness metrics
|
||||
|
||||
use crate::{CellCounts, GridStorage, Pose2D};
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Loop closure precision/recall metrics.
|
||||
///
|
||||
/// Evaluates the quality of loop closure detection by comparing against ground truth.
|
||||
/// Based on RTAB-Map's benchmark methodology.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct LoopClosurePR {
|
||||
/// True positives: correctly detected loop closures.
|
||||
pub true_positives: usize,
|
||||
|
||||
/// False positives: incorrectly detected loop closures.
|
||||
pub false_positives: usize,
|
||||
|
||||
/// False negatives: missed loop closures.
|
||||
pub false_negatives: usize,
|
||||
|
||||
/// Precision: TP / (TP + FP).
|
||||
pub precision: f32,
|
||||
|
||||
/// Recall: TP / (TP + FN).
|
||||
pub recall: f32,
|
||||
|
||||
/// F1 score: harmonic mean of precision and recall.
|
||||
pub f1_score: f32,
|
||||
|
||||
/// Maximum recall at 100% precision (key benchmark metric).
|
||||
pub max_recall_at_100_precision: f32,
|
||||
}
|
||||
|
||||
impl LoopClosurePR {
|
||||
/// Compute precision/recall from detections and ground truth.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `detections` - Detected loop closures: (from_node, to_node, score)
|
||||
/// * `ground_truth` - Ground truth loop closures
|
||||
/// * `tolerance` - Position tolerance for matching (meters)
|
||||
pub fn compute(
|
||||
detections: &[(usize, usize, f32)],
|
||||
ground_truth: &LoopClosureGroundTruth,
|
||||
tolerance: f32,
|
||||
) -> Self {
|
||||
if ground_truth.closures.is_empty() && detections.is_empty() {
|
||||
return Self {
|
||||
precision: 1.0,
|
||||
recall: 1.0,
|
||||
f1_score: 1.0,
|
||||
max_recall_at_100_precision: 1.0,
|
||||
..Default::default()
|
||||
};
|
||||
}
|
||||
|
||||
// Sort detections by score (descending) for PR curve
|
||||
let mut sorted_detections: Vec<_> = detections.to_vec();
|
||||
sorted_detections
|
||||
.sort_by(|a, b| b.2.partial_cmp(&a.2).unwrap_or(std::cmp::Ordering::Equal));
|
||||
|
||||
let mut tp = 0usize;
|
||||
let mut fp = 0usize;
|
||||
let mut matched_gt: Vec<bool> = vec![false; ground_truth.closures.len()];
|
||||
|
||||
// Match detections to ground truth
|
||||
for (from, to, _score) in &sorted_detections {
|
||||
let mut is_tp = false;
|
||||
|
||||
// Check if this detection matches any ground truth
|
||||
for (i, (gt_from, gt_to)) in ground_truth.closures.iter().enumerate() {
|
||||
if matched_gt[i] {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check if nodes match (with some tolerance for node indices)
|
||||
let from_matches = (*from as i32 - *gt_from as i32).abs() <= 1;
|
||||
let to_matches = (*to as i32 - *gt_to as i32).abs() <= 1;
|
||||
|
||||
// If we have poses, also check spatial proximity
|
||||
let spatially_close =
|
||||
if let (Some(poses), _) = (&ground_truth.poses, tolerance > 0.0) {
|
||||
if *from < poses.len() && *gt_from < poses.len() {
|
||||
poses[*from].distance(&poses[*gt_from]) < tolerance
|
||||
} else {
|
||||
true
|
||||
}
|
||||
} else {
|
||||
true
|
||||
};
|
||||
|
||||
if (from_matches && to_matches) || spatially_close {
|
||||
matched_gt[i] = true;
|
||||
is_tp = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if is_tp {
|
||||
tp += 1;
|
||||
} else {
|
||||
fp += 1;
|
||||
}
|
||||
}
|
||||
|
||||
let fn_count = matched_gt.iter().filter(|&&m| !m).count();
|
||||
|
||||
let precision = if tp + fp > 0 {
|
||||
tp as f32 / (tp + fp) as f32
|
||||
} else {
|
||||
1.0
|
||||
};
|
||||
|
||||
let recall = if tp + fn_count > 0 {
|
||||
tp as f32 / (tp + fn_count) as f32
|
||||
} else {
|
||||
1.0
|
||||
};
|
||||
|
||||
let f1_score = if precision + recall > 0.0 {
|
||||
2.0 * precision * recall / (precision + recall)
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
// Compute max recall at 100% precision
|
||||
// Walk through sorted detections until we hit a false positive
|
||||
let max_recall_at_100_precision = {
|
||||
let mut running_tp = 0usize;
|
||||
let running_fp = 0usize;
|
||||
let mut best_recall = 0.0f32;
|
||||
let mut matched: Vec<bool> = vec![false; ground_truth.closures.len()];
|
||||
|
||||
for (from, to, _score) in &sorted_detections {
|
||||
let mut is_tp = false;
|
||||
|
||||
for (i, (gt_from, gt_to)) in ground_truth.closures.iter().enumerate() {
|
||||
if matched[i] {
|
||||
continue;
|
||||
}
|
||||
let from_matches = (*from as i32 - *gt_from as i32).abs() <= 1;
|
||||
let to_matches = (*to as i32 - *gt_to as i32).abs() <= 1;
|
||||
|
||||
if from_matches && to_matches {
|
||||
matched[i] = true;
|
||||
is_tp = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if is_tp {
|
||||
running_tp += 1;
|
||||
let current_recall = running_tp as f32 / ground_truth.closures.len() as f32;
|
||||
let current_precision = running_tp as f32 / (running_tp + running_fp) as f32;
|
||||
if current_precision >= 0.9999 {
|
||||
best_recall = current_recall;
|
||||
}
|
||||
} else {
|
||||
// Once we hit a FP, precision < 100%, so stop
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
best_recall
|
||||
};
|
||||
|
||||
Self {
|
||||
true_positives: tp,
|
||||
false_positives: fp,
|
||||
false_negatives: fn_count,
|
||||
precision,
|
||||
recall,
|
||||
f1_score,
|
||||
max_recall_at_100_precision,
|
||||
}
|
||||
}
|
||||
|
||||
/// Print metrics.
|
||||
pub fn print(&self) {
|
||||
println!("=== Loop Closure Quality ===");
|
||||
println!("True Positives: {}", self.true_positives);
|
||||
println!("False Positives: {}", self.false_positives);
|
||||
println!("False Negatives: {}", self.false_negatives);
|
||||
println!("Precision: {:.2}%", self.precision * 100.0);
|
||||
println!("Recall: {:.2}%", self.recall * 100.0);
|
||||
println!("F1 Score: {:.2}%", self.f1_score * 100.0);
|
||||
println!(
|
||||
"Max Recall @100% Precision: {:.2}%",
|
||||
self.max_recall_at_100_precision * 100.0
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
/// Ground truth for loop closure evaluation.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct LoopClosureGroundTruth {
|
||||
/// List of (from_node, to_node) pairs that should match.
|
||||
pub closures: Vec<(usize, usize)>,
|
||||
|
||||
/// Optional: poses for spatial matching.
|
||||
#[serde(skip)]
|
||||
pub poses: Option<Vec<Pose2D>>,
|
||||
}
|
||||
|
||||
impl LoopClosureGroundTruth {
|
||||
/// Create from a list of closure pairs.
|
||||
pub fn from_pairs(closures: Vec<(usize, usize)>) -> Self {
|
||||
Self {
|
||||
closures,
|
||||
poses: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create with poses for spatial matching.
|
||||
pub fn with_poses(closures: Vec<(usize, usize)>, poses: Vec<Pose2D>) -> Self {
|
||||
Self {
|
||||
closures,
|
||||
poses: Some(poses),
|
||||
}
|
||||
}
|
||||
|
||||
/// Generate ground truth from trajectory based on spatial proximity.
|
||||
///
|
||||
/// Two nodes form a loop closure if:
|
||||
/// 1. They are at least `min_separation` nodes apart in the sequence
|
||||
/// 2. They are within `max_distance` meters of each other
|
||||
pub fn from_trajectory(poses: &[Pose2D], min_separation: usize, max_distance: f32) -> Self {
|
||||
let mut closures = Vec::new();
|
||||
|
||||
for i in 0..poses.len() {
|
||||
for j in (i + min_separation)..poses.len() {
|
||||
if poses[i].distance(&poses[j]) < max_distance {
|
||||
closures.push((i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Self {
|
||||
closures,
|
||||
poses: Some(poses.to_vec()),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Map quality metrics.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct MapQualityMetrics {
|
||||
/// Explored area in square meters.
|
||||
pub coverage_m2: f32,
|
||||
|
||||
/// Wall sharpness (0-1, higher is better).
|
||||
/// Measures how well-defined wall boundaries are.
|
||||
pub wall_sharpness: f32,
|
||||
|
||||
/// Submap consistency (0-1, higher is better).
|
||||
/// Measures agreement at submap boundaries.
|
||||
pub submap_consistency: f32,
|
||||
|
||||
/// Cell type distribution.
|
||||
pub cell_counts: CellCounts,
|
||||
|
||||
/// Percentage of cells that are known.
|
||||
pub known_percentage: f32,
|
||||
|
||||
/// Average obstacle thickness (cells).
|
||||
pub avg_wall_thickness: f32,
|
||||
}
|
||||
|
||||
impl MapQualityMetrics {
|
||||
/// Compute quality metrics from a grid.
|
||||
pub fn compute(grid: &GridStorage) -> Self {
|
||||
let cell_counts = grid.count_by_type();
|
||||
let resolution = grid.resolution();
|
||||
|
||||
let total_cells = cell_counts.total();
|
||||
let known_cells = cell_counts.known();
|
||||
|
||||
let known_percentage = if total_cells > 0 {
|
||||
known_cells as f32 / total_cells as f32 * 100.0
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
|
||||
let coverage_m2 = known_cells as f32 * resolution * resolution;
|
||||
|
||||
// Compute wall sharpness
|
||||
let wall_sharpness = Self::compute_wall_sharpness(grid);
|
||||
|
||||
// Compute average wall thickness
|
||||
let avg_wall_thickness = Self::compute_avg_wall_thickness(grid);
|
||||
|
||||
Self {
|
||||
coverage_m2,
|
||||
wall_sharpness,
|
||||
submap_consistency: 1.0, // Placeholder - would need submap data
|
||||
cell_counts,
|
||||
known_percentage,
|
||||
avg_wall_thickness,
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute wall sharpness by looking at transitions between wall and floor.
|
||||
fn compute_wall_sharpness(grid: &GridStorage) -> f32 {
|
||||
use crate::CellType;
|
||||
|
||||
let width = grid.width();
|
||||
let height = grid.height();
|
||||
|
||||
if width < 3 || height < 3 {
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
let mut sharp_transitions = 0usize;
|
||||
let mut total_wall_cells = 0usize;
|
||||
|
||||
for y in 1..(height - 1) {
|
||||
for x in 1..(width - 1) {
|
||||
let coord = crate::GridCoord::new(x as i32, y as i32);
|
||||
let cell = grid.get(coord);
|
||||
|
||||
if let Some(c) = cell
|
||||
&& c.cell_type == CellType::Wall
|
||||
{
|
||||
total_wall_cells += 1;
|
||||
|
||||
// Check 4-connected neighbors
|
||||
let neighbors = [
|
||||
crate::GridCoord::new(x as i32 - 1, y as i32),
|
||||
crate::GridCoord::new(x as i32 + 1, y as i32),
|
||||
crate::GridCoord::new(x as i32, y as i32 - 1),
|
||||
crate::GridCoord::new(x as i32, y as i32 + 1),
|
||||
];
|
||||
|
||||
let floor_neighbors = neighbors
|
||||
.iter()
|
||||
.filter_map(|&nc| grid.get(nc))
|
||||
.filter(|nc| nc.cell_type == CellType::Floor)
|
||||
.count();
|
||||
|
||||
// Wall cell with floor neighbor = sharp boundary
|
||||
if floor_neighbors > 0 && floor_neighbors < 4 {
|
||||
sharp_transitions += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if total_wall_cells == 0 {
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
// Ratio of wall cells that form sharp boundaries
|
||||
sharp_transitions as f32 / total_wall_cells as f32
|
||||
}
|
||||
|
||||
/// Compute average wall thickness.
|
||||
fn compute_avg_wall_thickness(grid: &GridStorage) -> f32 {
|
||||
use crate::CellType;
|
||||
|
||||
let width = grid.width();
|
||||
let height = grid.height();
|
||||
|
||||
let mut thicknesses = Vec::new();
|
||||
|
||||
// Scan horizontally
|
||||
for y in 0..height {
|
||||
let mut wall_run = 0;
|
||||
for x in 0..width {
|
||||
let coord = crate::GridCoord::new(x as i32, y as i32);
|
||||
let is_wall = grid
|
||||
.get(coord)
|
||||
.map(|c| c.cell_type == CellType::Wall)
|
||||
.unwrap_or(false);
|
||||
|
||||
if is_wall {
|
||||
wall_run += 1;
|
||||
} else if wall_run > 0 {
|
||||
thicknesses.push(wall_run as f32);
|
||||
wall_run = 0;
|
||||
}
|
||||
}
|
||||
if wall_run > 0 {
|
||||
thicknesses.push(wall_run as f32);
|
||||
}
|
||||
}
|
||||
|
||||
// Scan vertically
|
||||
for x in 0..width {
|
||||
let mut wall_run = 0;
|
||||
for y in 0..height {
|
||||
let coord = crate::GridCoord::new(x as i32, y as i32);
|
||||
let is_wall = grid
|
||||
.get(coord)
|
||||
.map(|c| c.cell_type == CellType::Wall)
|
||||
.unwrap_or(false);
|
||||
|
||||
if is_wall {
|
||||
wall_run += 1;
|
||||
} else if wall_run > 0 {
|
||||
thicknesses.push(wall_run as f32);
|
||||
wall_run = 0;
|
||||
}
|
||||
}
|
||||
if wall_run > 0 {
|
||||
thicknesses.push(wall_run as f32);
|
||||
}
|
||||
}
|
||||
|
||||
if thicknesses.is_empty() {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
thicknesses.iter().sum::<f32>() / thicknesses.len() as f32
|
||||
}
|
||||
|
||||
/// Print metrics.
|
||||
pub fn print(&self) {
|
||||
println!("=== Map Quality Metrics ===");
|
||||
println!("Coverage: {:.2} m²", self.coverage_m2);
|
||||
println!("Known cells: {:.1}%", self.known_percentage);
|
||||
println!("Wall sharpness: {:.2}", self.wall_sharpness);
|
||||
println!("Avg wall thickness: {:.2} cells", self.avg_wall_thickness);
|
||||
println!();
|
||||
println!("Cell distribution:");
|
||||
println!(" Floor: {}", self.cell_counts.floor);
|
||||
println!(" Wall: {}", self.cell_counts.wall);
|
||||
println!(" Cliff: {}", self.cell_counts.cliff);
|
||||
println!(" Bump: {}", self.cell_counts.bump);
|
||||
println!(" Unknown: {}", self.cell_counts.unknown);
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_pr_perfect() {
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![(0, 10), (5, 15)]);
|
||||
let detections = vec![(0, 10, 0.9), (5, 15, 0.8)];
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
assert_eq!(pr.true_positives, 2);
|
||||
assert_eq!(pr.false_positives, 0);
|
||||
assert_eq!(pr.false_negatives, 0);
|
||||
assert!((pr.precision - 1.0).abs() < 1e-5);
|
||||
assert!((pr.recall - 1.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_pr_with_fp() {
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![(0, 10)]);
|
||||
let detections = vec![(0, 10, 0.9), (1, 20, 0.5)]; // Second is FP
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
assert_eq!(pr.true_positives, 1);
|
||||
assert_eq!(pr.false_positives, 1);
|
||||
assert!((pr.precision - 0.5).abs() < 1e-5);
|
||||
assert!((pr.recall - 1.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_pr_with_fn() {
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![(0, 10), (5, 15)]);
|
||||
let detections = vec![(0, 10, 0.9)]; // Missing second closure
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
assert_eq!(pr.true_positives, 1);
|
||||
assert_eq!(pr.false_negatives, 1);
|
||||
assert!((pr.precision - 1.0).abs() < 1e-5);
|
||||
assert!((pr.recall - 0.5).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_ground_truth_from_trajectory() {
|
||||
let poses = vec![
|
||||
Pose2D::new(0.0, 0.0, 0.0),
|
||||
Pose2D::new(1.0, 0.0, 0.0),
|
||||
Pose2D::new(2.0, 0.0, 0.0),
|
||||
Pose2D::new(2.0, 1.0, 0.0),
|
||||
Pose2D::new(1.0, 1.0, 0.0),
|
||||
Pose2D::new(0.0, 1.0, 0.0),
|
||||
Pose2D::new(0.0, 0.5, 0.0), // Close to pose[0]
|
||||
];
|
||||
|
||||
let gt = LoopClosureGroundTruth::from_trajectory(&poses, 3, 1.0);
|
||||
|
||||
// Should detect closure between pose[0] and pose[6]
|
||||
assert!(!gt.closures.is_empty());
|
||||
assert!(gt.closures.iter().any(|&(a, b)| a == 0 && b == 6));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_empty_metrics() {
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![]);
|
||||
let detections: Vec<(usize, usize, f32)> = vec![];
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
assert!((pr.precision - 1.0).abs() < 1e-5);
|
||||
assert!((pr.recall - 1.0).abs() < 1e-5);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,367 @@
|
||||
//! Pose relations metrics for Cartographer-style evaluation.
|
||||
//!
|
||||
//! This module provides the core accuracy evaluation following Google Cartographer's
|
||||
//! `compute_relations_metrics` approach.
|
||||
//!
|
||||
//! ## Concept
|
||||
//!
|
||||
//! For each pose relation in the ground truth, we:
|
||||
//! 1. Look up the corresponding poses in the test trajectory
|
||||
//! 2. Compute the relative pose from the test trajectory
|
||||
//! 3. Compare against the ground truth relative pose
|
||||
//! 4. Aggregate errors into statistics
|
||||
//!
|
||||
//! This approach evaluates trajectory **consistency** rather than absolute accuracy,
|
||||
//! which works without external ground truth.
|
||||
|
||||
use super::ground_truth::{GroundTruthRelations, PoseRelation};
|
||||
use crate::Pose2D;
|
||||
use crate::core::normalize_angle;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Error for a single pose relation.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct RelationError {
|
||||
/// Translational error squared (meters²).
|
||||
pub translational_squared: f32,
|
||||
|
||||
/// Rotational error squared (radians²).
|
||||
pub rotational_squared: f32,
|
||||
|
||||
/// Covered distance for this relation (for weighting).
|
||||
pub covered_distance: f32,
|
||||
}
|
||||
|
||||
impl RelationError {
|
||||
/// Compute the error between a ground truth relation and test trajectory.
|
||||
pub fn compute(ground_truth: &PoseRelation, test_trajectory: &[Pose2D]) -> Option<Self> {
|
||||
let from_idx = ground_truth.from_node;
|
||||
let to_idx = ground_truth.to_node;
|
||||
|
||||
if from_idx >= test_trajectory.len() || to_idx >= test_trajectory.len() {
|
||||
return None;
|
||||
}
|
||||
|
||||
let test_from = &test_trajectory[from_idx];
|
||||
let test_to = &test_trajectory[to_idx];
|
||||
|
||||
// Compute relative pose in test trajectory
|
||||
let test_relative = test_from.inverse().compose(test_to);
|
||||
|
||||
// Compare with ground truth relative pose
|
||||
let trans_error_x = test_relative.x - ground_truth.relative_pose.x;
|
||||
let trans_error_y = test_relative.y - ground_truth.relative_pose.y;
|
||||
let rot_error = normalize_angle(test_relative.theta - ground_truth.relative_pose.theta);
|
||||
|
||||
Some(Self {
|
||||
translational_squared: trans_error_x * trans_error_x + trans_error_y * trans_error_y,
|
||||
rotational_squared: rot_error * rot_error,
|
||||
covered_distance: ground_truth.covered_distance,
|
||||
})
|
||||
}
|
||||
|
||||
/// Get translational error in meters.
|
||||
pub fn translational(&self) -> f32 {
|
||||
self.translational_squared.sqrt()
|
||||
}
|
||||
|
||||
/// Get rotational error in radians.
|
||||
pub fn rotational(&self) -> f32 {
|
||||
self.rotational_squared.sqrt()
|
||||
}
|
||||
}
|
||||
|
||||
/// Aggregated metrics from pose relations (Cartographer output format).
|
||||
///
|
||||
/// This struct contains the same metrics that Cartographer's
|
||||
/// `compute_relations_metrics` outputs.
|
||||
#[derive(Clone, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct RelationsMetrics {
|
||||
// === Translational error ===
|
||||
/// Mean absolute translational error (meters).
|
||||
pub abs_trans_error: f32,
|
||||
|
||||
/// Standard deviation of translational error (meters).
|
||||
pub abs_trans_std: f32,
|
||||
|
||||
/// Mean squared translational error (m²).
|
||||
pub sqr_trans_error: f32,
|
||||
|
||||
/// Standard deviation of squared translational error (m²).
|
||||
pub sqr_trans_std: f32,
|
||||
|
||||
// === Rotational error ===
|
||||
/// Mean absolute rotational error (degrees).
|
||||
pub abs_rot_error: f32,
|
||||
|
||||
/// Standard deviation of rotational error (degrees).
|
||||
pub abs_rot_std: f32,
|
||||
|
||||
/// Mean squared rotational error (deg²).
|
||||
pub sqr_rot_error: f32,
|
||||
|
||||
/// Standard deviation of squared rotational error (deg²).
|
||||
pub sqr_rot_std: f32,
|
||||
|
||||
// === Summary ===
|
||||
/// Number of relations evaluated.
|
||||
pub num_relations: usize,
|
||||
|
||||
/// Number of outliers (errors exceeding threshold).
|
||||
pub num_outliers: usize,
|
||||
|
||||
/// Whether the metrics pass the target threshold.
|
||||
pub passed_threshold: bool,
|
||||
|
||||
/// Threshold used for pass/fail (meters).
|
||||
pub threshold_meters: f32,
|
||||
}
|
||||
|
||||
impl RelationsMetrics {
|
||||
/// Compute metrics by comparing test trajectory to ground truth relations.
|
||||
///
|
||||
/// This is equivalent to Cartographer's `compute_relations_metrics`.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `test_trajectory` - The trajectory to evaluate
|
||||
/// * `ground_truth` - Ground truth relations from an optimized trajectory
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Aggregated accuracy metrics.
|
||||
pub fn compute(test_trajectory: &[Pose2D], ground_truth: &GroundTruthRelations) -> Self {
|
||||
Self::compute_with_threshold(test_trajectory, ground_truth, 0.01) // 1cm default
|
||||
}
|
||||
|
||||
/// Compute metrics with a custom pass/fail threshold.
|
||||
pub fn compute_with_threshold(
|
||||
test_trajectory: &[Pose2D],
|
||||
ground_truth: &GroundTruthRelations,
|
||||
threshold_meters: f32,
|
||||
) -> Self {
|
||||
if ground_truth.is_empty() {
|
||||
return Self {
|
||||
threshold_meters,
|
||||
passed_threshold: true,
|
||||
..Default::default()
|
||||
};
|
||||
}
|
||||
|
||||
// Compute errors for all relations
|
||||
let errors: Vec<RelationError> = ground_truth
|
||||
.relations
|
||||
.iter()
|
||||
.filter_map(|rel| RelationError::compute(rel, test_trajectory))
|
||||
.collect();
|
||||
|
||||
if errors.is_empty() {
|
||||
return Self {
|
||||
threshold_meters,
|
||||
passed_threshold: true,
|
||||
..Default::default()
|
||||
};
|
||||
}
|
||||
|
||||
let n = errors.len() as f32;
|
||||
|
||||
// Compute translational statistics
|
||||
let trans_errors: Vec<f32> = errors.iter().map(|e| e.translational()).collect();
|
||||
let sqr_trans: Vec<f32> = errors.iter().map(|e| e.translational_squared).collect();
|
||||
|
||||
let abs_trans_error = trans_errors.iter().sum::<f32>() / n;
|
||||
let sqr_trans_error = sqr_trans.iter().sum::<f32>() / n;
|
||||
|
||||
let abs_trans_std = std_dev(&trans_errors, abs_trans_error);
|
||||
let sqr_trans_std = std_dev(&sqr_trans, sqr_trans_error);
|
||||
|
||||
// Compute rotational statistics (convert to degrees)
|
||||
let rot_errors: Vec<f32> = errors.iter().map(|e| e.rotational().to_degrees()).collect();
|
||||
let sqr_rot: Vec<f32> = errors
|
||||
.iter()
|
||||
.map(|e| e.rotational_squared.to_degrees().powi(2))
|
||||
.collect();
|
||||
|
||||
let abs_rot_error = rot_errors.iter().sum::<f32>() / n;
|
||||
let sqr_rot_error = sqr_rot.iter().sum::<f32>() / n;
|
||||
|
||||
let abs_rot_std = std_dev(&rot_errors, abs_rot_error);
|
||||
let sqr_rot_std = std_dev(&sqr_rot, sqr_rot_error);
|
||||
|
||||
// Count outliers
|
||||
let num_outliers = trans_errors
|
||||
.iter()
|
||||
.filter(|&&e| e > ground_truth.config.outlier_threshold_meters)
|
||||
.count();
|
||||
|
||||
Self {
|
||||
abs_trans_error,
|
||||
abs_trans_std,
|
||||
sqr_trans_error,
|
||||
sqr_trans_std,
|
||||
abs_rot_error,
|
||||
abs_rot_std,
|
||||
sqr_rot_error,
|
||||
sqr_rot_std,
|
||||
num_relations: errors.len(),
|
||||
num_outliers,
|
||||
passed_threshold: abs_trans_error < threshold_meters,
|
||||
threshold_meters,
|
||||
}
|
||||
}
|
||||
|
||||
/// Pretty print metrics in Cartographer format.
|
||||
pub fn print(&self) {
|
||||
println!("=== Pose Relations Metrics ===");
|
||||
println!(
|
||||
"Abs translational error {:.5} +/- {:.5} m",
|
||||
self.abs_trans_error, self.abs_trans_std
|
||||
);
|
||||
println!(
|
||||
"Sqr translational error {:.5} +/- {:.5} m²",
|
||||
self.sqr_trans_error, self.sqr_trans_std
|
||||
);
|
||||
println!(
|
||||
"Abs rotational error {:.5} +/- {:.5} deg",
|
||||
self.abs_rot_error, self.abs_rot_std
|
||||
);
|
||||
println!(
|
||||
"Sqr rotational error {:.5} +/- {:.5} deg²",
|
||||
self.sqr_rot_error, self.sqr_rot_std
|
||||
);
|
||||
println!("Relations evaluated: {}", self.num_relations);
|
||||
println!("Outliers: {}", self.num_outliers);
|
||||
println!(
|
||||
"Status: {} (threshold: {}m)",
|
||||
if self.passed_threshold {
|
||||
"PASSED"
|
||||
} else {
|
||||
"FAILED"
|
||||
},
|
||||
self.threshold_meters
|
||||
);
|
||||
}
|
||||
|
||||
/// Format as a single-line summary.
|
||||
pub fn summary(&self) -> String {
|
||||
format!(
|
||||
"trans: {:.4}±{:.4}m, rot: {:.2}±{:.2}°, {} relations, {}",
|
||||
self.abs_trans_error,
|
||||
self.abs_trans_std,
|
||||
self.abs_rot_error,
|
||||
self.abs_rot_std,
|
||||
self.num_relations,
|
||||
if self.passed_threshold {
|
||||
"PASS"
|
||||
} else {
|
||||
"FAIL"
|
||||
}
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute standard deviation given values and their mean.
|
||||
fn std_dev(values: &[f32], mean: f32) -> f32 {
|
||||
if values.len() < 2 {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
let variance =
|
||||
values.iter().map(|v| (v - mean).powi(2)).sum::<f32>() / (values.len() - 1) as f32;
|
||||
variance.sqrt()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::evaluation::ground_truth::GroundTruthConfig;
|
||||
|
||||
fn create_straight_trajectory(n: usize, spacing: f32) -> Vec<Pose2D> {
|
||||
(0..n)
|
||||
.map(|i| Pose2D::new(i as f32 * spacing, 0.0, 0.0))
|
||||
.collect()
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_perfect_trajectory() {
|
||||
let trajectory = create_straight_trajectory(10, 0.5);
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
..Default::default()
|
||||
};
|
||||
let ground_truth = GroundTruthRelations::generate(&trajectory, &[], config);
|
||||
|
||||
// Same trajectory should have zero error
|
||||
let metrics = RelationsMetrics::compute(&trajectory, &ground_truth);
|
||||
|
||||
assert!(metrics.abs_trans_error < 1e-5);
|
||||
assert!(metrics.abs_rot_error < 1e-5);
|
||||
assert!(metrics.passed_threshold);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_trajectory_with_drift() {
|
||||
let ground_trajectory = create_straight_trajectory(10, 0.5);
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
..Default::default()
|
||||
};
|
||||
let ground_truth = GroundTruthRelations::generate(&ground_trajectory, &[], config);
|
||||
|
||||
// Test trajectory with small drift
|
||||
let test_trajectory: Vec<Pose2D> = ground_trajectory
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(i, p)| Pose2D::new(p.x + 0.001 * i as f32, p.y, p.theta))
|
||||
.collect();
|
||||
|
||||
let metrics = RelationsMetrics::compute(&test_trajectory, &ground_truth);
|
||||
|
||||
// Should have small but non-zero error
|
||||
assert!(metrics.abs_trans_error > 0.0);
|
||||
assert!(metrics.abs_trans_error < 0.01); // Less than 1cm
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_relation_error_computation() {
|
||||
let from_pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let to_pose = Pose2D::new(1.0, 0.0, 0.0);
|
||||
|
||||
let relation = PoseRelation::from_poses(0, 1, &from_pose, &to_pose, 1.0);
|
||||
|
||||
// Perfect match
|
||||
let test_trajectory = vec![from_pose, to_pose];
|
||||
let error = RelationError::compute(&relation, &test_trajectory).unwrap();
|
||||
|
||||
assert!(error.translational() < 1e-5);
|
||||
assert!(error.rotational() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_std_dev() {
|
||||
let values = vec![1.0, 2.0, 3.0, 4.0, 5.0];
|
||||
let mean = 3.0;
|
||||
let std = std_dev(&values, mean);
|
||||
|
||||
// Standard deviation should be sqrt(2.5) ≈ 1.58
|
||||
assert!((std - 1.5811).abs() < 0.01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_empty_ground_truth() {
|
||||
let trajectory = create_straight_trajectory(10, 0.5);
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 100.0, // Very high, so no relations
|
||||
..Default::default()
|
||||
};
|
||||
let ground_truth = GroundTruthRelations::generate(&trajectory, &[], config);
|
||||
|
||||
let metrics = RelationsMetrics::compute(&trajectory, &ground_truth);
|
||||
|
||||
assert_eq!(metrics.num_relations, 0);
|
||||
assert!(metrics.passed_threshold);
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,7 @@
|
||||
//! Types used by grid storage.
|
||||
|
||||
use crate::core::CellType;
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
/// Mutable reference to a single cell's fields in the SoA storage.
|
||||
///
|
||||
@@ -84,7 +85,7 @@ impl<'a> CellMut<'a> {
|
||||
}
|
||||
|
||||
/// Cell counts by type.
|
||||
#[derive(Clone, Copy, Debug, Default)]
|
||||
#[derive(Clone, Copy, Debug, Default, Serialize, Deserialize)]
|
||||
pub struct CellCounts {
|
||||
/// Unknown cells (not yet observed).
|
||||
pub unknown: usize,
|
||||
|
||||
@@ -67,6 +67,9 @@ pub mod modes;
|
||||
// Persistence (save/load)
|
||||
pub mod io;
|
||||
|
||||
// Evaluation framework (Cartographer-style)
|
||||
pub mod evaluation;
|
||||
|
||||
// Re-export commonly used types
|
||||
pub use core::{
|
||||
BumperSensors, Cell, CellType, CliffSensors, GridCoord, LidarScan, MotionModel, Odometry,
|
||||
|
||||
@@ -0,0 +1,273 @@
|
||||
//! Accuracy integration tests for VastuSLAM.
|
||||
//!
|
||||
//! These tests verify SLAM accuracy using the Cartographer-style pose relations metric.
|
||||
|
||||
mod common;
|
||||
|
||||
use vastu_slam::Pose2D;
|
||||
use vastu_slam::evaluation::{
|
||||
AbsoluteTrajectoryError, AccuracyMetrics, GroundTruthConfig, GroundTruthRelations,
|
||||
RelationsMetrics, RelativePoseError,
|
||||
};
|
||||
|
||||
/// Target accuracy: < 1cm translational error.
|
||||
const TARGET_TRANS_ERROR_M: f32 = 0.01;
|
||||
|
||||
/// Target accuracy: < 0.5 degrees rotational error.
|
||||
const TARGET_ROT_ERROR_DEG: f32 = 0.5;
|
||||
|
||||
// ============================================================================
|
||||
// Pose Relations Metric Tests (Cartographer-style)
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_relations_metric_perfect_trajectory() {
|
||||
// Ground truth: straight trajectory
|
||||
let trajectory = common::straight_trajectory(20, 0.5);
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
node_sampling_interval: 1,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let ground_truth = GroundTruthRelations::generate(&trajectory, &[], config);
|
||||
|
||||
// Test same trajectory - should have zero error
|
||||
let metrics = RelationsMetrics::compute(&trajectory, &ground_truth);
|
||||
|
||||
assert!(
|
||||
metrics.abs_trans_error < 1e-5,
|
||||
"Perfect trajectory should have zero translational error, got: {}",
|
||||
metrics.abs_trans_error
|
||||
);
|
||||
assert!(
|
||||
metrics.abs_rot_error < 1e-5,
|
||||
"Perfect trajectory should have zero rotational error, got: {}",
|
||||
metrics.abs_rot_error
|
||||
);
|
||||
assert!(metrics.passed_threshold);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_relations_metric_with_noise() {
|
||||
let ground_trajectory = common::straight_trajectory(50, 0.2);
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let ground_truth = GroundTruthRelations::generate(&ground_trajectory, &[], config);
|
||||
|
||||
// Add small noise (< 1mm)
|
||||
let noisy_trajectory = common::add_noise(&ground_trajectory, 0.0005, 0.001, 42);
|
||||
let metrics = RelationsMetrics::compute(&noisy_trajectory, &ground_truth);
|
||||
|
||||
println!("Noisy trajectory metrics: {}", metrics.summary());
|
||||
|
||||
// Should still be under target with small noise
|
||||
assert!(
|
||||
metrics.abs_trans_error < TARGET_TRANS_ERROR_M,
|
||||
"Trans error {} should be < {}m",
|
||||
metrics.abs_trans_error,
|
||||
TARGET_TRANS_ERROR_M
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_relations_metric_with_drift() {
|
||||
let ground_trajectory = common::straight_trajectory(50, 0.2);
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let ground_truth = GroundTruthRelations::generate(&ground_trajectory, &[], config);
|
||||
|
||||
// Add drift (1mm per meter traveled)
|
||||
let drifted_trajectory = common::add_drift(&ground_trajectory, 0.001);
|
||||
let metrics = RelationsMetrics::compute(&drifted_trajectory, &ground_truth);
|
||||
|
||||
println!("Drifted trajectory metrics: {}", metrics.summary());
|
||||
|
||||
// With 1mm/m drift over 10m, expect about 5mm average error
|
||||
assert!(
|
||||
metrics.abs_trans_error < 0.01,
|
||||
"Trans error {} with small drift should be < 1cm",
|
||||
metrics.abs_trans_error
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_relations_metric_square_loop() {
|
||||
// Square trajectory with loop closure
|
||||
let trajectory = common::square_trajectory(4.0, 10);
|
||||
|
||||
// Mark loop closure between first and last poses
|
||||
let loop_closures = vec![(0, trajectory.len() - 1)];
|
||||
|
||||
let config = GroundTruthConfig {
|
||||
min_covered_distance: 0.5,
|
||||
..Default::default()
|
||||
};
|
||||
|
||||
let ground_truth = GroundTruthRelations::generate(&trajectory, &loop_closures, config);
|
||||
|
||||
// Verify ground truth contains loop closure relation
|
||||
assert!(
|
||||
ground_truth
|
||||
.relations
|
||||
.iter()
|
||||
.any(|r| r.covered_distance > 10.0),
|
||||
"Should have at least one long-distance relation from loop closure"
|
||||
);
|
||||
|
||||
// Test with perfect trajectory
|
||||
let metrics = RelationsMetrics::compute(&trajectory, &ground_truth);
|
||||
assert!(metrics.passed_threshold);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ATE/RPE Tests (evo-compatible)
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_ate_perfect_trajectory() {
|
||||
let trajectory = common::straight_trajectory(20, 0.5);
|
||||
|
||||
let ate = AbsoluteTrajectoryError::compute(&trajectory, &trajectory);
|
||||
|
||||
assert!(
|
||||
ate.translation.rmse < 1e-5,
|
||||
"Perfect trajectory should have zero ATE"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_ate_with_constant_offset() {
|
||||
let ground_truth = common::straight_trajectory(20, 0.5);
|
||||
|
||||
// Add constant offset
|
||||
let estimated: Vec<Pose2D> = ground_truth
|
||||
.iter()
|
||||
.map(|p| Pose2D::new(p.x + 0.5, p.y + 0.3, p.theta))
|
||||
.collect();
|
||||
|
||||
// Without alignment, should have significant error
|
||||
let ate_no_align =
|
||||
AbsoluteTrajectoryError::compute_with_options(&estimated, &ground_truth, false);
|
||||
assert!(
|
||||
ate_no_align.translation.rmse > 0.5,
|
||||
"Without alignment, should have large error"
|
||||
);
|
||||
|
||||
// With alignment, should be near zero
|
||||
let ate_aligned = AbsoluteTrajectoryError::compute(&estimated, &ground_truth);
|
||||
assert!(
|
||||
ate_aligned.translation.rmse < 0.01,
|
||||
"With SE(2) alignment, constant offset should be corrected"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_rpe_with_local_errors() {
|
||||
let ground_truth = common::straight_trajectory(50, 0.2);
|
||||
|
||||
// Add noise to simulate local errors
|
||||
let estimated = common::add_noise(&ground_truth, 0.002, 0.005, 123);
|
||||
|
||||
let rpe = RelativePoseError::compute(&estimated, &ground_truth, 1.0);
|
||||
|
||||
println!(
|
||||
"RPE with noise: trans={:.4}m, rot={:.4}rad, pairs={}",
|
||||
rpe.translation.rmse, rpe.rotation.rmse, rpe.pairs_evaluated
|
||||
);
|
||||
|
||||
// RPE should detect local errors
|
||||
assert!(rpe.pairs_evaluated > 0, "Should evaluate some pairs");
|
||||
assert!(
|
||||
rpe.translation.rmse < 0.01,
|
||||
"With 2mm noise, RPE should be small"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_accuracy_metrics_all() {
|
||||
let ground_truth = common::figure_eight_trajectory(2.0, 40);
|
||||
let estimated = common::add_noise(&ground_truth, 0.001, 0.002, 456);
|
||||
|
||||
let metrics = AccuracyMetrics::compute(&estimated, &ground_truth, 1.0);
|
||||
|
||||
println!("=== Full Accuracy Report ===");
|
||||
metrics.print();
|
||||
|
||||
assert!(
|
||||
metrics.ate.translation.rmse < 0.02,
|
||||
"ATE should be small with minor noise"
|
||||
);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Regression Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_accuracy_regression_straight_line() {
|
||||
// This test ensures accuracy doesn't regress over time
|
||||
let ground_truth = common::straight_trajectory(100, 0.1);
|
||||
|
||||
let config = GroundTruthConfig::default();
|
||||
let gt_relations = GroundTruthRelations::generate(&ground_truth, &[], config);
|
||||
|
||||
// Simulate a reasonable SLAM output (small noise + small drift)
|
||||
let estimated = common::add_drift(
|
||||
&common::add_noise(&ground_truth, 0.001, 0.001, 789),
|
||||
0.0005, // 0.5mm/m drift
|
||||
);
|
||||
|
||||
let metrics =
|
||||
RelationsMetrics::compute_with_threshold(&estimated, >_relations, TARGET_TRANS_ERROR_M);
|
||||
|
||||
println!(
|
||||
"Regression test - Trans error: {:.4}m (target: <{}m)",
|
||||
metrics.abs_trans_error, TARGET_TRANS_ERROR_M
|
||||
);
|
||||
println!(
|
||||
"Regression test - Rot error: {:.4}deg (target: <{}deg)",
|
||||
metrics.abs_rot_error, TARGET_ROT_ERROR_DEG
|
||||
);
|
||||
|
||||
assert!(
|
||||
metrics.passed_threshold,
|
||||
"REGRESSION: Trans error {} exceeds target {}m",
|
||||
metrics.abs_trans_error, TARGET_TRANS_ERROR_M
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_accuracy_regression_square_loop() {
|
||||
let ground_truth = common::square_trajectory(5.0, 20);
|
||||
let loop_closures = vec![(0, ground_truth.len() - 1)];
|
||||
|
||||
let config = GroundTruthConfig::default();
|
||||
let gt_relations = GroundTruthRelations::generate(&ground_truth, &loop_closures, config);
|
||||
|
||||
// Simulate SLAM with small errors (1mm position noise)
|
||||
let estimated = common::add_noise(&ground_truth, 0.001, 0.002, 101);
|
||||
|
||||
let metrics =
|
||||
RelationsMetrics::compute_with_threshold(&estimated, >_relations, TARGET_TRANS_ERROR_M);
|
||||
|
||||
println!(
|
||||
"Square loop - Trans error: {:.4}m, Rot error: {:.4}deg",
|
||||
metrics.abs_trans_error, metrics.abs_rot_error
|
||||
);
|
||||
|
||||
assert!(
|
||||
metrics.passed_threshold,
|
||||
"REGRESSION: Square loop trans error {} exceeds target",
|
||||
metrics.abs_trans_error
|
||||
);
|
||||
}
|
||||
@@ -0,0 +1,250 @@
|
||||
//! Test utilities for VastuSLAM evaluation.
|
||||
//!
|
||||
//! This module provides helpers for creating test trajectories, maps, and scenarios.
|
||||
|
||||
#![allow(dead_code)]
|
||||
|
||||
use std::f32::consts::FRAC_PI_2;
|
||||
use vastu_slam::{LidarScan, Pose2D};
|
||||
|
||||
/// Create a straight-line trajectory.
|
||||
pub fn straight_trajectory(n: usize, spacing: f32) -> Vec<Pose2D> {
|
||||
(0..n)
|
||||
.map(|i| Pose2D::new(i as f32 * spacing, 0.0, 0.0))
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Create a square loop trajectory.
|
||||
pub fn square_trajectory(side_length: f32, points_per_side: usize) -> Vec<Pose2D> {
|
||||
let mut poses = Vec::new();
|
||||
let spacing = side_length / points_per_side as f32;
|
||||
|
||||
// Bottom side (left to right)
|
||||
for i in 0..points_per_side {
|
||||
poses.push(Pose2D::new(i as f32 * spacing, 0.0, 0.0));
|
||||
}
|
||||
|
||||
// Right side (bottom to top)
|
||||
for i in 0..points_per_side {
|
||||
poses.push(Pose2D::new(side_length, i as f32 * spacing, FRAC_PI_2));
|
||||
}
|
||||
|
||||
// Top side (right to left)
|
||||
for i in 0..points_per_side {
|
||||
poses.push(Pose2D::new(
|
||||
side_length - i as f32 * spacing,
|
||||
side_length,
|
||||
std::f32::consts::PI,
|
||||
));
|
||||
}
|
||||
|
||||
// Left side (top to bottom, back to start)
|
||||
for i in 0..points_per_side {
|
||||
poses.push(Pose2D::new(
|
||||
0.0,
|
||||
side_length - i as f32 * spacing,
|
||||
-FRAC_PI_2,
|
||||
));
|
||||
}
|
||||
|
||||
poses
|
||||
}
|
||||
|
||||
/// Create a figure-8 trajectory.
|
||||
pub fn figure_eight_trajectory(radius: f32, points: usize) -> Vec<Pose2D> {
|
||||
let mut poses = Vec::new();
|
||||
let half = points / 2;
|
||||
|
||||
// First circle (counter-clockwise)
|
||||
for i in 0..half {
|
||||
let angle = 2.0 * std::f32::consts::PI * i as f32 / half as f32;
|
||||
let x = radius * (1.0 - angle.cos());
|
||||
let y = radius * angle.sin();
|
||||
let theta = angle + FRAC_PI_2;
|
||||
poses.push(Pose2D::new(x, y, theta));
|
||||
}
|
||||
|
||||
// Second circle (clockwise)
|
||||
for i in 0..half {
|
||||
let angle = 2.0 * std::f32::consts::PI * i as f32 / half as f32;
|
||||
let x = 2.0 * radius + radius * (angle.cos() - 1.0);
|
||||
let y = -radius * angle.sin();
|
||||
let theta = -angle - FRAC_PI_2;
|
||||
poses.push(Pose2D::new(x, y, theta));
|
||||
}
|
||||
|
||||
poses
|
||||
}
|
||||
|
||||
/// Add Gaussian noise to a trajectory.
|
||||
pub fn add_noise(trajectory: &[Pose2D], trans_std: f32, rot_std: f32, seed: u64) -> Vec<Pose2D> {
|
||||
use std::num::Wrapping;
|
||||
|
||||
// Simple LCG PRNG for reproducibility
|
||||
let mut state = Wrapping(seed);
|
||||
let a = Wrapping(1664525u64);
|
||||
let c = Wrapping(1013904223u64);
|
||||
|
||||
let mut random = || -> f32 {
|
||||
state = a * state + c;
|
||||
// Box-Muller transform for Gaussian
|
||||
let u1 = (state.0 & 0xFFFF) as f32 / 65536.0 + 0.0001;
|
||||
state = a * state + c;
|
||||
let u2 = (state.0 & 0xFFFF) as f32 / 65536.0;
|
||||
(-2.0 * u1.ln()).sqrt() * (2.0 * std::f32::consts::PI * u2).cos()
|
||||
};
|
||||
|
||||
trajectory
|
||||
.iter()
|
||||
.map(|p| {
|
||||
Pose2D::new(
|
||||
p.x + random() * trans_std,
|
||||
p.y + random() * trans_std,
|
||||
p.theta + random() * rot_std,
|
||||
)
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Add drift to a trajectory (cumulative error).
|
||||
pub fn add_drift(trajectory: &[Pose2D], drift_per_m: f32) -> Vec<Pose2D> {
|
||||
if trajectory.is_empty() {
|
||||
return Vec::new();
|
||||
}
|
||||
|
||||
let mut result = Vec::with_capacity(trajectory.len());
|
||||
result.push(trajectory[0]);
|
||||
|
||||
let mut cumulative_drift = 0.0f32;
|
||||
|
||||
for i in 1..trajectory.len() {
|
||||
let dist = trajectory[i].distance(&trajectory[i - 1]);
|
||||
cumulative_drift += dist * drift_per_m;
|
||||
|
||||
let p = &trajectory[i];
|
||||
result.push(Pose2D::new(p.x + cumulative_drift, p.y, p.theta));
|
||||
}
|
||||
|
||||
result
|
||||
}
|
||||
|
||||
/// Create a simple lidar scan for testing.
|
||||
pub fn simple_scan(num_points: usize, max_range: f32) -> LidarScan {
|
||||
let angle_increment = 2.0 * std::f32::consts::PI / num_points as f32;
|
||||
let angles: Vec<f32> = (0..num_points)
|
||||
.map(|i| i as f32 * angle_increment - std::f32::consts::PI)
|
||||
.collect();
|
||||
|
||||
// Simple pattern: range varies with angle
|
||||
let ranges: Vec<f32> = angles
|
||||
.iter()
|
||||
.map(|a| max_range * 0.5 * (1.0 + a.cos().abs()))
|
||||
.collect();
|
||||
|
||||
LidarScan::new(ranges, angles, 0.15, max_range)
|
||||
}
|
||||
|
||||
/// Create a scan that hits walls in a square room.
|
||||
pub fn room_scan(
|
||||
room_width: f32,
|
||||
room_height: f32,
|
||||
robot_x: f32,
|
||||
robot_y: f32,
|
||||
num_points: usize,
|
||||
) -> LidarScan {
|
||||
let angle_increment = 2.0 * std::f32::consts::PI / num_points as f32;
|
||||
let max_range = (room_width * room_width + room_height * room_height).sqrt();
|
||||
|
||||
let mut ranges = Vec::with_capacity(num_points);
|
||||
let mut angles = Vec::with_capacity(num_points);
|
||||
|
||||
for i in 0..num_points {
|
||||
let angle = i as f32 * angle_increment - std::f32::consts::PI;
|
||||
angles.push(angle);
|
||||
|
||||
// Ray cast to find wall intersection
|
||||
let cos_a = angle.cos();
|
||||
let sin_a = angle.sin();
|
||||
|
||||
let mut range = max_range;
|
||||
|
||||
// Check intersection with each wall
|
||||
// Right wall (x = room_width)
|
||||
if cos_a > 0.0 {
|
||||
let t = (room_width - robot_x) / cos_a;
|
||||
if t > 0.0 && t < range {
|
||||
let y = robot_y + t * sin_a;
|
||||
if y >= 0.0 && y <= room_height {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Left wall (x = 0)
|
||||
if cos_a < 0.0 {
|
||||
let t = -robot_x / cos_a;
|
||||
if t > 0.0 && t < range {
|
||||
let y = robot_y + t * sin_a;
|
||||
if y >= 0.0 && y <= room_height {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Top wall (y = room_height)
|
||||
if sin_a > 0.0 {
|
||||
let t = (room_height - robot_y) / sin_a;
|
||||
if t > 0.0 && t < range {
|
||||
let x = robot_x + t * cos_a;
|
||||
if x >= 0.0 && x <= room_width {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Bottom wall (y = 0)
|
||||
if sin_a < 0.0 {
|
||||
let t = -robot_y / sin_a;
|
||||
if t > 0.0 && t < range {
|
||||
let x = robot_x + t * cos_a;
|
||||
if x >= 0.0 && x <= room_width {
|
||||
range = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ranges.push(range.min(max_range));
|
||||
}
|
||||
|
||||
LidarScan::new(ranges, angles, 0.15, max_range)
|
||||
}
|
||||
|
||||
/// Assert two poses are approximately equal.
|
||||
pub fn assert_poses_close(a: &Pose2D, b: &Pose2D, trans_tol: f32, rot_tol: f32) {
|
||||
let trans_error = a.distance(b);
|
||||
let rot_error = (a.theta - b.theta).abs();
|
||||
|
||||
assert!(
|
||||
trans_error < trans_tol,
|
||||
"Translation error {} exceeds tolerance {}",
|
||||
trans_error,
|
||||
trans_tol
|
||||
);
|
||||
assert!(
|
||||
rot_error < rot_tol,
|
||||
"Rotation error {} exceeds tolerance {}",
|
||||
rot_error,
|
||||
rot_tol
|
||||
);
|
||||
}
|
||||
|
||||
/// Assert trajectory ATE is under threshold.
|
||||
pub fn assert_ate_under(estimated: &[Pose2D], ground_truth: &[Pose2D], threshold: f32) {
|
||||
use vastu_slam::evaluation::AbsoluteTrajectoryError;
|
||||
|
||||
let ate = AbsoluteTrajectoryError::compute(estimated, ground_truth);
|
||||
|
||||
assert!(
|
||||
ate.translation.rmse < threshold,
|
||||
"ATE RMSE {} exceeds threshold {}",
|
||||
ate.translation.rmse,
|
||||
threshold
|
||||
);
|
||||
}
|
||||
@@ -0,0 +1,280 @@
|
||||
//! Performance integration tests for VastuSLAM.
|
||||
//!
|
||||
//! These tests verify that SLAM operations meet performance targets.
|
||||
|
||||
mod common;
|
||||
|
||||
use std::time::{Duration, Instant};
|
||||
use vastu_slam::evaluation::{OperationTiming, PerformanceTracker};
|
||||
use vastu_slam::{
|
||||
CorrelativeMatcher, CorrelativeMatcherConfig, MapConfig, OccupancyGridMap, Pose2D,
|
||||
};
|
||||
|
||||
/// Performance thresholds (in milliseconds) for release builds.
|
||||
/// Debug builds are expected to be ~5-10x slower.
|
||||
const SCAN_MATCHING_THRESHOLD_MS: f32 = 15.0;
|
||||
const MAP_UPDATE_THRESHOLD_MS: f32 = 2.0;
|
||||
|
||||
/// Multiplier for debug mode thresholds.
|
||||
#[cfg(debug_assertions)]
|
||||
const THRESHOLD_MULTIPLIER: f32 = 10.0;
|
||||
#[cfg(not(debug_assertions))]
|
||||
const THRESHOLD_MULTIPLIER: f32 = 1.0;
|
||||
|
||||
// ============================================================================
|
||||
// Operation Timing Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_scan_matching_performance() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Initialize map with some data
|
||||
for _ in 0..10 {
|
||||
let pose = Pose2D::new(0.0, 0.0, 0.0);
|
||||
let scan = common::room_scan(6.0, 6.0, 3.0, 3.0, 360);
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
let matcher_config = CorrelativeMatcherConfig::default();
|
||||
let matcher = CorrelativeMatcher::new(matcher_config);
|
||||
|
||||
let scan = common::room_scan(6.0, 6.0, 3.1, 3.0, 360);
|
||||
let initial_pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
// Warm-up
|
||||
for _ in 0..3 {
|
||||
let _ = matcher.match_scan(&scan, initial_pose, map.storage());
|
||||
}
|
||||
|
||||
// Measure
|
||||
let mut times = Vec::new();
|
||||
for _ in 0..20 {
|
||||
let start = Instant::now();
|
||||
let _ = matcher.match_scan(&scan, initial_pose, map.storage());
|
||||
times.push(start.elapsed());
|
||||
}
|
||||
|
||||
let timing = OperationTiming::from_durations("scan_matching", ×);
|
||||
|
||||
println!("Scan matching: {}", timing.summary());
|
||||
|
||||
let threshold = SCAN_MATCHING_THRESHOLD_MS * THRESHOLD_MULTIPLIER;
|
||||
assert!(
|
||||
timing.mean_ms < threshold,
|
||||
"Scan matching mean {:.2}ms exceeds threshold {}ms",
|
||||
timing.mean_ms,
|
||||
threshold
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_map_update_performance() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
let scan = common::room_scan(10.0, 10.0, 5.0, 5.0, 360);
|
||||
let pose = Pose2D::new(5.0, 5.0, 0.0);
|
||||
|
||||
// Warm-up
|
||||
for _ in 0..3 {
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
map.clear();
|
||||
|
||||
// Measure
|
||||
let mut times = Vec::new();
|
||||
for i in 0..50 {
|
||||
let p = Pose2D::new(5.0 + (i as f32 * 0.01), 5.0, 0.0);
|
||||
let start = Instant::now();
|
||||
map.observe_lidar(&scan, p);
|
||||
times.push(start.elapsed());
|
||||
}
|
||||
|
||||
let timing = OperationTiming::from_durations("map_update", ×);
|
||||
|
||||
println!("Map update: {}", timing.summary());
|
||||
|
||||
let threshold = MAP_UPDATE_THRESHOLD_MS * THRESHOLD_MULTIPLIER;
|
||||
assert!(
|
||||
timing.mean_ms < threshold,
|
||||
"Map update mean {:.2}ms exceeds threshold {}ms",
|
||||
timing.mean_ms,
|
||||
threshold
|
||||
);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Performance Tracker Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_performance_tracker_integration() {
|
||||
let mut tracker = PerformanceTracker::new();
|
||||
tracker.start();
|
||||
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
let matcher_config = CorrelativeMatcherConfig::default();
|
||||
let matcher = CorrelativeMatcher::new(matcher_config);
|
||||
|
||||
// Simulate a SLAM session
|
||||
for i in 0..20 {
|
||||
let pose = Pose2D::new(i as f32 * 0.2, 0.0, 0.0);
|
||||
let scan = common::room_scan(8.0, 8.0, 4.0 + i as f32 * 0.2, 4.0, 360);
|
||||
|
||||
// Time scan matching
|
||||
let match_start = Instant::now();
|
||||
let _ = matcher.match_scan(&scan, pose, map.storage());
|
||||
tracker.record("scan_matching", match_start.elapsed());
|
||||
|
||||
// Time map update
|
||||
let update_start = Instant::now();
|
||||
map.observe_lidar(&scan, pose);
|
||||
tracker.record("map_update", update_start.elapsed());
|
||||
|
||||
tracker.record_scan();
|
||||
}
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
|
||||
println!("\n=== Performance Report ===");
|
||||
metrics.print();
|
||||
|
||||
// Verify we have timing data
|
||||
assert!(
|
||||
metrics.operations.contains_key("scan_matching"),
|
||||
"Should have scan matching timing"
|
||||
);
|
||||
assert!(
|
||||
metrics.operations.contains_key("map_update"),
|
||||
"Should have map update timing"
|
||||
);
|
||||
assert_eq!(metrics.num_scans, 20);
|
||||
assert!(metrics.scans_per_second > 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_threshold_violations() {
|
||||
let mut tracker = PerformanceTracker::new();
|
||||
|
||||
// Simulate some operations
|
||||
tracker.record("fast_op", Duration::from_millis(5));
|
||||
tracker.record("slow_op", Duration::from_millis(50));
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
|
||||
let mut thresholds = std::collections::HashMap::new();
|
||||
thresholds.insert("fast_op".to_string(), 10.0);
|
||||
thresholds.insert("slow_op".to_string(), 20.0);
|
||||
|
||||
let violations = metrics.check_thresholds(&thresholds);
|
||||
|
||||
assert_eq!(violations.len(), 1, "Should have one violation");
|
||||
assert!(
|
||||
violations[0].contains("slow_op"),
|
||||
"Violation should be for slow_op"
|
||||
);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Real-time Factor Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_realtime_factor() {
|
||||
let mut tracker = PerformanceTracker::with_scan_interval(Duration::from_millis(200)); // 5Hz
|
||||
tracker.start();
|
||||
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Simulate processing faster than real-time
|
||||
for i in 0..50 {
|
||||
let pose = Pose2D::new(i as f32 * 0.1, 0.0, 0.0);
|
||||
let scan = common::simple_scan(360, 8.0);
|
||||
|
||||
let start = Instant::now();
|
||||
map.observe_lidar(&scan, pose);
|
||||
tracker.record("total", start.elapsed());
|
||||
tracker.record_scan();
|
||||
}
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
|
||||
println!(
|
||||
"Real-time factor: {:.2}x (scans/sec: {:.1})",
|
||||
metrics.real_time_factor, metrics.scans_per_second
|
||||
);
|
||||
|
||||
// Should be faster than real-time
|
||||
assert!(
|
||||
metrics.real_time_factor > 1.0,
|
||||
"Should process faster than real-time, got {:.2}x",
|
||||
metrics.real_time_factor
|
||||
);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Stress Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_sustained_performance() {
|
||||
let mut tracker = PerformanceTracker::new();
|
||||
tracker.start();
|
||||
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Process many scans to check for performance degradation
|
||||
let num_scans = 100;
|
||||
let mut early_times = Vec::new();
|
||||
let mut late_times = Vec::new();
|
||||
|
||||
for i in 0..num_scans {
|
||||
let angle = i as f32 * 0.1;
|
||||
let x = 5.0 + 3.0 * angle.cos();
|
||||
let y = 5.0 + 3.0 * angle.sin();
|
||||
let pose = Pose2D::new(x, y, angle);
|
||||
let scan = common::room_scan(10.0, 10.0, x, y, 360);
|
||||
|
||||
let start = Instant::now();
|
||||
map.observe_lidar(&scan, pose);
|
||||
let elapsed = start.elapsed();
|
||||
|
||||
tracker.record("map_update", elapsed);
|
||||
tracker.record_scan();
|
||||
|
||||
if i < 10 {
|
||||
early_times.push(elapsed);
|
||||
} else if i >= num_scans - 10 {
|
||||
late_times.push(elapsed);
|
||||
}
|
||||
}
|
||||
|
||||
let early_avg =
|
||||
early_times.iter().sum::<Duration>().as_secs_f32() / early_times.len() as f32 * 1000.0;
|
||||
let late_avg =
|
||||
late_times.iter().sum::<Duration>().as_secs_f32() / late_times.len() as f32 * 1000.0;
|
||||
|
||||
println!(
|
||||
"Early avg: {:.3}ms, Late avg: {:.3}ms (ratio: {:.2}x)",
|
||||
early_avg,
|
||||
late_avg,
|
||||
late_avg / early_avg
|
||||
);
|
||||
|
||||
// Performance shouldn't degrade by more than 2x
|
||||
assert!(
|
||||
late_avg < early_avg * 2.0,
|
||||
"Performance degradation too high: early={:.2}ms, late={:.2}ms",
|
||||
early_avg,
|
||||
late_avg
|
||||
);
|
||||
|
||||
let metrics = tracker.metrics();
|
||||
println!("\nSustained performance report:");
|
||||
metrics.print();
|
||||
}
|
||||
@@ -0,0 +1,267 @@
|
||||
//! Quality integration tests for VastuSLAM.
|
||||
//!
|
||||
//! These tests verify map quality and loop closure detection quality.
|
||||
|
||||
mod common;
|
||||
|
||||
use vastu_slam::evaluation::{LoopClosureGroundTruth, LoopClosurePR, MapQualityMetrics};
|
||||
use vastu_slam::{MapConfig, OccupancyGridMap, Pose2D};
|
||||
|
||||
// ============================================================================
|
||||
// Loop Closure Quality Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_pr_perfect_detection() {
|
||||
// Ground truth: these are the actual loop closures
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![(0, 50), (10, 60), (20, 70)]);
|
||||
|
||||
// Detections: perfectly detected all loop closures
|
||||
let detections = vec![
|
||||
(0, 50, 0.95), // Correct
|
||||
(10, 60, 0.90), // Correct
|
||||
(20, 70, 0.85), // Correct
|
||||
];
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
println!("Perfect detection PR:");
|
||||
pr.print();
|
||||
|
||||
assert_eq!(pr.true_positives, 3);
|
||||
assert_eq!(pr.false_positives, 0);
|
||||
assert_eq!(pr.false_negatives, 0);
|
||||
assert!((pr.precision - 1.0).abs() < 1e-5);
|
||||
assert!((pr.recall - 1.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_pr_with_false_positives() {
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![(0, 50), (10, 60)]);
|
||||
|
||||
// Detections include a false positive
|
||||
let detections = vec![
|
||||
(0, 50, 0.95), // Correct
|
||||
(10, 60, 0.90), // Correct
|
||||
(30, 80, 0.70), // False positive
|
||||
];
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
println!("With false positive PR:");
|
||||
pr.print();
|
||||
|
||||
assert_eq!(pr.true_positives, 2);
|
||||
assert_eq!(pr.false_positives, 1);
|
||||
assert!((pr.precision - 2.0 / 3.0).abs() < 1e-5);
|
||||
assert!((pr.recall - 1.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_pr_with_false_negatives() {
|
||||
let ground_truth = LoopClosureGroundTruth::from_pairs(vec![(0, 50), (10, 60), (20, 70)]);
|
||||
|
||||
// Only detected 2 out of 3 closures
|
||||
let detections = vec![
|
||||
(0, 50, 0.95),
|
||||
(10, 60, 0.90),
|
||||
// Missing (20, 70)
|
||||
];
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
println!("With false negative PR:");
|
||||
pr.print();
|
||||
|
||||
assert_eq!(pr.true_positives, 2);
|
||||
assert_eq!(pr.false_negatives, 1);
|
||||
assert!((pr.precision - 1.0).abs() < 1e-5);
|
||||
assert!((pr.recall - 2.0 / 3.0).abs() < 1e-5);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_loop_closure_ground_truth_from_trajectory() {
|
||||
// Create a trajectory that returns to start
|
||||
let trajectory = common::square_trajectory(4.0, 10);
|
||||
|
||||
// Generate ground truth based on spatial proximity
|
||||
let ground_truth = LoopClosureGroundTruth::from_trajectory(&trajectory, 20, 0.5);
|
||||
|
||||
println!(
|
||||
"Generated {} loop closure ground truths from trajectory",
|
||||
ground_truth.closures.len()
|
||||
);
|
||||
|
||||
// Should find at least one closure (start and end are close)
|
||||
assert!(
|
||||
!ground_truth.closures.is_empty(),
|
||||
"Should find loop closures in square trajectory"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_max_recall_at_100_precision() {
|
||||
let ground_truth =
|
||||
LoopClosureGroundTruth::from_pairs(vec![(0, 50), (10, 60), (20, 70), (30, 80)]);
|
||||
|
||||
// Detections sorted by score - first 2 are correct, 3rd is FP
|
||||
let detections = vec![
|
||||
(0, 50, 0.99), // Correct
|
||||
(10, 60, 0.95), // Correct
|
||||
(999, 888, 0.90), // False positive
|
||||
(20, 70, 0.85), // Correct but after FP
|
||||
];
|
||||
|
||||
let pr = LoopClosurePR::compute(&detections, &ground_truth, 1.0);
|
||||
|
||||
println!(
|
||||
"Max recall @ 100% precision: {:.2}%",
|
||||
pr.max_recall_at_100_precision * 100.0
|
||||
);
|
||||
|
||||
// Max recall at 100% precision is 2/4 = 50% (before the FP)
|
||||
assert!((pr.max_recall_at_100_precision - 0.5).abs() < 1e-5);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Map Quality Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_map_quality_empty_map() {
|
||||
let config = MapConfig::default();
|
||||
let map = OccupancyGridMap::new(config);
|
||||
|
||||
let quality = MapQualityMetrics::compute(map.storage());
|
||||
|
||||
assert_eq!(quality.coverage_m2, 0.0);
|
||||
assert_eq!(quality.known_percentage, 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_map_quality_with_room() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Create a simple room map
|
||||
let scan = common::room_scan(6.0, 6.0, 3.0, 3.0, 360);
|
||||
|
||||
// Multiple observations for better quality
|
||||
for i in 0..10 {
|
||||
let angle = i as f32 * 0.1;
|
||||
let pose = Pose2D::new(3.0, 3.0, angle);
|
||||
map.observe_lidar(&scan, pose);
|
||||
}
|
||||
|
||||
let quality = MapQualityMetrics::compute(map.storage());
|
||||
|
||||
println!("Room map quality:");
|
||||
quality.print();
|
||||
|
||||
// Should have explored some area
|
||||
assert!(quality.coverage_m2 > 0.0, "Should have explored area");
|
||||
assert!(quality.known_percentage > 0.0, "Should have known cells");
|
||||
assert!(quality.cell_counts.floor > 0, "Should have floor cells");
|
||||
assert!(quality.cell_counts.wall > 0, "Should have wall cells");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_map_quality_wall_sharpness() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Create clean room with sharp walls
|
||||
let scan = common::room_scan(6.0, 6.0, 3.0, 3.0, 720); // Higher resolution
|
||||
let _base_pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
for i in 0..20 {
|
||||
let angle = i as f32 * 0.05;
|
||||
let p = Pose2D::new(3.0, 3.0, angle);
|
||||
map.observe_lidar(&scan, p);
|
||||
}
|
||||
|
||||
let quality = MapQualityMetrics::compute(map.storage());
|
||||
|
||||
println!("Wall sharpness: {:.3}", quality.wall_sharpness);
|
||||
println!(
|
||||
"Avg wall thickness: {:.2} cells",
|
||||
quality.avg_wall_thickness
|
||||
);
|
||||
|
||||
// Clean scans should produce relatively sharp walls
|
||||
// This is more of a baseline than a strict requirement
|
||||
assert!(quality.wall_sharpness >= 0.0 && quality.wall_sharpness <= 1.0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Combined Quality Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_quality_trajectory_and_map() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Simulate robot moving through a room
|
||||
let trajectory = common::square_trajectory(4.0, 10);
|
||||
|
||||
for (i, pose) in trajectory.iter().enumerate() {
|
||||
// Robot at center of 8x8 room
|
||||
let scan = common::room_scan(8.0, 8.0, pose.x + 4.0, pose.y + 4.0, 360);
|
||||
map.observe_lidar(&scan, Pose2D::new(pose.x + 4.0, pose.y + 4.0, pose.theta));
|
||||
|
||||
// Check quality periodically
|
||||
if i % 10 == 9 {
|
||||
let quality = MapQualityMetrics::compute(map.storage());
|
||||
println!(
|
||||
"Step {}: coverage={:.2}m², known={:.1}%",
|
||||
i + 1,
|
||||
quality.coverage_m2,
|
||||
quality.known_percentage
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
let final_quality = MapQualityMetrics::compute(map.storage());
|
||||
|
||||
println!("\n=== Final Map Quality ===");
|
||||
final_quality.print();
|
||||
|
||||
// After full trajectory, should have good coverage
|
||||
assert!(
|
||||
final_quality.coverage_m2 > 10.0,
|
||||
"Should have significant coverage after full trajectory"
|
||||
);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Regression Tests
|
||||
// ============================================================================
|
||||
|
||||
#[test]
|
||||
fn test_quality_regression_basic_room() {
|
||||
let config = MapConfig::default();
|
||||
let mut map = OccupancyGridMap::new(config);
|
||||
|
||||
// Standard room scan
|
||||
let scan = common::room_scan(6.0, 6.0, 3.0, 3.0, 360);
|
||||
let robot_pose = Pose2D::new(3.0, 3.0, 0.0);
|
||||
|
||||
for _ in 0..5 {
|
||||
map.observe_lidar(&scan, robot_pose);
|
||||
}
|
||||
|
||||
let quality = MapQualityMetrics::compute(map.storage());
|
||||
|
||||
// Baseline expectations for a 6x6 room
|
||||
assert!(
|
||||
quality.cell_counts.wall > 100,
|
||||
"Should detect sufficient wall cells, got {}",
|
||||
quality.cell_counts.wall
|
||||
);
|
||||
assert!(
|
||||
quality.cell_counts.floor > 1000,
|
||||
"Should detect sufficient floor cells, got {}",
|
||||
quality.cell_counts.floor
|
||||
);
|
||||
}
|
||||
Reference in New Issue
Block a user