mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 21:46:36 +00:00
add features
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,125 @@
|
||||
# Feature: Core Factor Management
|
||||
|
||||
## Name
|
||||
Core Factor Management
|
||||
|
||||
## Description
|
||||
Provides the fundamental building blocks for factor graph construction by adding relative pose measurements (from VO), absolute GPS measurements (from LiteSAM or user anchors), and altitude priors. Handles scale resolution for monocular VO via GSD computation and applies robust kernels (Huber/Cauchy) for outlier handling.
|
||||
|
||||
## Component APIs Implemented
|
||||
- `add_relative_factor(flight_id, frame_i, frame_j, relative_pose, covariance) -> bool`
|
||||
- `add_absolute_factor(flight_id, frame_id, gps, covariance, is_user_anchor) -> bool`
|
||||
- `add_altitude_prior(flight_id, frame_id, altitude, covariance) -> bool`
|
||||
|
||||
## External Tools and Services
|
||||
- **GTSAM**: BetweenFactor, PriorFactor, UnaryFactor creation
|
||||
- **H02 GSD Calculator**: Scale resolution for monocular VO (GSD computation)
|
||||
- **H03 Robust Kernels**: Huber/Cauchy loss functions for outlier handling
|
||||
- **F17 Configuration Manager**: Camera parameters, altitude, frame spacing
|
||||
|
||||
## Internal Methods
|
||||
|
||||
### `_scale_relative_translation(flight_id, translation, frame_i, frame_j) -> np.ndarray`
|
||||
Scales unit translation vector from VO using GSD and expected displacement.
|
||||
- Retrieves altitude and camera params from F17
|
||||
- Calls H02.compute_gsd() to get ground sampling distance
|
||||
- Computes expected_displacement = frame_spacing × GSD
|
||||
- Returns scaled_translation = unit_translation × expected_displacement
|
||||
|
||||
### `_create_between_factor(frame_i, frame_j, pose, covariance) -> gtsam.BetweenFactor`
|
||||
Creates GTSAM BetweenFactor for relative pose measurement.
|
||||
- Creates Pose3 from translation and rotation
|
||||
- Wraps with robust kernel (Huber) for outlier handling
|
||||
- Returns configured factor
|
||||
|
||||
### `_create_prior_factor(frame_id, position, covariance, is_hard_constraint) -> gtsam.PriorFactor`
|
||||
Creates GTSAM PriorFactor for absolute position constraint.
|
||||
- Converts GPS (lat/lon) to ENU coordinates
|
||||
- Sets covariance based on source (5m for user anchor, 20-50m for LiteSAM)
|
||||
- Returns configured factor
|
||||
|
||||
### `_create_altitude_factor(frame_id, altitude, covariance) -> gtsam.UnaryFactor`
|
||||
Creates GTSAM UnaryFactor for Z-coordinate constraint.
|
||||
- Creates soft constraint on altitude
|
||||
- Prevents scale drift in monocular VO
|
||||
|
||||
### `_apply_robust_kernel(factor, kernel_type, threshold) -> gtsam.Factor`
|
||||
Wraps factor with robust kernel for outlier handling.
|
||||
- Supports Huber (default) and Cauchy kernels
|
||||
- Critical for 350m outlier handling
|
||||
|
||||
### `_gps_to_enu(gps, reference_origin) -> np.ndarray`
|
||||
Converts GPS coordinates to local ENU (East-North-Up) frame.
|
||||
- Uses reference origin (first frame or flight start GPS)
|
||||
- Returns (x, y, z) in meters
|
||||
|
||||
### `_ensure_graph_initialized(flight_id) -> bool`
|
||||
Ensures factor graph exists for flight, creates if needed.
|
||||
- Creates new GTSAM iSAM2 instance if not exists
|
||||
- Initializes reference origin for ENU conversion
|
||||
|
||||
## Unit Tests
|
||||
|
||||
### Test: add_relative_factor_scales_translation
|
||||
- Arrange: Mock H02, F17 with known GSD values
|
||||
- Act: Call add_relative_factor with unit translation
|
||||
- Assert: Factor added with correctly scaled translation
|
||||
|
||||
### Test: add_relative_factor_applies_huber_kernel
|
||||
- Arrange: Create factor graph
|
||||
- Act: Add relative factor
|
||||
- Assert: Factor has Huber kernel applied
|
||||
|
||||
### Test: add_absolute_factor_converts_gps_to_enu
|
||||
- Arrange: Set reference origin
|
||||
- Act: Add absolute factor with known GPS
|
||||
- Assert: Factor position in correct ENU coordinates
|
||||
|
||||
### Test: add_absolute_factor_sets_covariance_by_source
|
||||
- Arrange: Create factor graph
|
||||
- Act: Add user anchor (is_user_anchor=True)
|
||||
- Assert: Covariance is 5m (high confidence)
|
||||
|
||||
### Test: add_absolute_factor_litesam_covariance
|
||||
- Arrange: Create factor graph
|
||||
- Act: Add LiteSAM match (is_user_anchor=False)
|
||||
- Assert: Covariance is 20-50m
|
||||
|
||||
### Test: add_altitude_prior_creates_soft_constraint
|
||||
- Arrange: Create factor graph
|
||||
- Act: Add altitude prior
|
||||
- Assert: UnaryFactor created with correct covariance
|
||||
|
||||
### Test: add_relative_factor_returns_false_on_invalid_flight
|
||||
- Arrange: No graph for flight_id
|
||||
- Act: Call add_relative_factor
|
||||
- Assert: Returns False (or creates graph per implementation)
|
||||
|
||||
### Test: scale_resolution_uses_f17_config
|
||||
- Arrange: Mock F17 with specific altitude, camera params
|
||||
- Act: Add relative factor
|
||||
- Assert: H02 called with correct parameters from F17
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test: incremental_trajectory_building
|
||||
1. Initialize graph with first frame
|
||||
2. Add 100 relative factors from VO
|
||||
3. Add 100 altitude priors
|
||||
4. Verify all factors added successfully
|
||||
5. Verify graph structure correct
|
||||
|
||||
### Test: outlier_handling_350m
|
||||
1. Add normal relative factors (10 frames)
|
||||
2. Add 350m outlier factor (simulating tilt error)
|
||||
3. Add more normal factors
|
||||
4. Verify outlier factor weight reduced by Huber kernel
|
||||
5. Verify graph remains stable
|
||||
|
||||
### Test: mixed_factor_types
|
||||
1. Add relative factors
|
||||
2. Add absolute GPS factors
|
||||
3. Add altitude priors
|
||||
4. Verify all factor types coexist correctly
|
||||
5. Verify graph ready for optimization
|
||||
|
||||
+125
@@ -0,0 +1,125 @@
|
||||
# Feature: Trajectory Optimization & Retrieval
|
||||
|
||||
## Name
|
||||
Trajectory Optimization & Retrieval
|
||||
|
||||
## Description
|
||||
Provides core optimization functionality using GTSAM's iSAM2 for incremental updates and Levenberg-Marquardt for batch optimization. Retrieves optimized trajectory poses and marginal covariances for uncertainty quantification. Supports both real-time incremental optimization (after each frame) and batch refinement (when new absolute factors added).
|
||||
|
||||
## Component APIs Implemented
|
||||
- `optimize(flight_id, iterations) -> OptimizationResult`
|
||||
- `get_trajectory(flight_id) -> Dict[int, Pose]`
|
||||
- `get_marginal_covariance(flight_id, frame_id) -> np.ndarray`
|
||||
|
||||
## External Tools and Services
|
||||
- **GTSAM**: iSAM2 incremental optimizer, Levenberg-Marquardt batch solver
|
||||
- **numpy**: Matrix operations for covariance extraction
|
||||
|
||||
## Internal Methods
|
||||
|
||||
### `_run_isam2_update(flight_id, iterations) -> OptimizationResult`
|
||||
Runs incremental iSAM2 optimization on flight's factor graph.
|
||||
- Updates only affected nodes (fast, <100ms)
|
||||
- Used for real-time frame-by-frame optimization
|
||||
- Returns optimization statistics
|
||||
|
||||
### `_run_batch_optimization(flight_id, iterations) -> OptimizationResult`
|
||||
Runs full Levenberg-Marquardt optimization on entire graph.
|
||||
- Re-optimizes all poses (slower, ~500ms for 100 frames)
|
||||
- Used when new absolute factors added for back-propagation
|
||||
- Returns optimization statistics
|
||||
|
||||
### `_check_convergence(prev_error, curr_error, threshold) -> bool`
|
||||
Checks if optimization has converged.
|
||||
- Compares error reduction against threshold
|
||||
- Returns True if error reduction < threshold
|
||||
|
||||
### `_extract_poses_from_graph(flight_id) -> Dict[int, Pose]`
|
||||
Extracts all optimized poses from GTSAM Values.
|
||||
- Converts GTSAM Pose3 to internal Pose model
|
||||
- Includes position, orientation, timestamp
|
||||
|
||||
### `_compute_marginal_covariance(flight_id, frame_id) -> np.ndarray`
|
||||
Computes marginal covariance for specific frame.
|
||||
- Uses GTSAM Marginals class
|
||||
- Returns 6x6 covariance matrix [x, y, z, roll, pitch, yaw]
|
||||
|
||||
### `_get_optimized_frames(flight_id, prev_values, curr_values) -> List[int]`
|
||||
Determines which frames had pose updates.
|
||||
- Compares previous and current values
|
||||
- Returns list of frame IDs with significant changes
|
||||
|
||||
### `_compute_mean_reprojection_error(flight_id) -> float`
|
||||
Computes mean reprojection error across all factors.
|
||||
- Used for quality assessment
|
||||
- Should be < 1.0 pixels per AC
|
||||
|
||||
## Unit Tests
|
||||
|
||||
### Test: optimize_incremental_updates_affected_nodes
|
||||
- Arrange: Create graph with 10 frames, add new factor to frame 8
|
||||
- Act: Call optimize with few iterations
|
||||
- Assert: Only frames 8-10 significantly updated
|
||||
|
||||
### Test: optimize_batch_updates_all_frames
|
||||
- Arrange: Create graph with drift, add absolute GPS at frame 50
|
||||
- Act: Call optimize with batch mode
|
||||
- Assert: All frames updated (back-propagation)
|
||||
|
||||
### Test: optimize_returns_convergence_status
|
||||
- Arrange: Create well-constrained graph
|
||||
- Act: Call optimize
|
||||
- Assert: OptimizationResult.converged == True
|
||||
|
||||
### Test: get_trajectory_returns_all_poses
|
||||
- Arrange: Create graph with 100 frames, optimize
|
||||
- Act: Call get_trajectory
|
||||
- Assert: Returns dict with 100 poses
|
||||
|
||||
### Test: get_trajectory_poses_in_enu
|
||||
- Arrange: Create graph with known reference
|
||||
- Act: Get trajectory
|
||||
- Assert: Positions in ENU coordinates
|
||||
|
||||
### Test: get_marginal_covariance_returns_6x6
|
||||
- Arrange: Create and optimize graph
|
||||
- Act: Call get_marginal_covariance for frame
|
||||
- Assert: Returns (6, 6) numpy array
|
||||
|
||||
### Test: get_marginal_covariance_reduces_after_absolute_factor
|
||||
- Arrange: Create graph, get covariance for frame 50
|
||||
- Act: Add absolute GPS factor at frame 50, re-optimize
|
||||
- Assert: New covariance smaller than before
|
||||
|
||||
### Test: optimize_respects_iteration_limit
|
||||
- Arrange: Create complex graph
|
||||
- Act: Call optimize with iterations=5
|
||||
- Assert: iterations_used <= 5
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test: drift_correction_with_absolute_gps
|
||||
1. Build trajectory with VO only (100 frames) - will drift
|
||||
2. Add absolute GPS factor at frame 50
|
||||
3. Optimize (batch mode)
|
||||
4. Verify trajectory corrects
|
||||
5. Verify frames 1-49 also corrected (back-propagation)
|
||||
|
||||
### Test: user_anchor_immediate_refinement
|
||||
1. Build trajectory to frame 237
|
||||
2. Add user anchor (high confidence, is_user_anchor=True)
|
||||
3. Optimize
|
||||
4. Verify trajectory snaps to anchor
|
||||
5. Verify low covariance at anchor frame
|
||||
|
||||
### Test: performance_incremental_under_100ms
|
||||
1. Create graph with 50 frames
|
||||
2. Add new relative factor
|
||||
3. Time incremental optimization
|
||||
4. Assert: < 100ms
|
||||
|
||||
### Test: performance_batch_under_500ms
|
||||
1. Create graph with 100 frames
|
||||
2. Time batch optimization
|
||||
3. Assert: < 500ms
|
||||
|
||||
+139
@@ -0,0 +1,139 @@
|
||||
# Feature: Chunk Subgraph Operations
|
||||
|
||||
## Name
|
||||
Chunk Subgraph Operations
|
||||
|
||||
## Description
|
||||
Manages chunk-level factor graph subgraphs for handling tracking loss recovery. Creates independent subgraphs for route chunks (map fragments), adds factors to chunk-specific subgraphs, anchors chunks with GPS measurements, and optimizes chunks independently. F10 provides low-level factor graph operations only; F12 owns chunk metadata (status, is_active, etc.).
|
||||
|
||||
## Component APIs Implemented
|
||||
- `create_chunk_subgraph(flight_id, chunk_id, start_frame_id) -> bool`
|
||||
- `add_relative_factor_to_chunk(flight_id, chunk_id, frame_i, frame_j, relative_pose, covariance) -> bool`
|
||||
- `add_chunk_anchor(flight_id, chunk_id, frame_id, gps, covariance) -> bool`
|
||||
- `get_chunk_trajectory(flight_id, chunk_id) -> Dict[int, Pose]`
|
||||
- `optimize_chunk(flight_id, chunk_id, iterations) -> OptimizationResult`
|
||||
|
||||
## External Tools and Services
|
||||
- **GTSAM**: Subgraph management, BetweenFactor, PriorFactor
|
||||
- **H03 Robust Kernels**: Huber kernel for chunk factors
|
||||
|
||||
## Internal Methods
|
||||
|
||||
### `_create_subgraph(flight_id, chunk_id) -> gtsam.NonlinearFactorGraph`
|
||||
Creates new GTSAM subgraph for chunk.
|
||||
- Initializes empty factor graph
|
||||
- Stores in chunk subgraph dictionary
|
||||
|
||||
### `_initialize_chunk_origin(flight_id, chunk_id, start_frame_id) -> bool`
|
||||
Initializes first frame pose in chunk's local coordinate system.
|
||||
- Sets origin pose at identity (or relative to previous chunk)
|
||||
- Adds prior factor for origin frame
|
||||
|
||||
### `_add_factor_to_subgraph(flight_id, chunk_id, factor) -> bool`
|
||||
Adds factor to chunk's specific subgraph.
|
||||
- Verifies chunk exists
|
||||
- Appends factor to chunk's graph
|
||||
- Marks chunk as needing optimization
|
||||
|
||||
### `_get_chunk_subgraph(flight_id, chunk_id) -> Optional[gtsam.NonlinearFactorGraph]`
|
||||
Retrieves chunk's subgraph if exists.
|
||||
- Returns None if chunk not found
|
||||
|
||||
### `_extract_chunk_poses(flight_id, chunk_id) -> Dict[int, Pose]`
|
||||
Extracts all optimized poses from chunk's subgraph.
|
||||
- Poses in chunk's local coordinate system
|
||||
- Converts GTSAM Pose3 to internal Pose model
|
||||
|
||||
### `_optimize_subgraph(flight_id, chunk_id, iterations) -> OptimizationResult`
|
||||
Runs Levenberg-Marquardt on chunk's isolated subgraph.
|
||||
- Optimizes only chunk's factors
|
||||
- Does not affect other chunks
|
||||
- Returns optimization result
|
||||
|
||||
### `_anchor_chunk_to_global(flight_id, chunk_id, frame_id, enu_position) -> bool`
|
||||
Adds GPS anchor to chunk's subgraph as PriorFactor.
|
||||
- Converts GPS to ENU (may need separate reference or global reference)
|
||||
- Adds prior factor for anchor frame
|
||||
- Returns success status
|
||||
|
||||
### `_track_chunk_frames(flight_id, chunk_id, frame_id) -> bool`
|
||||
Tracks which frames belong to which chunk.
|
||||
- Updates frame-to-chunk mapping
|
||||
- Used for factor routing
|
||||
|
||||
## Unit Tests
|
||||
|
||||
### Test: create_chunk_subgraph_returns_true
|
||||
- Arrange: Valid flight_id
|
||||
- Act: Call create_chunk_subgraph
|
||||
- Assert: Returns True, subgraph created
|
||||
|
||||
### Test: create_chunk_subgraph_initializes_origin
|
||||
- Arrange: Create chunk
|
||||
- Act: Check subgraph
|
||||
- Assert: Has prior factor for start_frame_id
|
||||
|
||||
### Test: add_relative_factor_to_chunk_success
|
||||
- Arrange: Create chunk
|
||||
- Act: Add relative factor to chunk
|
||||
- Assert: Returns True, factor in chunk's subgraph
|
||||
|
||||
### Test: add_relative_factor_to_chunk_nonexistent_chunk
|
||||
- Arrange: No chunk created
|
||||
- Act: Add relative factor to nonexistent chunk
|
||||
- Assert: Returns False
|
||||
|
||||
### Test: add_chunk_anchor_success
|
||||
- Arrange: Create chunk with frames
|
||||
- Act: Add GPS anchor
|
||||
- Assert: Returns True, prior factor added
|
||||
|
||||
### Test: get_chunk_trajectory_returns_poses
|
||||
- Arrange: Create chunk, add factors, optimize
|
||||
- Act: Call get_chunk_trajectory
|
||||
- Assert: Returns dict with chunk poses
|
||||
|
||||
### Test: get_chunk_trajectory_empty_chunk
|
||||
- Arrange: Create chunk without factors
|
||||
- Act: Call get_chunk_trajectory
|
||||
- Assert: Returns dict with only origin pose
|
||||
|
||||
### Test: optimize_chunk_success
|
||||
- Arrange: Create chunk with factors
|
||||
- Act: Call optimize_chunk
|
||||
- Assert: Returns OptimizationResult with converged=True
|
||||
|
||||
### Test: optimize_chunk_isolation
|
||||
- Arrange: Create chunk_1 and chunk_2 with factors
|
||||
- Act: Optimize chunk_1
|
||||
- Assert: chunk_2 poses unchanged
|
||||
|
||||
### Test: multiple_chunks_simultaneous
|
||||
- Arrange: Create 3 chunks
|
||||
- Act: Add factors to each
|
||||
- Assert: All chunks maintain independent state
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test: multi_chunk_creation_and_isolation
|
||||
1. Create chunk_1 with frames 1-10
|
||||
2. Create chunk_2 with frames 20-30 (disconnected)
|
||||
3. Add relative factors to each chunk
|
||||
4. Verify chunks optimized independently
|
||||
5. Verify factors isolated to respective chunks
|
||||
|
||||
### Test: chunk_anchoring
|
||||
1. Create chunk with frames 1-10
|
||||
2. Add relative factors
|
||||
3. Add chunk_anchor at frame 5
|
||||
4. Optimize chunk
|
||||
5. Verify trajectory consistent with anchor
|
||||
6. Verify has_anchor reflected (via F12 query)
|
||||
|
||||
### Test: chunk_trajectory_local_coordinates
|
||||
1. Create chunk
|
||||
2. Add relative factors
|
||||
3. Get chunk trajectory
|
||||
4. Verify poses in chunk's local coordinate system
|
||||
5. Verify origin at identity or start frame
|
||||
|
||||
+142
@@ -0,0 +1,142 @@
|
||||
# Feature: Chunk Merging & Global Optimization
|
||||
|
||||
## Name
|
||||
Chunk Merging & Global Optimization
|
||||
|
||||
## Description
|
||||
Provides Sim(3) similarity transformation for merging chunk subgraphs and global optimization across all chunks. Handles scale, rotation, and translation differences between chunks (critical for monocular VO with scale ambiguity). Called by F12 Route Chunk Manager after chunk matching succeeds.
|
||||
|
||||
## Component APIs Implemented
|
||||
- `merge_chunk_subgraphs(flight_id, new_chunk_id, main_chunk_id, transform: Sim3Transform) -> bool`
|
||||
- `optimize_global(flight_id, iterations) -> OptimizationResult`
|
||||
|
||||
## External Tools and Services
|
||||
- **GTSAM**: Graph merging, global optimization
|
||||
- **numpy/scipy**: Sim(3) transformation computation
|
||||
|
||||
## Internal Methods
|
||||
|
||||
### `_apply_sim3_transform(poses: Dict[int, Pose], transform: Sim3Transform) -> Dict[int, Pose]`
|
||||
Applies Sim(3) similarity transformation to all poses.
|
||||
- Translation: p' = s * R * p + t
|
||||
- Rotation: R' = R_transform * R_original
|
||||
- Scale: All positions scaled by transform.scale
|
||||
- Critical for merging chunks with different scales
|
||||
|
||||
### `_merge_subgraphs(flight_id, source_chunk_id, dest_chunk_id) -> bool`
|
||||
Merges source chunk's subgraph into destination chunk's subgraph.
|
||||
- Copies all factors from source to destination
|
||||
- Updates node keys to avoid conflicts
|
||||
- Preserves factor graph structure
|
||||
|
||||
### `_update_frame_to_chunk_mapping(flight_id, source_chunk_id, dest_chunk_id) -> bool`
|
||||
Updates frame-to-chunk mapping after merge.
|
||||
- All frames from source_chunk now belong to dest_chunk
|
||||
- Used for routing future factors
|
||||
|
||||
### `_transform_chunk_factors(flight_id, chunk_id, transform: Sim3Transform) -> bool`
|
||||
Transforms all factors in chunk's subgraph by Sim(3).
|
||||
- Updates BetweenFactor translations with scale
|
||||
- Updates PriorFactor positions
|
||||
- Preserves covariances (may need adjustment for scale)
|
||||
|
||||
### `_validate_merge_preconditions(flight_id, new_chunk_id, main_chunk_id) -> bool`
|
||||
Validates that merge can proceed.
|
||||
- Verifies both chunks exist
|
||||
- Verifies new_chunk has anchor (required for merge)
|
||||
- Returns False if preconditions not met
|
||||
|
||||
### `_collect_all_chunk_subgraphs(flight_id) -> List[gtsam.NonlinearFactorGraph]`
|
||||
Collects all chunk subgraphs for global optimization.
|
||||
- Returns list of all subgraphs
|
||||
- Filters out merged/inactive chunks (via F12)
|
||||
|
||||
### `_run_global_optimization(flight_id, iterations) -> OptimizationResult`
|
||||
Runs optimization across all anchored chunks.
|
||||
- Transforms chunks to global coordinate system
|
||||
- Runs Levenberg-Marquardt on combined graph
|
||||
- Updates all chunk trajectories
|
||||
|
||||
### `_compute_sim3_from_correspondences(source_poses, dest_poses) -> Sim3Transform`
|
||||
Computes Sim(3) transformation from pose correspondences.
|
||||
- Uses Horn's method or similar for rigid + scale estimation
|
||||
- Returns translation, rotation, scale
|
||||
|
||||
### `_update_all_chunk_trajectories(flight_id) -> bool`
|
||||
Updates all chunk trajectories after global optimization.
|
||||
- Extracts poses from global graph
|
||||
- Distributes to chunk-specific storage
|
||||
|
||||
## Unit Tests
|
||||
|
||||
### Test: merge_chunk_subgraphs_success
|
||||
- Arrange: Create chunk_1 (anchored), chunk_2 (with anchor)
|
||||
- Act: Call merge_chunk_subgraphs with Sim3 transform
|
||||
- Assert: Returns True, chunks merged
|
||||
|
||||
### Test: merge_chunk_subgraphs_unanchored_fails
|
||||
- Arrange: Create chunk_1, chunk_2 (no anchor)
|
||||
- Act: Call merge_chunk_subgraphs
|
||||
- Assert: Returns False
|
||||
|
||||
### Test: merge_applies_sim3_transform
|
||||
- Arrange: Create chunks with known poses, define transform
|
||||
- Act: Merge chunks
|
||||
- Assert: new_chunk poses transformed correctly
|
||||
|
||||
### Test: merge_updates_frame_mapping
|
||||
- Arrange: Create chunks, merge
|
||||
- Act: Query frame-to-chunk mapping
|
||||
- Assert: new_chunk frames now belong to main_chunk
|
||||
|
||||
### Test: optimize_global_returns_result
|
||||
- Arrange: Create multiple anchored chunks
|
||||
- Act: Call optimize_global
|
||||
- Assert: Returns OptimizationResult with all frames
|
||||
|
||||
### Test: optimize_global_achieves_consistency
|
||||
- Arrange: Create chunks with overlapping anchors
|
||||
- Act: Optimize global
|
||||
- Assert: Anchor frames align within tolerance
|
||||
|
||||
### Test: sim3_transform_handles_scale
|
||||
- Arrange: Poses with scale=2.0 difference
|
||||
- Act: Apply Sim(3) transform
|
||||
- Assert: Scaled positions match expected
|
||||
|
||||
### Test: sim3_transform_handles_rotation
|
||||
- Arrange: Poses with 90° rotation difference
|
||||
- Act: Apply Sim(3) transform
|
||||
- Assert: Rotated poses match expected
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test: chunk_anchoring_and_merging
|
||||
1. Create chunk_1 (frames 1-10), chunk_2 (frames 20-30)
|
||||
2. Add relative factors to each
|
||||
3. Add chunk_anchor to chunk_2 (frame 25)
|
||||
4. Optimize chunk_2 → local consistency improved
|
||||
5. Merge chunk_2 into chunk_1 with Sim(3) transform
|
||||
6. Optimize global → both chunks globally consistent
|
||||
7. Verify final trajectory coherent across chunks
|
||||
|
||||
### Test: simultaneous_multi_chunk_processing
|
||||
1. Create 3 chunks simultaneously (disconnected segments)
|
||||
2. Process frames in each chunk independently
|
||||
3. Anchor each chunk asynchronously
|
||||
4. Merge chunks as anchors become available
|
||||
5. Verify final global trajectory consistent
|
||||
|
||||
### Test: global_optimization_performance
|
||||
1. Create 5 chunks with 20 frames each
|
||||
2. Anchor all chunks
|
||||
3. Time optimize_global
|
||||
4. Assert: < 500ms for 100 total frames
|
||||
|
||||
### Test: scale_resolution_across_chunks
|
||||
1. Create chunk_1 with scale drift (monocular VO)
|
||||
2. Create chunk_2 with different scale drift
|
||||
3. Anchor both chunks
|
||||
4. Merge with Sim(3) accounting for scale
|
||||
5. Verify merged trajectory has consistent scale
|
||||
|
||||
+152
@@ -0,0 +1,152 @@
|
||||
# Feature: Multi-Flight Graph Lifecycle
|
||||
|
||||
## Name
|
||||
Multi-Flight Graph Lifecycle
|
||||
|
||||
## Description
|
||||
Manages flight-scoped factor graph state for concurrent multi-flight processing. Each flight maintains an independent factor graph (Dict[str, FactorGraph] keyed by flight_id), enabling parallel processing without cross-contamination. Provides cleanup when flights are deleted.
|
||||
|
||||
## Component APIs Implemented
|
||||
- `delete_flight_graph(flight_id) -> bool`
|
||||
|
||||
## External Tools and Services
|
||||
- **GTSAM**: Factor graph instance management
|
||||
|
||||
## Internal Methods
|
||||
|
||||
### `_get_or_create_flight_graph(flight_id) -> FlightGraphState`
|
||||
Gets existing flight graph state or creates new one.
|
||||
- Manages Dict[str, FlightGraphState] internally
|
||||
- Initializes iSAM2 instance, reference origin, chunk subgraphs
|
||||
- Thread-safe access
|
||||
|
||||
### `_initialize_flight_graph(flight_id) -> FlightGraphState`
|
||||
Initializes new factor graph state for flight.
|
||||
- Creates new iSAM2 instance
|
||||
- Initializes empty chunk subgraph dictionary
|
||||
- Sets reference origin to None (set on first absolute factor)
|
||||
|
||||
### `_cleanup_flight_resources(flight_id) -> bool`
|
||||
Cleans up all resources associated with flight.
|
||||
- Removes factor graph from memory
|
||||
- Cleans up chunk subgraphs
|
||||
- Clears frame-to-chunk mappings
|
||||
- Releases GTSAM objects
|
||||
|
||||
### `_validate_flight_exists(flight_id) -> bool`
|
||||
Validates that flight graph exists.
|
||||
- Returns True if flight_id in graph dictionary
|
||||
- Used as guard for all operations
|
||||
|
||||
### `_get_all_flight_ids() -> List[str]`
|
||||
Returns list of all active flight IDs.
|
||||
- Used for administrative/monitoring purposes
|
||||
|
||||
### `_get_flight_statistics(flight_id) -> FlightGraphStats`
|
||||
Returns statistics about flight's factor graph.
|
||||
- Number of frames, factors, chunks
|
||||
- Memory usage estimate
|
||||
- Last optimization time
|
||||
|
||||
## Data Structures
|
||||
|
||||
### `FlightGraphState`
|
||||
Internal state container for a flight's factor graph.
|
||||
```python
|
||||
class FlightGraphState:
|
||||
flight_id: str
|
||||
isam2: gtsam.ISAM2
|
||||
values: gtsam.Values
|
||||
reference_origin: Optional[GPSPoint] # ENU reference
|
||||
chunk_subgraphs: Dict[str, gtsam.NonlinearFactorGraph]
|
||||
chunk_values: Dict[str, gtsam.Values]
|
||||
frame_to_chunk: Dict[int, str]
|
||||
created_at: datetime
|
||||
last_optimized: Optional[datetime]
|
||||
```
|
||||
|
||||
### `FlightGraphStats`
|
||||
Statistics for monitoring.
|
||||
```python
|
||||
class FlightGraphStats:
|
||||
flight_id: str
|
||||
num_frames: int
|
||||
num_factors: int
|
||||
num_chunks: int
|
||||
num_active_chunks: int
|
||||
estimated_memory_mb: float
|
||||
last_optimization_time_ms: float
|
||||
```
|
||||
|
||||
## Unit Tests
|
||||
|
||||
### Test: delete_flight_graph_success
|
||||
- Arrange: Create flight graph with factors
|
||||
- Act: Call delete_flight_graph
|
||||
- Assert: Returns True, graph removed
|
||||
|
||||
### Test: delete_flight_graph_nonexistent
|
||||
- Arrange: No graph for flight_id
|
||||
- Act: Call delete_flight_graph
|
||||
- Assert: Returns False (graceful handling)
|
||||
|
||||
### Test: delete_flight_graph_cleans_chunks
|
||||
- Arrange: Create flight with multiple chunks
|
||||
- Act: Delete flight graph
|
||||
- Assert: All chunk subgraphs cleaned up
|
||||
|
||||
### Test: flight_isolation_no_cross_contamination
|
||||
- Arrange: Create flight_1, flight_2
|
||||
- Act: Add factors to flight_1
|
||||
- Assert: flight_2 graph unchanged
|
||||
|
||||
### Test: concurrent_flight_processing
|
||||
- Arrange: Create flight_1, flight_2
|
||||
- Act: Add factors to both concurrently
|
||||
- Assert: Both maintain independent state
|
||||
|
||||
### Test: get_or_create_creates_new
|
||||
- Arrange: No existing graph
|
||||
- Act: Call internal _get_or_create_flight_graph
|
||||
- Assert: New graph created with default state
|
||||
|
||||
### Test: get_or_create_returns_existing
|
||||
- Arrange: Graph exists for flight_id
|
||||
- Act: Call internal _get_or_create_flight_graph
|
||||
- Assert: Same graph instance returned
|
||||
|
||||
### Test: flight_statistics_accurate
|
||||
- Arrange: Create flight with known factors
|
||||
- Act: Get flight statistics
|
||||
- Assert: Counts match expected
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test: multi_flight_concurrent_processing
|
||||
1. Create flight_1, flight_2, flight_3 concurrently
|
||||
2. Add factors to each flight in parallel
|
||||
3. Optimize each flight independently
|
||||
4. Verify each flight has correct trajectory
|
||||
5. Delete flight_2
|
||||
6. Verify flight_1, flight_3 unaffected
|
||||
|
||||
### Test: flight_cleanup_memory_release
|
||||
1. Create flight with 1000 frames
|
||||
2. Optimize trajectory
|
||||
3. Note memory usage
|
||||
4. Delete flight graph
|
||||
5. Verify memory released (within tolerance)
|
||||
|
||||
### Test: reference_origin_per_flight
|
||||
1. Create flight_1 with start GPS at point A
|
||||
2. Create flight_2 with start GPS at point B
|
||||
3. Add absolute factors to each
|
||||
4. Verify ENU coordinates relative to respective origins
|
||||
|
||||
### Test: chunk_cleanup_on_flight_delete
|
||||
1. Create flight with 5 chunks
|
||||
2. Add factors and anchors to chunks
|
||||
3. Delete flight
|
||||
4. Verify all chunk subgraphs cleaned up
|
||||
5. Verify no memory leaks
|
||||
|
||||
Reference in New Issue
Block a user