codetiger
0980b31e77
Major improvements to the Clearance-Based Visibility Graph (CBVG) system: CBVG Graph: - Replace grid-based node generation with ray-based placement along lidar scans - Add angular gap analysis for frontier detection (nodes with uncovered sectors) - Add clique optimization for merging redundant nodes - Add path planning with node exclusion for alternative approach angles - Increase max_nodes to 5000 for large multi-room layouts - Add configurable max_lidar_range for node creation Exploration Controller: - Track CBVG goal nodes that led to collision for each frontier - Add oscillation detection and frontier suppression - Implement multi-attempt collision recovery before marking unreachable - Plan alternative paths by excluding previously failed goal nodes - Handle collisions during recovery state Vector Map: - Add get_path_excluding_nodes() for alternative approach planning - Integrate CBVG-based frontier detection with configurable parameters Other: - Scan module: Include max-range readings for open-space node creation - Example: Add command-line map file selection - Temporarily disable release optimizations for debugging
269 lines
9.2 KiB
YAML
269 lines
9.2 KiB
YAML
# VastuMap YAML Configuration
|
|
# All values must be specified (no preset inheritance)
|
|
# Units: meters (m), radians (rad), counts (n)
|
|
|
|
# ============================================================================
|
|
# Line Extraction (Split-Merge Algorithm)
|
|
# ============================================================================
|
|
extraction:
|
|
# Maximum perpendicular distance for splitting (m)
|
|
split_threshold: 0.05
|
|
# Minimum points to form a valid line
|
|
min_points: 5
|
|
# Minimum line length to keep (m)
|
|
min_length: 0.05
|
|
# Maximum gap between consecutive points (m)
|
|
max_point_gap: 0.10
|
|
# Merge threshold for collinear segments (m)
|
|
merge_threshold: 0.05
|
|
# Coefficient for range-adaptive threshold scaling
|
|
adaptive_range_scale: 0.03
|
|
|
|
# ============================================================================
|
|
# RANSAC Line Extraction (for hybrid mode)
|
|
# ============================================================================
|
|
ransac_extraction:
|
|
# Number of RANSAC iterations per line
|
|
iterations: 100
|
|
# Inlier distance threshold (m)
|
|
inlier_threshold: 0.02
|
|
# Minimum inliers for valid line
|
|
min_inliers: 5
|
|
# Minimum line length (m)
|
|
min_line_length: 0.1
|
|
# Maximum gap between inliers (m) - prevents connecting walls across doorways
|
|
max_point_gap: 0.10
|
|
# Maximum lines to extract (0 = unlimited)
|
|
max_lines: 0
|
|
# Random seed (0 = random)
|
|
seed: 0
|
|
|
|
# Whether to use RANSAC + Split-Merge hybrid extraction
|
|
use_hybrid_extraction: true
|
|
|
|
# ============================================================================
|
|
# Corner Detection
|
|
# ============================================================================
|
|
corner:
|
|
# Minimum angle between lines to form corner (rad)
|
|
min_angle: 0.524 # 30 degrees
|
|
# Maximum angle between lines to form corner (rad)
|
|
max_angle: 2.618 # 150 degrees
|
|
# Maximum endpoint distance for corner detection (m)
|
|
max_endpoint_distance: 0.05
|
|
# Curvature detection window size (points on each side)
|
|
curvature_window_size: 3
|
|
# Minimum curvature angle for corner candidate (rad)
|
|
curvature_threshold: 0.785 # 45 degrees
|
|
# Merge distance for deduplicating corners (m)
|
|
merge_distance: 0.10
|
|
# Non-maximum suppression radius (m)
|
|
nms_radius: 0.15
|
|
|
|
# ============================================================================
|
|
# ICP Scan Matching
|
|
# ============================================================================
|
|
matching:
|
|
# Maximum ICP iterations
|
|
max_iterations: 30
|
|
# Convergence threshold for pose change
|
|
convergence_threshold: 0.0001
|
|
# Maximum correspondence distance (m)
|
|
max_correspondence_distance: 0.5
|
|
|
|
# Robust cost function for outlier handling
|
|
# Options: None, Huber, Cauchy, Tukey, GemanMcClure
|
|
robust_cost:
|
|
type: Huber
|
|
delta: 0.03
|
|
|
|
# Use Iteratively Reweighted Least Squares
|
|
use_irls: true
|
|
# Minimum correspondences required
|
|
min_correspondences: 10
|
|
# Minimum match confidence to accept
|
|
min_confidence: 0.3
|
|
# Use RANSAC for initial pose estimate
|
|
use_ransac_init: true
|
|
# Use batch (SIMD) correspondence search
|
|
use_batch_search: true
|
|
|
|
# Lidar noise model
|
|
noise_model:
|
|
sigma_base: 0.01
|
|
sigma_range: 0.001
|
|
|
|
# Coarse search when confidence is low
|
|
use_coarse_search: true
|
|
coarse_search_confidence_threshold: 0.5
|
|
|
|
# Multi-resolution ICP (coarse-to-fine)
|
|
multi_resolution:
|
|
enabled: true
|
|
levels: 3
|
|
subsample_factor: 2
|
|
iterations_per_level: [3, 5, 10]
|
|
|
|
# ============================================================================
|
|
# Line Association (Map Integration)
|
|
# ============================================================================
|
|
association:
|
|
# Maximum perpendicular distance for association (m)
|
|
max_perpendicular_distance: 0.15
|
|
# Maximum angular difference (rad)
|
|
max_angle_difference: 0.2
|
|
# Minimum overlap ratio for valid association
|
|
min_overlap_ratio: 0.3
|
|
# Maximum gap between line endpoints (m)
|
|
max_endpoint_gap: 0.3
|
|
# Weight for distance in quality scoring
|
|
distance_weight: 10.0
|
|
# Weight for angle in quality scoring
|
|
angle_weight: 5.0
|
|
|
|
# ============================================================================
|
|
# Line Merging
|
|
# ============================================================================
|
|
merger:
|
|
# Weight given to existing map line (0-1)
|
|
map_weight: 0.8
|
|
# Whether to extend line endpoints
|
|
extend_endpoints: true
|
|
# Maximum extension per observation (m)
|
|
max_extension: 0.5
|
|
# Minimum extension to apply (m)
|
|
min_extension: 0.05
|
|
|
|
# ============================================================================
|
|
# Frontier Detection (Gap-Based)
|
|
# ============================================================================
|
|
frontier:
|
|
# Maximum endpoint distance for "connected" (m)
|
|
connection_distance: 0.3
|
|
# Minimum distance from robot for valid frontier (m)
|
|
min_distance_from_robot: 1.0
|
|
# Safety margin: viewpoint offset from wall (m)
|
|
safety_margin: 0.4
|
|
# Gap probe distance: how far to probe for unexplored gaps (m)
|
|
gap_probe_distance: 2.0
|
|
# Robot radius for viewpoint validation (m)
|
|
robot_radius: 0.16
|
|
# Maximum iterations for viewpoint adjustment
|
|
max_viewpoint_iterations: 5
|
|
# Viewpoint adjustment step size (m)
|
|
viewpoint_step: 0.1
|
|
# Minimum gap width for safe robot passage (m)
|
|
# Gaps narrower than this are ignored (robot can't fit)
|
|
min_gap_width: 0.4
|
|
# Maximum gap width to consider as a single gap (m)
|
|
# Allow gaps across rooms (up to 10m)
|
|
max_gap_width: 10.0
|
|
# CBVG-based frontier detection parameters
|
|
# Minimum uncovered angle for a node to be a frontier (radians)
|
|
# 1.75 rad = 100 degrees
|
|
min_frontier_angle: 1.75
|
|
# Maximum distance to check for walls when detecting frontiers (m)
|
|
max_wall_distance: 0.5
|
|
|
|
# ============================================================================
|
|
# Occupancy Queries
|
|
# ============================================================================
|
|
occupancy:
|
|
# Distance threshold for "occupied" (m)
|
|
obstacle_distance: 0.1
|
|
# Distance threshold for "known" (m)
|
|
known_distance: 2.0
|
|
|
|
# ============================================================================
|
|
# Clearance-Based Visibility Graph (CBVG) - Path Planning
|
|
# ============================================================================
|
|
cbvg:
|
|
# Minimum clearance from walls in open areas (m)
|
|
min_clearance: 0.3
|
|
# Minimum clearance in narrow passages (m)
|
|
# Used when corridor is too narrow for full clearance
|
|
min_clearance_narrow: 0.18
|
|
# Sample resolution along Voronoi edges (m)
|
|
voronoi_sample_step: 0.1
|
|
# Maximum edge length between nodes (m)
|
|
# Increased from 0.8 to 1.5 for better graph connectivity during exploration
|
|
max_edge_length: 1.5
|
|
# Interval for placing coverage nodes in open areas (m)
|
|
# Ensures nodes exist even in areas without medial axis
|
|
coverage_node_interval: 0.35
|
|
# Distance threshold for merging nearby nodes (m)
|
|
node_merge_distance: 0.15
|
|
# Maximum nodes in the graph
|
|
max_nodes: 500
|
|
# Clearance sample step for edge validation (m)
|
|
edge_clearance_step: 0.05
|
|
# Wall sampling step for Voronoi approximation (m)
|
|
wall_sample_step: 0.1
|
|
# Minimum clique size for merging during optimization
|
|
# Cliques (mutually visible node groups) smaller than this are kept separate
|
|
min_clique_size_for_merge: 3
|
|
# Number of lidar scans between clique optimization runs
|
|
# Set to 1 to optimize after every scan for best graph quality
|
|
optimize_interval: 1
|
|
# Maximum range for CBVG node creation (m)
|
|
# Nodes are only placed up to this distance from robot
|
|
# (wall detection still uses full lidar range)
|
|
max_lidar_range: 1.0
|
|
|
|
# ============================================================================
|
|
# Loop Closure Detection
|
|
# ============================================================================
|
|
loop_closure:
|
|
# Minimum travel distance before checking loop closure (m)
|
|
min_travel_distance: 5.0
|
|
# Maximum descriptor distance for candidates
|
|
max_descriptor_distance: 0.5
|
|
# Minimum ICP confidence to accept closure
|
|
min_verification_confidence: 0.6
|
|
# Distance between keyframes (m)
|
|
keyframe_interval: 2.0
|
|
# Radius for computing descriptors (m)
|
|
descriptor_radius: 2.0
|
|
# Maximum candidates to verify per keyframe
|
|
max_candidates_to_verify: 3
|
|
# Minimum lines for valid keyframe
|
|
min_lines_for_keyframe: 3
|
|
# Minimum corners for valid keyframe
|
|
min_corners_for_keyframe: 2
|
|
|
|
# ICP config for loop closure verification (nested)
|
|
icp_config:
|
|
max_iterations: 30
|
|
convergence_threshold: 0.0001
|
|
max_correspondence_distance: 0.5
|
|
robust_cost:
|
|
type: Huber
|
|
delta: 0.03
|
|
use_irls: true
|
|
min_correspondences: 10
|
|
min_confidence: 0.3
|
|
use_ransac_init: true
|
|
use_batch_search: true
|
|
noise_model:
|
|
sigma_base: 0.01
|
|
sigma_range: 0.001
|
|
use_coarse_search: true
|
|
coarse_search_confidence_threshold: 0.5
|
|
multi_resolution:
|
|
enabled: true
|
|
levels: 3
|
|
subsample_factor: 2
|
|
iterations_per_level: [3, 5, 10]
|
|
|
|
# ============================================================================
|
|
# Global Settings
|
|
# ============================================================================
|
|
# Minimum match confidence to use scan matching (vs. odometry)
|
|
min_match_confidence: 0.3
|
|
|
|
# Whether to update map with new observations
|
|
mapping_enabled: true
|
|
|
|
# Whether loop closure detection is enabled
|
|
loop_closure_enabled: true
|