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

Refactor: code cleanup and modularization

Browse Source 
- Remove duplicate VectorMapConfig from src/config.rs (kept in vector_map/config.rs)
- Add shared geometry utilities to src/core/math.rs (centroid, covariance, principal direction)
- Consolidate fit_line() and fit_line_checked() via shared implementation
- Update ransac_lines.rs to use shared geometry utilities
- Split loop_closure/mod.rs (778 lines) into 4 focused submodules:
  - config.rs: LoopClosureConfig
  - detector.rs: LoopClosureDetector implementation
  - traits.rs: LoopDetector trait
  - types.rs: Keyframe, LoopClosure structs

Net reduction: ~836 lines of duplicated code
This commit is contained in:
codetiger
2025-12-22 18:01:17 +05:30
parent 68242ca310
commit 770ee910f0
10 changed files with 1109 additions and 1193 deletions
Download Patch File Download Diff File Expand all files Collapse all files
vastu-map/src/config.rs
-258
View File
@@ -3,7 +3,6 @@
//! All configuration parameters have sensible defaults for indoor robot navigation.
use serde::{Deserialize, Serialize};
use std::f32::consts::PI;
/// Lidar measurement noise model.
///
@@ -212,258 +211,10 @@ impl ExplorationConfig {
}
}
/// Configuration for VectorMap SLAM.
///
/// All parameters have sensible defaults. Create with `Default::default()`
/// and modify as needed.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct VectorMapConfig {
// ─────────────────────────────────────────────────────────────────────────
// Input Filtering
// ─────────────────────────────────────────────────────────────────────────
/// Minimum quality value for scan points (0-255).
/// Points below this threshold are filtered out.
/// Default: 10
pub min_scan_quality: u8,
/// Minimum valid range in meters.
/// Points closer than this are filtered out.
/// Default: 0.15m
pub min_scan_range: f32,
/// Maximum valid range in meters.
/// Points farther than this are filtered out.
/// Default: 12.0m
pub max_scan_range: f32,
// ─────────────────────────────────────────────────────────────────────────
// Line Extraction (Split-and-Merge)
// ─────────────────────────────────────────────────────────────────────────
/// Maximum perpendicular distance from point to line for splitting.
/// Points deviating more than this trigger a split.
/// Default: 0.05m (5cm)
pub split_distance_threshold: f32,
/// Minimum line length to keep.
/// Shorter lines are discarded.
/// Default: 0.10m (10cm)
pub min_line_length: f32,
/// Minimum number of points to form a line.
/// Default: 5
pub min_points_per_line: usize,
/// Maximum gap between consecutive points to be considered continuous.
/// Gaps larger than this start a new line segment.
/// Default: 0.30m (30cm)
pub max_point_gap: f32,
// ─────────────────────────────────────────────────────────────────────────
// Corner Detection
// ─────────────────────────────────────────────────────────────────────────
/// Minimum angle between lines to form a corner (radians).
/// Default: 30° (π/6)
pub corner_angle_min: f32,
/// Maximum angle between lines to form a corner (radians).
/// Default: 150° (5π/6)
pub corner_angle_max: f32,
/// Maximum distance between line endpoints to form a corner.
/// Default: 0.05m (5cm)
pub corner_distance_threshold: f32,
// ─────────────────────────────────────────────────────────────────────────
// Line Association (Map Integration)
// ─────────────────────────────────────────────────────────────────────────
/// Maximum angular difference between line directions for association (radians).
/// Default: 10° (π/18)
pub association_angle_tolerance: f32,
/// Maximum perpendicular distance between lines for association.
/// Default: 0.05m (5cm)
pub association_distance_tolerance: f32,
/// Minimum overlap ratio required for merging lines.
/// Default: 0.3 (30%)
pub association_overlap_ratio: f32,
// ─────────────────────────────────────────────────────────────────────────
// Scan Matching
// ─────────────────────────────────────────────────────────────────────────
/// Maximum iterations for Point-to-Line ICP.
/// Default: 50
pub icp_max_iterations: usize,
/// Convergence threshold for ICP (translation change in meters).
/// ICP stops when pose change is below this.
/// Default: 0.001m (1mm)
pub icp_convergence_threshold: f32,
/// Maximum rotation change for ICP convergence (radians).
/// Default: 0.001 rad (~0.06°)
pub icp_rotation_threshold: f32,
/// Maximum correspondence distance for ICP.
/// Points farther from any line are treated as outliers.
/// Default: 0.30m (30cm)
pub icp_max_correspondence_dist: f32,
/// Minimum percentage of points with valid correspondences.
/// Below this, scan matching is considered failed.
/// Default: 0.3 (30%)
pub icp_min_correspondence_ratio: f32,
/// Number of RANSAC iterations for coarse alignment.
/// Default: 100
pub ransac_iterations: usize,
/// Inlier distance threshold for RANSAC.
/// Default: 0.05m (5cm)
pub ransac_inlier_threshold: f32,
/// Lidar noise model for correspondence weighting.
/// Used to weight measurements by their estimated uncertainty.
pub noise_model: LidarNoiseModel,
// ─────────────────────────────────────────────────────────────────────────
// Confidence Thresholds
// ─────────────────────────────────────────────────────────────────────────
/// Minimum confidence to accept a scan match.
/// Below this, odometry is used instead.
/// Default: 0.3
pub min_match_confidence: f32,
}
impl Default for VectorMapConfig {
fn default() -> Self {
Self {
// Input filtering
min_scan_quality: 10,
min_scan_range: 0.15,
max_scan_range: 12.0,
// Line extraction
split_distance_threshold: 0.05,
min_line_length: 0.10,
min_points_per_line: 5,
max_point_gap: 0.30,
// Corner detection
corner_angle_min: PI / 6.0, // 30°
corner_angle_max: 5.0 * PI / 6.0, // 150°
corner_distance_threshold: 0.05,
// Line association
association_angle_tolerance: PI / 18.0, // 10°
association_distance_tolerance: 0.05,
association_overlap_ratio: 0.3,
// Scan matching
icp_max_iterations: 50,
icp_convergence_threshold: 0.001,
icp_rotation_threshold: 0.001,
icp_max_correspondence_dist: 0.30,
icp_min_correspondence_ratio: 0.3,
ransac_iterations: 100,
ransac_inlier_threshold: 0.05,
noise_model: LidarNoiseModel::default(),
// Confidence
min_match_confidence: 0.3,
}
}
}
impl VectorMapConfig {
/// Create a new configuration with default values.
pub fn new() -> Self {
Self::default()
}
/// Builder-style setter for min_scan_quality.
pub fn with_min_scan_quality(mut self, value: u8) -> Self {
self.min_scan_quality = value;
self
}
/// Builder-style setter for scan range.
pub fn with_scan_range(mut self, min: f32, max: f32) -> Self {
self.min_scan_range = min;
self.max_scan_range = max;
self
}
/// Builder-style setter for split distance threshold.
pub fn with_split_threshold(mut self, value: f32) -> Self {
self.split_distance_threshold = value;
self
}
/// Builder-style setter for minimum line length.
pub fn with_min_line_length(mut self, value: f32) -> Self {
self.min_line_length = value;
self
}
/// Builder-style setter for ICP parameters.
pub fn with_icp_params(mut self, max_iterations: usize, convergence_threshold: f32) -> Self {
self.icp_max_iterations = max_iterations;
self.icp_convergence_threshold = convergence_threshold;
self
}
/// Builder-style setter for RANSAC iterations.
pub fn with_ransac_iterations(mut self, value: usize) -> Self {
self.ransac_iterations = value;
self
}
/// Builder-style setter for lidar noise model.
pub fn with_noise_model(mut self, model: LidarNoiseModel) -> Self {
self.noise_model = model;
self
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_default_config() {
let config = VectorMapConfig::default();
assert_eq!(config.min_scan_quality, 10);
assert_eq!(config.min_scan_range, 0.15);
assert_eq!(config.max_scan_range, 12.0);
assert_eq!(config.split_distance_threshold, 0.05);
assert_eq!(config.min_line_length, 0.10);
assert_eq!(config.min_points_per_line, 5);
assert_eq!(config.icp_max_iterations, 50);
assert_eq!(config.ransac_iterations, 100);
}
#[test]
fn test_builder_pattern() {
let config = VectorMapConfig::new()
.with_min_scan_quality(20)
.with_scan_range(0.10, 8.0)
.with_split_threshold(0.03)
.with_min_line_length(0.15)
.with_icp_params(100, 0.0005)
.with_ransac_iterations(200);
assert_eq!(config.min_scan_quality, 20);
assert_eq!(config.min_scan_range, 0.10);
assert_eq!(config.max_scan_range, 8.0);
assert_eq!(config.split_distance_threshold, 0.03);
assert_eq!(config.min_line_length, 0.15);
assert_eq!(config.icp_max_iterations, 100);
assert_eq!(config.icp_convergence_threshold, 0.0005);
assert_eq!(config.ransac_iterations, 200);
}
#[test]
fn test_lidar_noise_model_default() {
let model = LidarNoiseModel::default();
@@ -550,15 +301,6 @@ mod tests {
}
}
#[test]
fn test_config_with_noise_model() {
let custom_model = LidarNoiseModel::new(0.02, 0.002);
let config = VectorMapConfig::new().with_noise_model(custom_model);
assert_eq!(config.noise_model.sigma_base, 0.02);
assert_eq!(config.noise_model.sigma_range, 0.002);
}
#[test]
fn test_exploration_config_default() {
let config = ExplorationConfig::default();
vastu-map/src/core/math.rs
+299
View File
@@ -104,6 +104,187 @@ pub fn sq(x: f32) -> f32 {
x * x
}
// ─────────────────────────────────────────────────────────────────────────────
// Geometry Utilities (shared by line fitting, RANSAC, etc.)
// ─────────────────────────────────────────────────────────────────────────────
use super::Point2D;
/// Covariance matrix elements for 2D point sets.
///
/// Represents a symmetric 2x2 matrix:
/// ```text
/// | cxx cxy |
/// | cxy cyy |
/// ```
#[derive(Debug, Clone, Copy, Default)]
pub struct Covariance2D {
/// Sum of (x - cx)^2
pub cxx: f32,
/// Sum of (y - cy)^2
pub cyy: f32,
/// Sum of (x - cx)(y - cy)
pub cxy: f32,
}
/// Compute the centroid of a set of points.
///
/// # Example
/// ```
/// use vastu_map::core::{Point2D, math::compute_centroid};
///
/// let points = [Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0), Point2D::new(1.0, 1.0)];
/// let centroid = compute_centroid(&points);
/// assert!((centroid.x - 1.0).abs() < 1e-6);
/// assert!((centroid.y - 1.0 / 3.0).abs() < 1e-6);
/// ```
#[inline]
pub fn compute_centroid(points: &[Point2D]) -> Point2D {
if points.is_empty() {
return Point2D::new(0.0, 0.0);
}
let n = points.len() as f32;
let mut sum_x: f32 = 0.0;
let mut sum_y: f32 = 0.0;
for p in points {
sum_x += p.x;
sum_y += p.y;
}
Point2D::new(sum_x / n, sum_y / n)
}
/// Compute the 2x2 covariance matrix elements for a set of points.
///
/// The covariance matrix describes the spread and orientation of points
/// around their centroid. Used for Total Least Squares line fitting and PCA.
///
/// # Arguments
/// * `points` - Point set
/// * `centroid` - Pre-computed centroid (use [`compute_centroid`])
///
/// # Example
/// ```
/// use vastu_map::core::{Point2D, math::{compute_centroid, compute_covariance}};
///
/// let points = [
/// Point2D::new(0.0, 0.0),
/// Point2D::new(1.0, 1.0),
/// Point2D::new(2.0, 2.0),
/// ];
/// let centroid = compute_centroid(&points);
/// let cov = compute_covariance(&points, centroid);
///
/// // Points lie on y=x line, so cxx == cyy and cxy > 0
/// assert!((cov.cxx - cov.cyy).abs() < 1e-6);
/// assert!(cov.cxy > 0.0);
/// ```
#[inline]
pub fn compute_covariance(points: &[Point2D], centroid: Point2D) -> Covariance2D {
let mut cxx: f32 = 0.0;
let mut cyy: f32 = 0.0;
let mut cxy: f32 = 0.0;
for p in points {
let dx = p.x - centroid.x;
let dy = p.y - centroid.y;
cxx += dx * dx;
cyy += dy * dy;
cxy += dx * dy;
}
Covariance2D { cxx, cyy, cxy }
}
/// Compute weighted covariance matrix elements.
///
/// Points with higher weights contribute more to the covariance.
/// Useful for range-weighted lidar point fitting.
///
/// # Arguments
/// * `points` - Point set
/// * `weights` - Weight for each point (must have same length as points)
/// * `centroid` - Pre-computed weighted centroid
///
/// # Panics
/// Panics if `points.len() != weights.len()`.
#[inline]
pub fn compute_covariance_weighted(
points: &[Point2D],
weights: &[f32],
centroid: Point2D,
) -> Covariance2D {
debug_assert_eq!(points.len(), weights.len());
let mut cxx: f32 = 0.0;
let mut cyy: f32 = 0.0;
let mut cxy: f32 = 0.0;
for (p, &w) in points.iter().zip(weights.iter()) {
let dx = p.x - centroid.x;
let dy = p.y - centroid.y;
cxx += w * dx * dx;
cyy += w * dy * dy;
cxy += w * dx * dy;
}
Covariance2D { cxx, cyy, cxy }
}
/// Compute the principal direction from a 2x2 covariance matrix.
///
/// Returns the eigenvector corresponding to the larger eigenvalue,
/// which represents the direction of maximum variance.
///
/// Returns `None` if the covariance matrix is degenerate (e.g., all points
/// coincident or forming a circle).
///
/// # Example
/// ```
/// use vastu_map::core::{Point2D, math::{compute_centroid, compute_covariance, principal_direction}};
///
/// // Points on a horizontal line
/// let points = [Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0), Point2D::new(2.0, 0.0)];
/// let centroid = compute_centroid(&points);
/// let cov = compute_covariance(&points, centroid);
/// let dir = principal_direction(&cov).unwrap();
///
/// // Direction should be horizontal (1, 0) or (-1, 0)
/// assert!(dir.y.abs() < 1e-6);
/// assert!((dir.x.abs() - 1.0).abs() < 1e-6);
/// ```
#[inline]
pub fn principal_direction(cov: &Covariance2D) -> Option<Point2D> {
// Find eigenvectors of 2x2 covariance matrix
// For 2x2 symmetric matrix, eigenvalues are:
// λ = (cxx + cyy)/2 ± sqrt(((cxx - cyy)/2)² + cxy²)
let trace_half = (cov.cxx + cov.cyy) / 2.0;
let det_sqrt = ((cov.cxx - cov.cyy) / 2.0).powi(2) + cov.cxy * cov.cxy;
if det_sqrt < f32::EPSILON {
// Degenerate case - points form a circle or single point
return None;
}
let det_sqrt = det_sqrt.sqrt();
let lambda1 = trace_half + det_sqrt; // Larger eigenvalue
// Eigenvector for larger eigenvalue (line direction)
// For eigenvalue λ1, eigenvector satisfies (cxx - λ1)x + cxy*y = 0
// Direction: (cxy, λ1 - cxx) or (-cxy, cxx - λ1)
if cov.cxy.abs() > f32::EPSILON {
Some(Point2D::new(lambda1 - cov.cyy, cov.cxy).normalized())
} else if cov.cxx > cov.cyy {
Some(Point2D::new(1.0, 0.0)) // Horizontal line
} else {
Some(Point2D::new(0.0, 1.0)) // Vertical line
}
}
#[cfg(test)]
mod tests {
use super::*;
@@ -170,4 +351,122 @@ mod tests {
assert_eq!(sq(-3.0), 9.0);
assert_eq!(sq(0.0), 0.0);
}
// ─────────────────────────────────────────────────────────────────────────
// Geometry Utility Tests
// ─────────────────────────────────────────────────────────────────────────
#[test]
fn test_compute_centroid() {
// Empty points
let empty: &[Point2D] = &[];
let c = compute_centroid(empty);
assert_eq!(c.x, 0.0);
assert_eq!(c.y, 0.0);
// Single point
let single = [Point2D::new(3.0, 4.0)];
let c = compute_centroid(&single);
assert_relative_eq!(c.x, 3.0, epsilon = 1e-6);
assert_relative_eq!(c.y, 4.0, epsilon = 1e-6);
// Triangle
let triangle = [
Point2D::new(0.0, 0.0),
Point2D::new(3.0, 0.0),
Point2D::new(0.0, 3.0),
];
let c = compute_centroid(&triangle);
assert_relative_eq!(c.x, 1.0, epsilon = 1e-6);
assert_relative_eq!(c.y, 1.0, epsilon = 1e-6);
}
#[test]
fn test_compute_covariance() {
// Points on horizontal line (y=0)
let horizontal = [
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
];
let centroid = compute_centroid(&horizontal);
let cov = compute_covariance(&horizontal, centroid);
// cxx > 0, cyy = 0, cxy = 0
assert!(cov.cxx > 0.0);
assert_relative_eq!(cov.cyy, 0.0, epsilon = 1e-6);
assert_relative_eq!(cov.cxy, 0.0, epsilon = 1e-6);
// Points on diagonal (y=x)
let diagonal = [
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 1.0),
Point2D::new(2.0, 2.0),
];
let centroid = compute_centroid(&diagonal);
let cov = compute_covariance(&diagonal, centroid);
// cxx == cyy, cxy > 0
assert_relative_eq!(cov.cxx, cov.cyy, epsilon = 1e-6);
assert!(cov.cxy > 0.0);
}
#[test]
fn test_principal_direction_horizontal() {
let points = [
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 0.0),
Point2D::new(2.0, 0.0),
];
let centroid = compute_centroid(&points);
let cov = compute_covariance(&points, centroid);
let dir = principal_direction(&cov).unwrap();
// Direction should be horizontal
assert!(dir.y.abs() < 1e-6);
assert!((dir.x.abs() - 1.0).abs() < 1e-6);
}
#[test]
fn test_principal_direction_vertical() {
let points = [
Point2D::new(0.0, 0.0),
Point2D::new(0.0, 1.0),
Point2D::new(0.0, 2.0),
];
let centroid = compute_centroid(&points);
let cov = compute_covariance(&points, centroid);
let dir = principal_direction(&cov).unwrap();
// Direction should be vertical
assert!(dir.x.abs() < 1e-6);
assert!((dir.y.abs() - 1.0).abs() < 1e-6);
}
#[test]
fn test_principal_direction_diagonal() {
let points = [
Point2D::new(0.0, 0.0),
Point2D::new(1.0, 1.0),
Point2D::new(2.0, 2.0),
];
let centroid = compute_centroid(&points);
let cov = compute_covariance(&points, centroid);
let dir = principal_direction(&cov).unwrap();
// Direction should be 45° (normalized: ~0.707, ~0.707)
assert_relative_eq!(dir.x.abs(), dir.y.abs(), epsilon = 1e-5);
assert!((dir.length() - 1.0).abs() < 1e-6);
}
#[test]
fn test_principal_direction_degenerate() {
// Single point - degenerate
let cov = Covariance2D {
cxx: 0.0,
cyy: 0.0,
cxy: 0.0,
};
assert!(principal_direction(&cov).is_none());
}
}
vastu-map/src/extraction/line_fitting.rs
+25 -138
View File
@@ -39,6 +39,7 @@
use crate::config::LidarNoiseModel;
use crate::core::Point2D;
use crate::core::math::{compute_centroid, compute_covariance, principal_direction};
use crate::features::Line2D;
// ============================================================================
@@ -269,64 +270,29 @@ impl std::error::Error for FitError {}
/// // Line should be approximately horizontal
/// ```
pub fn fit_line(points: &[Point2D]) -> Option<Line2D> {
fit_line_impl(points).ok()
}
/// Internal implementation shared by fit_line and fit_line_checked.
fn fit_line_impl(points: &[Point2D]) -> Result<Line2D, FitError> {
if points.len() < 2 {
return None;
return Err(FitError::InsufficientPoints {
got: points.len(),
required: 2,
});
}
// Compute centroid
let n = points.len() as f32;
let mut sum_x: f32 = 0.0;
let mut sum_y: f32 = 0.0;
// Use shared geometry utilities
let centroid = compute_centroid(points);
let cov = compute_covariance(points, centroid);
for p in points {
sum_x += p.x;
sum_y += p.y;
}
let centroid = Point2D::new(sum_x / n, sum_y / n);
// Compute covariance matrix elements
// Cov = | cxx cxy |
// | cxy cyy |
let mut cxx: f32 = 0.0;
let mut cyy: f32 = 0.0;
let mut cxy: f32 = 0.0;
for p in points {
let dx = p.x - centroid.x;
let dy = p.y - centroid.y;
cxx += dx * dx;
cyy += dy * dy;
cxy += dx * dy;
}
// Find eigenvectors of 2x2 covariance matrix
// For 2x2 symmetric matrix, eigenvalues are:
// λ = (cxx + cyy)/2 ± sqrt(((cxx - cyy)/2)² + cxy²)
let trace_half = (cxx + cyy) / 2.0;
let det_sqrt = ((cxx - cyy) / 2.0).powi(2) + cxy * cxy;
if det_sqrt < f32::EPSILON {
// Degenerate case - points form a circle or single point
// Use simple linear regression fallback
return fit_line_simple(points);
}
let det_sqrt = det_sqrt.sqrt();
let lambda1 = trace_half + det_sqrt; // Larger eigenvalue
let _lambda2 = trace_half - det_sqrt; // Smaller eigenvalue
// Eigenvector for larger eigenvalue (line direction)
// For eigenvalue λ1, eigenvector satisfies (cxx - λ1)x + cxy*y = 0
// Direction: (cxy, λ1 - cxx) or (-cxy, cxx - λ1)
let direction = if cxy.abs() > f32::EPSILON {
Point2D::new(lambda1 - cyy, cxy).normalized()
} else if cxx > cyy {
Point2D::new(1.0, 0.0) // Horizontal line
} else {
Point2D::new(0.0, 1.0) // Vertical line
// Get principal direction from covariance matrix
let direction = match principal_direction(&cov) {
Some(dir) => dir,
None => {
// Degenerate case - try simple fallback
return fit_line_simple_impl(points);
}
};
// Find extent of points along line direction
@@ -349,7 +315,7 @@ pub fn fit_line(points: &[Point2D]) -> Option<Line2D> {
centroid.y + t_max * direction.y,
);
Some(Line2D::with_point_count(start, end, points.len() as u32))
Ok(Line2D::with_point_count(start, end, points.len() as u32))
}
/// Fit a line to a set of points using Total Least Squares (with explicit errors).
@@ -372,83 +338,11 @@ pub fn fit_line(points: &[Point2D]) -> Option<Line2D> {
/// assert!(matches!(result, Err(FitError::InsufficientPoints { .. })));
/// ```
pub fn fit_line_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
if points.len() < 2 {
return Err(FitError::InsufficientPoints {
got: points.len(),
required: 2,
});
}
// Compute centroid
let n = points.len() as f32;
let mut sum_x: f32 = 0.0;
let mut sum_y: f32 = 0.0;
for p in points {
sum_x += p.x;
sum_y += p.y;
}
let centroid = Point2D::new(sum_x / n, sum_y / n);
// Compute covariance matrix elements
let mut cxx: f32 = 0.0;
let mut cyy: f32 = 0.0;
let mut cxy: f32 = 0.0;
for p in points {
let dx = p.x - centroid.x;
let dy = p.y - centroid.y;
cxx += dx * dx;
cyy += dy * dy;
cxy += dx * dy;
}
// Find eigenvectors of 2x2 covariance matrix
let trace_half = (cxx + cyy) / 2.0;
let det_sqrt = ((cxx - cyy) / 2.0).powi(2) + cxy * cxy;
if det_sqrt < f32::EPSILON {
// Degenerate case - try simple fallback
return fit_line_simple_checked(points);
}
let det_sqrt = det_sqrt.sqrt();
let lambda1 = trace_half + det_sqrt;
// Eigenvector for larger eigenvalue (line direction)
let direction = if cxy.abs() > f32::EPSILON {
Point2D::new(lambda1 - cyy, cxy).normalized()
} else if cxx > cyy {
Point2D::new(1.0, 0.0)
} else {
Point2D::new(0.0, 1.0)
};
// Find extent of points along line direction
let mut t_min = f32::MAX;
let mut t_max = f32::MIN;
for p in points {
let t = (p.x - centroid.x) * direction.x + (p.y - centroid.y) * direction.y;
t_min = t_min.min(t);
t_max = t_max.max(t);
}
let start = Point2D::new(
centroid.x + t_min * direction.x,
centroid.y + t_min * direction.y,
);
let end = Point2D::new(
centroid.x + t_max * direction.x,
centroid.y + t_max * direction.y,
);
Ok(Line2D::with_point_count(start, end, points.len() as u32))
fit_line_impl(points)
}
/// Simple line fitting fallback (checked version).
fn fit_line_simple_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
/// Simple line fitting fallback (shared implementation).
fn fit_line_simple_impl(points: &[Point2D]) -> Result<Line2D, FitError> {
if points.len() < 2 {
return Err(FitError::InsufficientPoints {
got: points.len(),
@@ -464,14 +358,7 @@ fn fit_line_simple_checked(points: &[Point2D]) -> Result<Line2D, FitError> {
/// Simple line fitting fallback using first and last points.
fn fit_line_simple(points: &[Point2D]) -> Option<Line2D> {
if points.len() < 2 {
return None;
}
Some(Line2D::with_point_count(
points[0],
points[points.len() - 1],
points.len() as u32,
))
fit_line_simple_impl(points).ok()
}
/// Compute range-based weights for line fitting.
vastu-map/src/extraction/ransac_lines.rs
+13 -38
View File
@@ -22,6 +22,7 @@
use serde::{Deserialize, Serialize};
use crate::core::Point2D;
use crate::core::math::{compute_centroid, compute_covariance};
use crate::features::Line2D;
use super::line_fitting::fit_line;
@@ -300,25 +301,12 @@ fn split_at_gaps(
return vec![(inlier_points.to_vec(), inlier_indices.to_vec())];
}
// Compute principal direction using PCA-like approach
let n = inlier_points.len() as f32;
let mean_x: f32 = inlier_points.iter().map(|p| p.x).sum::<f32>() / n;
let mean_y: f32 = inlier_points.iter().map(|p| p.y).sum::<f32>() / n;
// Use shared utilities for centroid and covariance
let centroid = compute_centroid(inlier_points);
let cov = compute_covariance(inlier_points, centroid);
// Compute covariance matrix elements
let mut cxx = 0.0f32;
let mut cxy = 0.0f32;
let mut cyy = 0.0f32;
for p in inlier_points {
let dx = p.x - mean_x;
let dy = p.y - mean_y;
cxx += dx * dx;
cxy += dx * dy;
cyy += dy * dy;
}
// Principal direction from dominant eigenvector
let theta = 0.5 * (2.0 * cxy).atan2(cxx - cyy);
// Principal direction from dominant eigenvector (using atan2 for angle)
let theta = 0.5 * (2.0 * cov.cxy).atan2(cov.cxx - cov.cyy);
let dir_x = theta.cos();
let dir_y = theta.sin();
@@ -327,7 +315,7 @@ fn split_at_gaps(
.iter()
.enumerate()
.map(|(i, p)| {
let proj = (p.x - mean_x) * dir_x + (p.y - mean_y) * dir_y;
let proj = (p.x - centroid.x) * dir_x + (p.y - centroid.y) * dir_y;
(i, proj)
})
.collect();
@@ -654,32 +642,19 @@ fn split_at_gaps_with_scratch(config: &RansacLineConfig, scratch: &mut RansacScr
return false;
}
// Compute principal direction using PCA-like approach
let n = num_inliers as f32;
let mean_x: f32 = scratch.inlier_points.iter().map(|p| p.x).sum::<f32>() / n;
let mean_y: f32 = scratch.inlier_points.iter().map(|p| p.y).sum::<f32>() / n;
// Use shared utilities for centroid and covariance
let centroid = compute_centroid(&scratch.inlier_points);
let cov = compute_covariance(&scratch.inlier_points, centroid);
// Compute covariance matrix elements
let mut cxx = 0.0f32;
let mut cxy = 0.0f32;
let mut cyy = 0.0f32;
for p in &scratch.inlier_points {
let dx = p.x - mean_x;
let dy = p.y - mean_y;
cxx += dx * dx;
cxy += dx * dy;
cyy += dy * dy;
}
// Principal direction from dominant eigenvector
let theta = 0.5 * (2.0 * cxy).atan2(cxx - cyy);
// Principal direction from dominant eigenvector (using atan2 for angle)
let theta = 0.5 * (2.0 * cov.cxy).atan2(cov.cxx - cov.cyy);
let dir_x = theta.cos();
let dir_y = theta.sin();
// Project points onto principal direction and sort
scratch.projection_buffer.clear();
for (i, p) in scratch.inlier_points.iter().enumerate() {
let proj = (p.x - mean_x) * dir_x + (p.y - mean_y) * dir_y;
let proj = (p.x - centroid.x) * dir_x + (p.y - centroid.y) * dir_y;
scratch.projection_buffer.push((i, proj));
}
scratch
vastu-map/src/loop_closure/config.rs
+76
View File
@@ -0,0 +1,76 @@
//! Configuration for loop closure detection.
use serde::{Deserialize, Serialize};
use crate::matching::IcpConfig;
/// Configuration for loop closure detection.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct LoopClosureConfig {
/// Minimum travel distance before considering loop closure (meters).
/// Prevents detecting loops immediately after keyframe creation.
/// Default: 5.0m
pub min_travel_distance: f32,
/// Maximum descriptor distance for candidate retrieval.
/// Lower values are more selective but may miss valid loops.
/// Default: 0.5
pub max_descriptor_distance: f32,
/// Minimum ICP confidence to accept a loop closure.
/// Default: 0.6
pub min_verification_confidence: f32,
/// Distance between keyframes (meters).
/// Default: 2.0m
pub keyframe_interval: f32,
/// Radius for computing corner descriptors (meters).
/// Default: 2.0m
pub descriptor_radius: f32,
/// Maximum number of candidates to verify with ICP per keyframe.
/// Default: 3
pub max_candidates_to_verify: usize,
/// Minimum number of lines required for a valid keyframe.
/// Default: 3
pub min_lines_for_keyframe: usize,
/// Minimum number of corners required for a valid keyframe.
/// Default: 2
pub min_corners_for_keyframe: usize,
/// ICP configuration for verification.
pub icp_config: IcpConfig,
}
impl Default for LoopClosureConfig {
fn default() -> Self {
Self {
min_travel_distance: 5.0,
max_descriptor_distance: 0.5,
min_verification_confidence: 0.6,
keyframe_interval: 2.0,
descriptor_radius: 2.0,
max_candidates_to_verify: 3,
min_lines_for_keyframe: 3,
min_corners_for_keyframe: 2,
icp_config: IcpConfig::default(),
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_config_default() {
let config = LoopClosureConfig::default();
assert_eq!(config.min_travel_distance, 5.0);
assert_eq!(config.max_descriptor_distance, 0.5);
assert_eq!(config.min_verification_confidence, 0.6);
assert_eq!(config.keyframe_interval, 2.0);
}
}
vastu-map/src/loop_closure/detector.rs
+502
View File
@@ -0,0 +1,502 @@
//! Loop closure detector implementation.
use crate::core::{Point2D, Pose2D};
use crate::features::{Corner2D, Line2D, ScanDescriptor};
use crate::matching::match_scan;
use super::config::LoopClosureConfig;
use super::types::{Keyframe, LoopClosure};
/// Loop closure detector using keyframe-based matching.
pub struct LoopClosureDetector {
/// Stored keyframes.
keyframes: Vec<Keyframe>,
/// Configuration.
config: LoopClosureConfig,
/// Last pose where a keyframe was created.
last_keyframe_pose: Option<Pose2D>,
/// Total distance traveled.
total_distance: f32,
/// Next keyframe ID.
next_keyframe_id: usize,
}
impl LoopClosureDetector {
/// Create a new loop closure detector.
pub fn new(config: LoopClosureConfig) -> Self {
Self {
keyframes: Vec::new(),
config,
last_keyframe_pose: None,
total_distance: 0.0,
next_keyframe_id: 0,
}
}
/// Get the configuration.
pub fn config(&self) -> &LoopClosureConfig {
&self.config
}
/// Get all keyframes.
pub fn keyframes(&self) -> &[Keyframe] {
&self.keyframes
}
/// Get the number of keyframes.
pub fn keyframe_count(&self) -> usize {
self.keyframes.len()
}
/// Get the total distance traveled.
pub fn total_distance(&self) -> f32 {
self.total_distance
}
/// Clear all keyframes and reset state.
pub fn clear(&mut self) {
self.keyframes.clear();
self.last_keyframe_pose = None;
self.total_distance = 0.0;
self.next_keyframe_id = 0;
}
/// Process a new observation and check for loop closures.
///
/// # Arguments
/// * `current_pose` - Current robot pose in world frame
/// * `lines` - Extracted lines in world frame
/// * `corners` - Extracted corners in world frame
/// * `points` - Scan points in robot frame (for ICP verification)
///
/// # Returns
/// A loop closure if one is detected, None otherwise.
pub fn process(
&mut self,
current_pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure> {
// Update distance traveled
if let Some(last_pose) = self.last_keyframe_pose {
let dx = current_pose.x - last_pose.x;
let dy = current_pose.y - last_pose.y;
self.total_distance += (dx * dx + dy * dy).sqrt();
}
// Check if we need to create a keyframe
let should_create_keyframe = self.should_create_keyframe(current_pose, lines, corners);
let mut loop_closure = None;
if should_create_keyframe {
// First check for loop closure candidates before adding keyframe
loop_closure = self.find_loop_closure(current_pose, lines, corners, points);
// Create and store keyframe
let keyframe = Keyframe::new(
self.next_keyframe_id,
current_pose,
lines.to_vec(),
corners.to_vec(),
points.to_vec(),
self.total_distance,
self.config.descriptor_radius,
);
self.keyframes.push(keyframe);
self.next_keyframe_id += 1;
self.last_keyframe_pose = Some(current_pose);
}
loop_closure
}
/// Check if a new keyframe should be created.
fn should_create_keyframe(
&self,
current_pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
) -> bool {
// Need minimum features
if lines.len() < self.config.min_lines_for_keyframe {
return false;
}
if corners.len() < self.config.min_corners_for_keyframe {
return false;
}
// First keyframe
if self.last_keyframe_pose.is_none() {
return true;
}
// Check distance since last keyframe
let last_pose = self.last_keyframe_pose.unwrap();
let dx = current_pose.x - last_pose.x;
let dy = current_pose.y - last_pose.y;
let distance = (dx * dx + dy * dy).sqrt();
distance >= self.config.keyframe_interval
}
/// Find loop closure candidates and verify with ICP.
fn find_loop_closure(
&self,
current_pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure> {
// Need enough keyframes for loop closure
if self.keyframes.is_empty() {
return None;
}
// Compute descriptor for current observation
let current_descriptor =
ScanDescriptor::compute(lines, corners, self.config.descriptor_radius);
if !current_descriptor.is_valid_for_matching() {
return None;
}
// Find candidates using descriptor distance
let mut candidates: Vec<(usize, f32)> = self
.keyframes
.iter()
.enumerate()
.filter_map(|(idx, kf)| {
// Skip recent keyframes (min travel distance)
let travel_since_kf = self.total_distance - kf.distance_traveled;
if travel_since_kf < self.config.min_travel_distance {
return None;
}
// Check descriptor validity
if !kf.descriptor.is_valid_for_matching() {
return None;
}
// Quick signature pre-filter
let sig_diff = (current_descriptor.quick_signature()
^ kf.descriptor.quick_signature())
.count_ones();
if sig_diff > 16 {
return None; // Too different
}
// Full descriptor distance
let dist = current_descriptor.distance(&kf.descriptor);
if dist <= self.config.max_descriptor_distance {
Some((idx, dist))
} else {
None
}
})
.collect();
// Sort by descriptor distance (best first)
candidates.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));
// Verify top candidates with ICP
for (kf_idx, descriptor_dist) in
candidates.iter().take(self.config.max_candidates_to_verify)
{
let keyframe = &self.keyframes[*kf_idx];
// Compute initial guess: relative pose from keyframe to current
let initial_guess = keyframe.pose.inverse().compose(current_pose);
// Transform current points to keyframe frame
let transformed_points: Vec<Point2D> = points
.iter()
.map(|p| {
// Transform from current robot frame to world, then to keyframe frame
let world = current_pose.transform_point(*p);
keyframe.pose.inverse().transform_point(world)
})
.collect();
// Run ICP verification
let match_result = match_scan(&transformed_points, &keyframe.lines, initial_guess);
if match_result.converged
&& match_result.confidence >= self.config.min_verification_confidence
{
// Found a valid loop closure
let relative_pose = keyframe.pose.inverse().compose(current_pose);
return Some(LoopClosure {
from_keyframe: keyframe.id,
to_keyframe: self.next_keyframe_id,
relative_pose,
covariance: match_result.covariance,
confidence: match_result.confidence,
descriptor_distance: *descriptor_dist,
});
}
}
None
}
/// Force add a keyframe at the current position.
/// Useful for initialization or when loop closure should be checked
/// regardless of distance traveled.
pub fn force_keyframe(
&mut self,
pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> usize {
let keyframe = Keyframe::new(
self.next_keyframe_id,
pose,
lines.to_vec(),
corners.to_vec(),
points.to_vec(),
self.total_distance,
self.config.descriptor_radius,
);
let id = self.next_keyframe_id;
self.keyframes.push(keyframe);
self.next_keyframe_id += 1;
self.last_keyframe_pose = Some(pose);
id
}
/// Get a keyframe by ID.
pub fn get_keyframe(&self, id: usize) -> Option<&Keyframe> {
self.keyframes.iter().find(|kf| kf.id == id)
}
/// Check if loop closure detection is possible.
/// Returns true if there are enough keyframes and sufficient travel distance.
pub fn can_detect_loop(&self) -> bool {
if self.keyframes.is_empty() {
return false;
}
// Check if we've traveled enough since any keyframe
self.keyframes.iter().any(|kf| {
let travel_since = self.total_distance - kf.distance_traveled;
travel_since >= self.config.min_travel_distance
})
}
}
impl Default for LoopClosureDetector {
fn default() -> Self {
Self::new(LoopClosureConfig::default())
}
}
#[cfg(test)]
mod tests {
use super::*;
fn make_simple_lines() -> Vec<Line2D> {
vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0)),
Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(2.0, 2.0)),
Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(0.0, 2.0)),
Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(0.0, 0.0)),
]
}
fn make_simple_corners() -> Vec<Corner2D> {
vec![
Corner2D::new(Point2D::new(0.0, 0.0), 3, 0, std::f32::consts::FRAC_PI_2),
Corner2D::new(Point2D::new(2.0, 0.0), 0, 1, std::f32::consts::FRAC_PI_2),
Corner2D::new(Point2D::new(2.0, 2.0), 1, 2, std::f32::consts::FRAC_PI_2),
Corner2D::new(Point2D::new(0.0, 2.0), 2, 3, std::f32::consts::FRAC_PI_2),
]
}
fn make_simple_points() -> Vec<Point2D> {
let mut points = Vec::new();
// Wall points
for i in 0..20 {
let t = i as f32 / 19.0 * 2.0;
points.push(Point2D::new(t, 0.0));
points.push(Point2D::new(t, 2.0));
points.push(Point2D::new(0.0, t));
points.push(Point2D::new(2.0, t));
}
points
}
#[test]
fn test_detector_new() {
let detector = LoopClosureDetector::default();
assert_eq!(detector.keyframe_count(), 0);
assert_eq!(detector.total_distance(), 0.0);
assert!(!detector.can_detect_loop());
}
#[test]
fn test_first_keyframe_created() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
let result = detector.process(Pose2D::identity(), &lines, &corners, &points);
assert!(result.is_none()); // No loop on first observation
assert_eq!(detector.keyframe_count(), 1);
}
#[test]
fn test_keyframe_interval() {
let config = LoopClosureConfig {
keyframe_interval: 1.0,
..Default::default()
};
let mut detector = LoopClosureDetector::new(config);
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
// First keyframe
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 1);
// Too close - no new keyframe
detector.process(Pose2D::new(0.5, 0.0, 0.0), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 1);
// Far enough - new keyframe
detector.process(Pose2D::new(1.5, 0.0, 0.0), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 2);
}
#[test]
fn test_insufficient_features_no_keyframe() {
let mut detector = LoopClosureDetector::default();
let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; // Only 1 line
let corners = vec![]; // No corners
let points = make_simple_points();
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 0); // Not enough features
}
#[test]
fn test_force_keyframe() {
let mut detector = LoopClosureDetector::default();
let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))];
let corners = vec![];
let points = make_simple_points();
let id = detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(id, 0);
assert_eq!(detector.keyframe_count(), 1);
}
#[test]
fn test_get_keyframe() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.force_keyframe(Pose2D::new(1.0, 2.0, 0.5), &lines, &corners, &points);
let kf = detector.get_keyframe(0);
assert!(kf.is_some());
assert_eq!(kf.unwrap().pose.x, 1.0);
assert_eq!(kf.unwrap().pose.y, 2.0);
assert!(detector.get_keyframe(999).is_none());
}
#[test]
fn test_clear() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 1);
detector.clear();
assert_eq!(detector.keyframe_count(), 0);
assert_eq!(detector.total_distance(), 0.0);
}
#[test]
fn test_can_detect_loop() {
let config = LoopClosureConfig {
min_travel_distance: 2.0,
keyframe_interval: 1.0,
..Default::default()
};
let mut detector = LoopClosureDetector::new(config);
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
assert!(!detector.can_detect_loop());
// Add first keyframe
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert!(!detector.can_detect_loop()); // Not traveled enough
// Move far enough
detector.process(Pose2D::new(3.0, 0.0, 0.0), &lines, &corners, &points);
assert!(detector.can_detect_loop());
}
#[test]
fn test_total_distance_tracking() {
let config = LoopClosureConfig {
keyframe_interval: 1.0,
..Default::default()
};
let mut detector = LoopClosureDetector::new(config);
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.total_distance(), 0.0);
// Move 1 meter
detector.process(Pose2D::new(1.0, 0.0, 0.0), &lines, &corners, &points);
assert!((detector.total_distance() - 1.0).abs() < 0.01);
// Move 1 more meter diagonally
detector.process(Pose2D::new(2.0, 1.0, 0.0), &lines, &corners, &points);
let expected = 1.0 + (2.0_f32).sqrt();
assert!((detector.total_distance() - expected).abs() < 0.01);
}
#[test]
fn test_keyframe_descriptor() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points);
let kf = detector.get_keyframe(0).unwrap();
assert!(kf.descriptor.is_valid_for_matching());
assert_eq!(kf.descriptor.num_lines, 4);
assert_eq!(kf.descriptor.num_corners, 4);
}
}
vastu-map/src/loop_closure/mod.rs
+18 -748
View File
@@ -28,751 +28,21 @@
//! println!("Loop closure detected: {} -> {}", closure.from_keyframe, closure.to_keyframe);
//! }
//! ```
use serde::{Deserialize, Serialize};
use crate::core::{Point2D, Pose2D};
use crate::features::{Corner2D, Line2D, ScanDescriptor};
use crate::matching::{IcpConfig, match_scan};
// ─────────────────────────────────────────────────────────────────────────────
// Configuration
// ─────────────────────────────────────────────────────────────────────────────
/// Configuration for loop closure detection.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct LoopClosureConfig {
/// Minimum travel distance before considering loop closure (meters).
/// Prevents detecting loops immediately after keyframe creation.
/// Default: 5.0m
pub min_travel_distance: f32,
/// Maximum descriptor distance for candidate retrieval.
/// Lower values are more selective but may miss valid loops.
/// Default: 0.5
pub max_descriptor_distance: f32,
/// Minimum ICP confidence to accept a loop closure.
/// Default: 0.6
pub min_verification_confidence: f32,
/// Distance between keyframes (meters).
/// Default: 2.0m
pub keyframe_interval: f32,
/// Radius for computing corner descriptors (meters).
/// Default: 2.0m
pub descriptor_radius: f32,
/// Maximum number of candidates to verify with ICP per keyframe.
/// Default: 3
pub max_candidates_to_verify: usize,
/// Minimum number of lines required for a valid keyframe.
/// Default: 3
pub min_lines_for_keyframe: usize,
/// Minimum number of corners required for a valid keyframe.
/// Default: 2
pub min_corners_for_keyframe: usize,
/// ICP configuration for verification.
pub icp_config: IcpConfig,
}
impl Default for LoopClosureConfig {
fn default() -> Self {
Self {
min_travel_distance: 5.0,
max_descriptor_distance: 0.5,
min_verification_confidence: 0.6,
keyframe_interval: 2.0,
descriptor_radius: 2.0,
max_candidates_to_verify: 3,
min_lines_for_keyframe: 3,
min_corners_for_keyframe: 2,
icp_config: IcpConfig::default(),
}
}
}
// ─────────────────────────────────────────────────────────────────────────────
// Keyframe
// ─────────────────────────────────────────────────────────────────────────────
/// A keyframe stores a snapshot of the map at a particular pose.
#[derive(Clone, Debug)]
pub struct Keyframe {
/// Unique keyframe identifier.
pub id: usize,
/// Robot pose when keyframe was created.
pub pose: Pose2D,
/// Scan descriptor for quick matching.
pub descriptor: ScanDescriptor,
/// Line features in world frame.
pub lines: Vec<Line2D>,
/// Corner features in world frame.
pub corners: Vec<Corner2D>,
/// Points in robot frame (for ICP verification).
pub points: Vec<Point2D>,
/// Distance traveled when keyframe was created.
pub distance_traveled: f32,
}
impl Keyframe {
/// Create a new keyframe.
pub fn new(
id: usize,
pose: Pose2D,
lines: Vec<Line2D>,
corners: Vec<Corner2D>,
points: Vec<Point2D>,
distance_traveled: f32,
descriptor_radius: f32,
) -> Self {
let descriptor = ScanDescriptor::compute(&lines, &corners, descriptor_radius);
Self {
id,
pose,
descriptor,
lines,
corners,
points,
distance_traveled,
}
}
}
// ─────────────────────────────────────────────────────────────────────────────
// Loop Closure Result
// ─────────────────────────────────────────────────────────────────────────────
/// A detected loop closure constraint.
#[derive(Clone, Debug)]
pub struct LoopClosure {
/// Index of the earlier keyframe.
pub from_keyframe: usize,
/// Index of the later keyframe (current).
pub to_keyframe: usize,
/// Relative pose from `from_keyframe` to `to_keyframe`.
pub relative_pose: Pose2D,
/// Covariance matrix of the relative pose estimate.
/// Format: [[σxx, σxy, σxθ], [σxy, σyy, σyθ], [σxθ, σyθ, σθθ]]
pub covariance: [[f32; 3]; 3],
/// Confidence of the loop closure (0.0 to 1.0).
pub confidence: f32,
/// Descriptor distance used for candidate retrieval.
pub descriptor_distance: f32,
}
// ─────────────────────────────────────────────────────────────────────────────
// Loop Closure Detector
// ─────────────────────────────────────────────────────────────────────────────
/// Loop closure detector using keyframe-based matching.
pub struct LoopClosureDetector {
/// Stored keyframes.
keyframes: Vec<Keyframe>,
/// Configuration.
config: LoopClosureConfig,
/// Last pose where a keyframe was created.
last_keyframe_pose: Option<Pose2D>,
/// Total distance traveled.
total_distance: f32,
/// Next keyframe ID.
next_keyframe_id: usize,
}
impl LoopClosureDetector {
/// Create a new loop closure detector.
pub fn new(config: LoopClosureConfig) -> Self {
Self {
keyframes: Vec::new(),
config,
last_keyframe_pose: None,
total_distance: 0.0,
next_keyframe_id: 0,
}
}
/// Get the configuration.
pub fn config(&self) -> &LoopClosureConfig {
&self.config
}
/// Get all keyframes.
pub fn keyframes(&self) -> &[Keyframe] {
&self.keyframes
}
/// Get the number of keyframes.
pub fn keyframe_count(&self) -> usize {
self.keyframes.len()
}
/// Get the total distance traveled.
pub fn total_distance(&self) -> f32 {
self.total_distance
}
/// Clear all keyframes and reset state.
pub fn clear(&mut self) {
self.keyframes.clear();
self.last_keyframe_pose = None;
self.total_distance = 0.0;
self.next_keyframe_id = 0;
}
/// Process a new observation and check for loop closures.
///
/// # Arguments
/// * `current_pose` - Current robot pose in world frame
/// * `lines` - Extracted lines in world frame
/// * `corners` - Extracted corners in world frame
/// * `points` - Scan points in robot frame (for ICP verification)
///
/// # Returns
/// A loop closure if one is detected, None otherwise.
pub fn process(
&mut self,
current_pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure> {
// Update distance traveled
if let Some(last_pose) = self.last_keyframe_pose {
let dx = current_pose.x - last_pose.x;
let dy = current_pose.y - last_pose.y;
self.total_distance += (dx * dx + dy * dy).sqrt();
}
// Check if we need to create a keyframe
let should_create_keyframe = self.should_create_keyframe(current_pose, lines, corners);
let mut loop_closure = None;
if should_create_keyframe {
// First check for loop closure candidates before adding keyframe
loop_closure = self.find_loop_closure(current_pose, lines, corners, points);
// Create and store keyframe
let keyframe = Keyframe::new(
self.next_keyframe_id,
current_pose,
lines.to_vec(),
corners.to_vec(),
points.to_vec(),
self.total_distance,
self.config.descriptor_radius,
);
self.keyframes.push(keyframe);
self.next_keyframe_id += 1;
self.last_keyframe_pose = Some(current_pose);
}
loop_closure
}
/// Check if a new keyframe should be created.
fn should_create_keyframe(
&self,
current_pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
) -> bool {
// Need minimum features
if lines.len() < self.config.min_lines_for_keyframe {
return false;
}
if corners.len() < self.config.min_corners_for_keyframe {
return false;
}
// First keyframe
if self.last_keyframe_pose.is_none() {
return true;
}
// Check distance since last keyframe
let last_pose = self.last_keyframe_pose.unwrap();
let dx = current_pose.x - last_pose.x;
let dy = current_pose.y - last_pose.y;
let distance = (dx * dx + dy * dy).sqrt();
distance >= self.config.keyframe_interval
}
/// Find loop closure candidates and verify with ICP.
fn find_loop_closure(
&self,
current_pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure> {
// Need enough keyframes for loop closure
if self.keyframes.is_empty() {
return None;
}
// Compute descriptor for current observation
let current_descriptor =
ScanDescriptor::compute(lines, corners, self.config.descriptor_radius);
if !current_descriptor.is_valid_for_matching() {
return None;
}
// Find candidates using descriptor distance
let mut candidates: Vec<(usize, f32)> = self
.keyframes
.iter()
.enumerate()
.filter_map(|(idx, kf)| {
// Skip recent keyframes (min travel distance)
let travel_since_kf = self.total_distance - kf.distance_traveled;
if travel_since_kf < self.config.min_travel_distance {
return None;
}
// Check descriptor validity
if !kf.descriptor.is_valid_for_matching() {
return None;
}
// Quick signature pre-filter
let sig_diff = (current_descriptor.quick_signature()
^ kf.descriptor.quick_signature())
.count_ones();
if sig_diff > 16 {
return None; // Too different
}
// Full descriptor distance
let dist = current_descriptor.distance(&kf.descriptor);
if dist <= self.config.max_descriptor_distance {
Some((idx, dist))
} else {
None
}
})
.collect();
// Sort by descriptor distance (best first)
candidates.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));
// Verify top candidates with ICP
for (kf_idx, descriptor_dist) in
candidates.iter().take(self.config.max_candidates_to_verify)
{
let keyframe = &self.keyframes[*kf_idx];
// Compute initial guess: relative pose from keyframe to current
let initial_guess = keyframe.pose.inverse().compose(current_pose);
// Transform current points to keyframe frame
let transformed_points: Vec<Point2D> = points
.iter()
.map(|p| {
// Transform from current robot frame to world, then to keyframe frame
let world = current_pose.transform_point(*p);
keyframe.pose.inverse().transform_point(world)
})
.collect();
// Run ICP verification
let match_result = match_scan(&transformed_points, &keyframe.lines, initial_guess);
if match_result.converged
&& match_result.confidence >= self.config.min_verification_confidence
{
// Found a valid loop closure
let relative_pose = keyframe.pose.inverse().compose(current_pose);
return Some(LoopClosure {
from_keyframe: keyframe.id,
to_keyframe: self.next_keyframe_id,
relative_pose,
covariance: match_result.covariance,
confidence: match_result.confidence,
descriptor_distance: *descriptor_dist,
});
}
}
None
}
/// Force add a keyframe at the current position.
/// Useful for initialization or when loop closure should be checked
/// regardless of distance traveled.
pub fn force_keyframe(
&mut self,
pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> usize {
let keyframe = Keyframe::new(
self.next_keyframe_id,
pose,
lines.to_vec(),
corners.to_vec(),
points.to_vec(),
self.total_distance,
self.config.descriptor_radius,
);
let id = self.next_keyframe_id;
self.keyframes.push(keyframe);
self.next_keyframe_id += 1;
self.last_keyframe_pose = Some(pose);
id
}
/// Get a keyframe by ID.
pub fn get_keyframe(&self, id: usize) -> Option<&Keyframe> {
self.keyframes.iter().find(|kf| kf.id == id)
}
/// Check if loop closure detection is possible.
/// Returns true if there are enough keyframes and sufficient travel distance.
pub fn can_detect_loop(&self) -> bool {
if self.keyframes.is_empty() {
return false;
}
// Check if we've traveled enough since any keyframe
self.keyframes.iter().any(|kf| {
let travel_since = self.total_distance - kf.distance_traveled;
travel_since >= self.config.min_travel_distance
})
}
}
impl Default for LoopClosureDetector {
fn default() -> Self {
Self::new(LoopClosureConfig::default())
}
}
// ─────────────────────────────────────────────────────────────────────────────
// LoopDetector Trait
// ─────────────────────────────────────────────────────────────────────────────
/// Trait for loop closure detection algorithms.
///
/// Implementations detect when the robot revisits a previously mapped area,
/// providing constraints for pose graph optimization.
///
/// # Example
///
/// ```rust,ignore
/// use vastu_map::loop_closure::{LoopDetector, LoopClosureDetector, LoopClosureConfig};
/// use vastu_map::core::Pose2D;
///
/// let mut detector = LoopClosureDetector::new(LoopClosureConfig::default());
///
/// if let Some(closure) = detector.detect(pose, &lines, &corners, &points) {
/// println!("Loop detected with confidence {}", closure.confidence);
/// }
/// ```
pub trait LoopDetector: Send + Sync {
/// Process an observation and detect potential loop closures.
///
/// # Arguments
/// * `pose` - Current robot pose in world frame
/// * `lines` - Extracted lines in world frame
/// * `corners` - Extracted corners in world frame
/// * `points` - Scan points in robot frame (for ICP verification)
///
/// # Returns
/// A loop closure if one is detected, None otherwise.
fn detect(
&mut self,
pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure>;
/// Clear all stored state (keyframes, history, etc.).
fn clear(&mut self);
/// Check if loop detection is currently possible.
fn can_detect(&self) -> bool;
/// Get the number of stored keyframes.
fn keyframe_count(&self) -> usize;
}
impl LoopDetector for LoopClosureDetector {
fn detect(
&mut self,
pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure> {
self.process(pose, lines, corners, points)
}
fn clear(&mut self) {
LoopClosureDetector::clear(self)
}
fn can_detect(&self) -> bool {
self.can_detect_loop()
}
fn keyframe_count(&self) -> usize {
self.keyframes.len()
}
}
// ─────────────────────────────────────────────────────────────────────────────
// Tests
// ─────────────────────────────────────────────────────────────────────────────
#[cfg(test)]
mod tests {
use super::*;
fn make_simple_lines() -> Vec<Line2D> {
vec![
Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(2.0, 0.0)),
Line2D::new(Point2D::new(2.0, 0.0), Point2D::new(2.0, 2.0)),
Line2D::new(Point2D::new(2.0, 2.0), Point2D::new(0.0, 2.0)),
Line2D::new(Point2D::new(0.0, 2.0), Point2D::new(0.0, 0.0)),
]
}
fn make_simple_corners() -> Vec<Corner2D> {
vec![
Corner2D::new(Point2D::new(0.0, 0.0), 3, 0, std::f32::consts::FRAC_PI_2),
Corner2D::new(Point2D::new(2.0, 0.0), 0, 1, std::f32::consts::FRAC_PI_2),
Corner2D::new(Point2D::new(2.0, 2.0), 1, 2, std::f32::consts::FRAC_PI_2),
Corner2D::new(Point2D::new(0.0, 2.0), 2, 3, std::f32::consts::FRAC_PI_2),
]
}
fn make_simple_points() -> Vec<Point2D> {
let mut points = Vec::new();
// Wall points
for i in 0..20 {
let t = i as f32 / 19.0 * 2.0;
points.push(Point2D::new(t, 0.0));
points.push(Point2D::new(t, 2.0));
points.push(Point2D::new(0.0, t));
points.push(Point2D::new(2.0, t));
}
points
}
#[test]
fn test_config_default() {
let config = LoopClosureConfig::default();
assert_eq!(config.min_travel_distance, 5.0);
assert_eq!(config.max_descriptor_distance, 0.5);
assert_eq!(config.min_verification_confidence, 0.6);
assert_eq!(config.keyframe_interval, 2.0);
}
#[test]
fn test_detector_new() {
let detector = LoopClosureDetector::default();
assert_eq!(detector.keyframe_count(), 0);
assert_eq!(detector.total_distance(), 0.0);
assert!(!detector.can_detect_loop());
}
#[test]
fn test_first_keyframe_created() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
let result = detector.process(Pose2D::identity(), &lines, &corners, &points);
assert!(result.is_none()); // No loop on first observation
assert_eq!(detector.keyframe_count(), 1);
}
#[test]
fn test_keyframe_interval() {
let config = LoopClosureConfig {
keyframe_interval: 1.0,
..Default::default()
};
let mut detector = LoopClosureDetector::new(config);
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
// First keyframe
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 1);
// Too close - no new keyframe
detector.process(Pose2D::new(0.5, 0.0, 0.0), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 1);
// Far enough - new keyframe
detector.process(Pose2D::new(1.5, 0.0, 0.0), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 2);
}
#[test]
fn test_insufficient_features_no_keyframe() {
let mut detector = LoopClosureDetector::default();
let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))]; // Only 1 line
let corners = vec![]; // No corners
let points = make_simple_points();
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 0); // Not enough features
}
#[test]
fn test_force_keyframe() {
let mut detector = LoopClosureDetector::default();
let lines = vec![Line2D::new(Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0))];
let corners = vec![];
let points = make_simple_points();
let id = detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(id, 0);
assert_eq!(detector.keyframe_count(), 1);
}
#[test]
fn test_get_keyframe() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.force_keyframe(Pose2D::new(1.0, 2.0, 0.5), &lines, &corners, &points);
let kf = detector.get_keyframe(0);
assert!(kf.is_some());
assert_eq!(kf.unwrap().pose.x, 1.0);
assert_eq!(kf.unwrap().pose.y, 2.0);
assert!(detector.get_keyframe(999).is_none());
}
#[test]
fn test_clear() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.keyframe_count(), 1);
detector.clear();
assert_eq!(detector.keyframe_count(), 0);
assert_eq!(detector.total_distance(), 0.0);
}
#[test]
fn test_can_detect_loop() {
let config = LoopClosureConfig {
min_travel_distance: 2.0,
keyframe_interval: 1.0,
..Default::default()
};
let mut detector = LoopClosureDetector::new(config);
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
assert!(!detector.can_detect_loop());
// Add first keyframe
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert!(!detector.can_detect_loop()); // Not traveled enough
// Move far enough
detector.process(Pose2D::new(3.0, 0.0, 0.0), &lines, &corners, &points);
assert!(detector.can_detect_loop());
}
#[test]
fn test_total_distance_tracking() {
let config = LoopClosureConfig {
keyframe_interval: 1.0,
..Default::default()
};
let mut detector = LoopClosureDetector::new(config);
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.process(Pose2D::identity(), &lines, &corners, &points);
assert_eq!(detector.total_distance(), 0.0);
// Move 1 meter
detector.process(Pose2D::new(1.0, 0.0, 0.0), &lines, &corners, &points);
assert!((detector.total_distance() - 1.0).abs() < 0.01);
// Move 1 more meter diagonally
detector.process(Pose2D::new(2.0, 1.0, 0.0), &lines, &corners, &points);
let expected = 1.0 + (2.0_f32).sqrt();
assert!((detector.total_distance() - expected).abs() < 0.01);
}
#[test]
fn test_keyframe_descriptor() {
let mut detector = LoopClosureDetector::default();
let lines = make_simple_lines();
let corners = make_simple_corners();
let points = make_simple_points();
detector.force_keyframe(Pose2D::identity(), &lines, &corners, &points);
let kf = detector.get_keyframe(0).unwrap();
assert!(kf.descriptor.is_valid_for_matching());
assert_eq!(kf.descriptor.num_lines, 4);
assert_eq!(kf.descriptor.num_corners, 4);
}
#[test]
fn test_loop_closure_struct() {
let closure = LoopClosure {
from_keyframe: 0,
to_keyframe: 5,
relative_pose: Pose2D::new(1.0, 2.0, 0.5),
covariance: [[0.01, 0.0, 0.0], [0.0, 0.01, 0.0], [0.0, 0.0, 0.001]],
confidence: 0.85,
descriptor_distance: 0.3,
};
assert_eq!(closure.from_keyframe, 0);
assert_eq!(closure.to_keyframe, 5);
assert_eq!(closure.confidence, 0.85);
}
}
//!
//! # Module Structure
//!
//! - [`config`]: Configuration for loop closure detection
//! - [`types`]: Core types (Keyframe, LoopClosure)
//! - [`detector`]: LoopClosureDetector implementation
//! - [`traits`]: LoopDetector trait for custom implementations
mod config;
mod detector;
mod traits;
mod types;
// Re-export public API
pub use config::LoopClosureConfig;
pub use detector::LoopClosureDetector;
pub use traits::LoopDetector;
pub use types::{Keyframe, LoopClosure};
vastu-map/src/loop_closure/traits.rs
+77
View File
@@ -0,0 +1,77 @@
//! Trait definitions for loop closure detection.
use crate::core::{Point2D, Pose2D};
use crate::features::{Corner2D, Line2D};
use super::detector::LoopClosureDetector;
use super::types::LoopClosure;
/// Trait for loop closure detection algorithms.
///
/// Implementations detect when the robot revisits a previously mapped area,
/// providing constraints for pose graph optimization.
///
/// # Example
///
/// ```rust,ignore
/// use vastu_map::loop_closure::{LoopDetector, LoopClosureDetector, LoopClosureConfig};
/// use vastu_map::core::Pose2D;
///
/// let mut detector = LoopClosureDetector::new(LoopClosureConfig::default());
///
/// if let Some(closure) = detector.detect(pose, &lines, &corners, &points) {
/// println!("Loop detected with confidence {}", closure.confidence);
/// }
/// ```
pub trait LoopDetector: Send + Sync {
/// Process an observation and detect potential loop closures.
///
/// # Arguments
/// * `pose` - Current robot pose in world frame
/// * `lines` - Extracted lines in world frame
/// * `corners` - Extracted corners in world frame
/// * `points` - Scan points in robot frame (for ICP verification)
///
/// # Returns
/// A loop closure if one is detected, None otherwise.
fn detect(
&mut self,
pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure>;
/// Clear all stored state (keyframes, history, etc.).
fn clear(&mut self);
/// Check if loop detection is currently possible.
fn can_detect(&self) -> bool;
/// Get the number of stored keyframes.
fn keyframe_count(&self) -> usize;
}
impl LoopDetector for LoopClosureDetector {
fn detect(
&mut self,
pose: Pose2D,
lines: &[Line2D],
corners: &[Corner2D],
points: &[Point2D],
) -> Option<LoopClosure> {
self.process(pose, lines, corners, points)
}
fn clear(&mut self) {
LoopClosureDetector::clear(self)
}
fn can_detect(&self) -> bool {
self.can_detect_loop()
}
fn keyframe_count(&self) -> usize {
self.keyframes().len()
}
}
vastu-map/src/loop_closure/types.rs
+97
View File
@@ -0,0 +1,97 @@
//! Core types for loop closure detection.
use crate::core::{Point2D, Pose2D};
use crate::features::{Corner2D, Line2D, ScanDescriptor};
/// A keyframe stores a snapshot of the map at a particular pose.
#[derive(Clone, Debug)]
pub struct Keyframe {
/// Unique keyframe identifier.
pub id: usize,
/// Robot pose when keyframe was created.
pub pose: Pose2D,
/// Scan descriptor for quick matching.
pub descriptor: ScanDescriptor,
/// Line features in world frame.
pub lines: Vec<Line2D>,
/// Corner features in world frame.
pub corners: Vec<Corner2D>,
/// Points in robot frame (for ICP verification).
pub points: Vec<Point2D>,
/// Distance traveled when keyframe was created.
pub distance_traveled: f32,
}
impl Keyframe {
/// Create a new keyframe.
pub fn new(
id: usize,
pose: Pose2D,
lines: Vec<Line2D>,
corners: Vec<Corner2D>,
points: Vec<Point2D>,
distance_traveled: f32,
descriptor_radius: f32,
) -> Self {
let descriptor = ScanDescriptor::compute(&lines, &corners, descriptor_radius);
Self {
id,
pose,
descriptor,
lines,
corners,
points,
distance_traveled,
}
}
}
/// A detected loop closure constraint.
#[derive(Clone, Debug)]
pub struct LoopClosure {
/// Index of the earlier keyframe.
pub from_keyframe: usize,
/// Index of the later keyframe (current).
pub to_keyframe: usize,
/// Relative pose from `from_keyframe` to `to_keyframe`.
pub relative_pose: Pose2D,
/// Covariance matrix of the relative pose estimate.
/// Format: [[σxx, σxy, σxθ], [σxy, σyy, σyθ], [σxθ, σyθ, σθθ]]
pub covariance: [[f32; 3]; 3],
/// Confidence of the loop closure (0.0 to 1.0).
pub confidence: f32,
/// Descriptor distance used for candidate retrieval.
pub descriptor_distance: f32,
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_loop_closure_struct() {
let closure = LoopClosure {
from_keyframe: 0,
to_keyframe: 5,
relative_pose: Pose2D::new(1.0, 2.0, 0.5),
covariance: [[0.01, 0.0, 0.0], [0.0, 0.01, 0.0], [0.0, 0.0, 0.001]],
confidence: 0.85,
descriptor_distance: 0.3,
};
assert_eq!(closure.from_keyframe, 0);
assert_eq!(closure.to_keyframe, 5);
assert_eq!(closure.confidence, 0.85);
}
}
vastu-map/tests/integration/harness.rs
+2 -11
View File
@@ -247,8 +247,6 @@ pub struct ObservationRecord {
pub confidence: f32,
/// Drift magnitude (translation error in meters).
pub drift_translation: f32,
/// Lidar scan at this observation (robot frame) - stored at intervals for visualization.
pub scan: Option<vastu_map::core::PointCloud2D>,
}
/// Trajectory history for visualization.
@@ -265,13 +263,7 @@ impl TrajectoryHistory {
}
/// Record an observation.
pub fn record(
&mut self,
truth_pose: Pose2D,
slam_pose: Pose2D,
confidence: f32,
scan: Option<vastu_map::core::PointCloud2D>,
) {
pub fn record(&mut self, truth_pose: Pose2D, slam_pose: Pose2D, confidence: f32) {
let drift_translation =
((truth_pose.x - slam_pose.x).powi(2) + (truth_pose.y - slam_pose.y).powi(2)).sqrt();
@@ -280,7 +272,6 @@ impl TrajectoryHistory {
slam_pose,
confidence,
drift_translation,
scan,
});
}
}
@@ -536,7 +527,7 @@ impl TestHarness {
// Record to trajectory history for visualization
// Note: We no longer store scans here since ScanStore handles it
self.trajectory
.record(current_truth, result.pose, result.confidence, None);
.record(current_truth, result.pose, result.confidence);
log::debug!(
"Observation {}: truth=({:.2}, {:.2}, {:.1}°), slam=({:.2}, {:.2}, {:.1}°), conf={:.2}, icp_iters={}",