mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 19:26:37 +00:00
initial structure implemented
docs -> _docs
This commit is contained in:
Oleksandr Bezdieniezhnykh
+125
@@ -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
|
||||
|
||||
+830
@@ -0,0 +1,830 @@
|
||||
# Factor Graph Optimizer
|
||||
|
||||
## Interface Definition
|
||||
|
||||
**Interface Name**: `IFactorGraphOptimizer`
|
||||
|
||||
### Interface Methods
|
||||
|
||||
```python
|
||||
class IFactorGraphOptimizer(ABC):
|
||||
"""
|
||||
GTSAM-based factor graph optimizer for trajectory estimation.
|
||||
|
||||
## Multi-Flight Support
|
||||
All state-modifying methods require `flight_id` parameter.
|
||||
Each flight maintains an independent factor graph state.
|
||||
F10 internally manages Dict[str, FactorGraph] keyed by flight_id.
|
||||
|
||||
This enables:
|
||||
- Concurrent processing of multiple flights
|
||||
- Flight-scoped optimization without cross-contamination
|
||||
- Independent cleanup via delete_flight_graph()
|
||||
"""
|
||||
|
||||
@abstractmethod
|
||||
def add_relative_factor(self, flight_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def add_absolute_factor(self, flight_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray, is_user_anchor: bool) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def add_altitude_prior(self, flight_id: str, frame_id: int, altitude: float, covariance: float) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def optimize(self, flight_id: str, iterations: int) -> OptimizationResult:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_trajectory(self, flight_id: str) -> Dict[int, Pose]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_marginal_covariance(self, flight_id: str, frame_id: int) -> np.ndarray:
|
||||
pass
|
||||
|
||||
# Chunk operations - F10 only manages factor graph subgraphs
|
||||
# F12 owns chunk metadata (status, is_active, etc.)
|
||||
@abstractmethod
|
||||
def create_chunk_subgraph(self, flight_id: str, chunk_id: str, start_frame_id: int) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def add_relative_factor_to_chunk(self, flight_id: str, chunk_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def add_chunk_anchor(self, flight_id: str, chunk_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray) -> bool:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def merge_chunk_subgraphs(self, flight_id: str, new_chunk_id: str, main_chunk_id: str, transform: Sim3Transform) -> bool:
|
||||
"""Merges new_chunk INTO main_chunk. Extends main_chunk with new_chunk's subgraph."""
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_chunk_trajectory(self, flight_id: str, chunk_id: str) -> Dict[int, Pose]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def optimize_chunk(self, flight_id: str, chunk_id: str, iterations: int) -> OptimizationResult:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def optimize_global(self, flight_id: str, iterations: int) -> OptimizationResult:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def delete_flight_graph(self, flight_id: str) -> bool:
|
||||
"""Cleanup factor graph when flight is deleted."""
|
||||
pass
|
||||
```
|
||||
|
||||
## Component Description
|
||||
|
||||
### Responsibilities
|
||||
- GTSAM-based fusion of relative and absolute measurements
|
||||
- Incremental optimization (iSAM2) for real-time performance
|
||||
- Robust kernels (Huber/Cauchy) for 350m outlier handling
|
||||
- Scale resolution through altitude priors and absolute GPS
|
||||
- Trajectory smoothing and global consistency
|
||||
- Back-propagation of refinements to previous frames
|
||||
- **Low-level factor graph chunk operations** (subgraph creation, factor addition, optimization)
|
||||
- **Sim(3) transformation for chunk merging**
|
||||
|
||||
### Chunk Responsibility Clarification
|
||||
|
||||
**F10 provides low-level factor graph operations only**:
|
||||
- `create_chunk_subgraph()`: Creates subgraph in factor graph (returns bool, not ChunkHandle)
|
||||
- `add_relative_factor_to_chunk()`: Adds factors to chunk's subgraph
|
||||
- `add_chunk_anchor()`: Adds GPS anchor to chunk's subgraph
|
||||
- `merge_chunk_subgraphs()`: Applies Sim(3) transform and merges subgraphs
|
||||
- `optimize_chunk()`, `optimize_global()`: Runs optimization
|
||||
|
||||
**F10 does NOT own chunk metadata** - only factor graph data structures.
|
||||
|
||||
**F12 is the source of truth for ALL chunk state** (see F12 spec):
|
||||
- ChunkHandle with all metadata (is_active, has_anchor, matching_status)
|
||||
- Chunk lifecycle management
|
||||
- Chunk readiness determination
|
||||
- High-level chunk queries
|
||||
- F12 calls F10 for factor graph operations
|
||||
|
||||
**F11 coordinates recovery** (see F11 spec):
|
||||
- Triggers chunk creation via F12
|
||||
- Coordinates matching workflows
|
||||
- Emits chunk-related events
|
||||
|
||||
### Scope
|
||||
- Non-linear least squares optimization
|
||||
- Factor graph representation of SLAM problem
|
||||
- Handles monocular scale ambiguity
|
||||
- Real-time incremental updates
|
||||
- Asynchronous batch refinement
|
||||
- **Multi-chunk factor graph with independent subgraphs**
|
||||
- **Chunk-level optimization and global merging**
|
||||
- **Sim(3) similarity transformation for chunk alignment**
|
||||
|
||||
### Design Pattern: Composition Over Complex Interface
|
||||
|
||||
F10 uses **composition** to keep the interface manageable. Rather than exposing 20+ methods in a monolithic interface, complex operations are composed from simpler primitives:
|
||||
|
||||
**Primitive Operations**:
|
||||
- `add_relative_factor()`, `add_absolute_factor()`, `add_altitude_prior()` - Factor management
|
||||
- `optimize()`, `get_trajectory()`, `get_marginal_covariance()` - Core optimization
|
||||
|
||||
**Chunk Operations** (composed from primitives):
|
||||
- `create_new_chunk()`, `add_relative_factor_to_chunk()`, `add_chunk_anchor()` - Chunk factor management
|
||||
- `merge_chunks()`, `optimize_chunk()`, `optimize_global()` - Chunk optimization
|
||||
|
||||
**Callers compose these primitives** for complex workflows (e.g., F11 composes anchor + merge + optimize_global).
|
||||
|
||||
## API Methods
|
||||
|
||||
### `add_relative_factor(frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool`
|
||||
|
||||
**Description**: Adds relative pose measurement between consecutive frames.
|
||||
|
||||
**Called By**:
|
||||
- F07 Sequential VO (frame-to-frame odometry)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
frame_i: int # Previous frame ID
|
||||
frame_j: int # Current frame ID (typically frame_i + 1)
|
||||
relative_pose: RelativePose:
|
||||
translation: np.ndarray # (3,) - unit vector (scale ambiguous from VO)
|
||||
rotation: np.ndarray # (3, 3) or quaternion
|
||||
covariance: np.ndarray # (6, 6) - uncertainty
|
||||
```
|
||||
|
||||
**Scale Resolution**:
|
||||
F07 returns unit translation vectors due to monocular scale ambiguity. F10 resolves scale by:
|
||||
1. Using altitude prior to constrain Z-axis
|
||||
2. **Computing expected displacement**: Call H02 GSD Calculator.compute_gsd() to get GSD
|
||||
- GSD = (sensor_width × altitude) / (focal_length × resolution_width)
|
||||
- expected_displacement ≈ frame_spacing × GSD (typically ~100m)
|
||||
3. Scaling: scaled_translation = unit_translation × expected_displacement
|
||||
4. Global refinement using absolute GPS factors from F09 LiteSAM
|
||||
|
||||
**Explicit Flow**:
|
||||
```python
|
||||
# In add_relative_factor():
|
||||
# altitude comes from F17 Configuration Manager (predefined operational altitude)
|
||||
# focal_length, sensor_width from F17 Configuration Manager
|
||||
config = F17.get_flight_config(flight_id)
|
||||
altitude = config.altitude # Predefined altitude, NOT from EXIF
|
||||
gsd = H02.compute_gsd(altitude, config.camera_params.focal_length,
|
||||
config.camera_params.sensor_width,
|
||||
config.camera_params.resolution_width)
|
||||
expected_displacement = frame_spacing * gsd # ~100m typical at 300m altitude
|
||||
scaled_translation = relative_pose.translation * expected_displacement
|
||||
# Add scaled_translation to factor graph
|
||||
```
|
||||
|
||||
**Note**: Altitude comes from F17 Configuration Manager (predefined operational altitude), NOT from EXIF metadata. The problem statement specifies images don't have GPS metadata.
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if factor added successfully
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Create BetweenFactor in GTSAM
|
||||
2. Apply robust kernel (Huber) to handle outliers
|
||||
3. Add to factor graph
|
||||
4. Mark graph as needing optimization
|
||||
|
||||
**Robust Kernel**:
|
||||
- **Huber loss**: Downweights large errors (>threshold)
|
||||
- **Critical** for 350m outlier handling from tilt
|
||||
|
||||
**Test Cases**:
|
||||
1. **Normal motion**: Factor added, contributes to optimization
|
||||
2. **Large displacement** (350m outlier): Huber kernel reduces weight
|
||||
3. **Consecutive factors**: Chain of relative factors builds trajectory
|
||||
|
||||
---
|
||||
|
||||
### `add_absolute_factor(frame_id: int, gps: GPSPoint, covariance: np.ndarray, is_user_anchor: bool) -> bool`
|
||||
|
||||
**Description**: Adds absolute GPS measurement for drift correction or user anchor.
|
||||
|
||||
**Called By**:
|
||||
- F09 Metric Refinement (after LiteSAM alignment)
|
||||
- F11 Failure Recovery Coordinator (user-provided anchors)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
frame_id: int
|
||||
gps: GPSPoint:
|
||||
lat: float
|
||||
lon: float
|
||||
covariance: np.ndarray # (2, 2) or (3, 3) - GPS uncertainty
|
||||
is_user_anchor: bool # True for user-provided fixes (high confidence)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if factor added
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Convert GPS to local ENU coordinates (East-North-Up)
|
||||
2. Create PriorFactor or UnaryFactor
|
||||
3. Set covariance (low for user anchors, higher for LiteSAM)
|
||||
4. Add to factor graph
|
||||
5. Trigger optimization (immediate for user anchors)
|
||||
|
||||
**Covariance Settings**:
|
||||
- **User anchor**: σ = 5m (high confidence)
|
||||
- **LiteSAM match**: σ = 20-50m (depends on confidence)
|
||||
|
||||
**Test Cases**:
|
||||
1. **LiteSAM GPS**: Adds absolute factor, corrects drift
|
||||
2. **User anchor**: High confidence, immediately refines trajectory
|
||||
3. **Multiple absolute factors**: Graph optimizes to balance all
|
||||
|
||||
---
|
||||
|
||||
### `add_altitude_prior(frame_id: int, altitude: float, covariance: float) -> bool`
|
||||
|
||||
**Description**: Adds altitude constraint to resolve monocular scale ambiguity.
|
||||
|
||||
**Called By**:
|
||||
- Main processing loop (for each frame)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
frame_id: int
|
||||
altitude: float # Predefined altitude in meters
|
||||
covariance: float # Altitude uncertainty (e.g., 50m)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if prior added
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Create UnaryFactor for Z-coordinate
|
||||
2. Set as soft constraint (not hard constraint)
|
||||
3. Add to factor graph
|
||||
|
||||
**Purpose**:
|
||||
- Resolves scale ambiguity in monocular VO
|
||||
- Prevents scale drift (trajectory collapsing or exploding)
|
||||
- Soft constraint allows adjustment based on absolute GPS
|
||||
|
||||
**Test Cases**:
|
||||
1. **Without altitude prior**: Scale drifts over time
|
||||
2. **With altitude prior**: Scale stabilizes
|
||||
3. **Conflicting measurements**: Optimizer balances VO and altitude
|
||||
|
||||
---
|
||||
|
||||
### `optimize(iterations: int) -> OptimizationResult`
|
||||
|
||||
**Description**: Runs optimization to refine trajectory.
|
||||
|
||||
**Called By**:
|
||||
- Main processing loop (incremental after each frame)
|
||||
- Asynchronous refinement thread (batch optimization)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
iterations: int # Max iterations (typically 5-10 for incremental, 50-100 for batch)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
OptimizationResult:
|
||||
converged: bool
|
||||
final_error: float
|
||||
iterations_used: int
|
||||
optimized_frames: List[int] # Frames with updated poses
|
||||
```
|
||||
|
||||
**Processing Details**:
|
||||
- **Incremental** (iSAM2): Updates only affected nodes
|
||||
- **Batch**: Re-optimizes entire trajectory when new absolute factors added
|
||||
- **Robust M-estimation**: Automatically downweights outliers
|
||||
|
||||
**Optimization Algorithm** (Levenberg-Marquardt):
|
||||
1. Linearize factor graph around current estimate
|
||||
2. Solve linear system
|
||||
3. Update pose estimates
|
||||
4. Check convergence (error reduction < threshold)
|
||||
|
||||
**Test Cases**:
|
||||
1. **Incremental optimization**: Fast (<100ms), local update
|
||||
2. **Batch optimization**: Slower (~500ms), refines entire trajectory
|
||||
3. **Convergence**: Error reduces, converges within iterations
|
||||
|
||||
---
|
||||
|
||||
### `get_trajectory() -> Dict[int, Pose]`
|
||||
|
||||
**Description**: Retrieves complete optimized trajectory.
|
||||
|
||||
**Called By**:
|
||||
- F13 Result Manager (for publishing results)
|
||||
- F12 Coordinate Transformer (for GPS conversion)
|
||||
|
||||
**Input**: None
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
Dict[int, Pose]:
|
||||
frame_id -> Pose:
|
||||
position: np.ndarray # (x, y, z) in ENU
|
||||
orientation: np.ndarray # Quaternion or rotation matrix
|
||||
timestamp: datetime
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Extract all pose estimates from graph
|
||||
2. Convert to appropriate coordinate system
|
||||
3. Return dictionary
|
||||
|
||||
**Test Cases**:
|
||||
1. **After optimization**: Returns all frame poses
|
||||
2. **Refined trajectory**: Poses updated after batch optimization
|
||||
|
||||
---
|
||||
|
||||
### `get_marginal_covariance(frame_id: int) -> np.ndarray`
|
||||
|
||||
**Description**: Gets uncertainty (covariance) of a pose estimate.
|
||||
|
||||
**Called By**:
|
||||
- F11 Failure Recovery Coordinator (to detect high uncertainty)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
frame_id: int
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
np.ndarray: (6, 6) covariance matrix [x, y, z, roll, pitch, yaw]
|
||||
```
|
||||
|
||||
**Purpose**:
|
||||
- Uncertainty quantification
|
||||
- Trigger user input when uncertainty too high (> 50m radius)
|
||||
|
||||
**Test Cases**:
|
||||
1. **Well-constrained pose**: Small covariance
|
||||
2. **Unconstrained pose**: Large covariance
|
||||
3. **After absolute factor**: Covariance reduces
|
||||
|
||||
---
|
||||
|
||||
### `create_new_chunk(chunk_id: str, start_frame_id: int) -> ChunkHandle`
|
||||
|
||||
**Description**: Creates a new map fragment/chunk with its own subgraph.
|
||||
|
||||
**Called By**:
|
||||
- F02 Flight Processor (when tracking lost)
|
||||
- F12 Route Chunk Manager (chunk lifecycle)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_id: str # Unique chunk identifier
|
||||
start_frame_id: int # First frame in chunk
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
ChunkHandle:
|
||||
chunk_id: str
|
||||
flight_id: str
|
||||
start_frame_id: int
|
||||
end_frame_id: Optional[int]
|
||||
frames: List[int]
|
||||
is_active: bool
|
||||
has_anchor: bool
|
||||
anchor_frame_id: Optional[int]
|
||||
anchor_gps: Optional[GPSPoint]
|
||||
matching_status: str # "unanchored", "matching", "anchored", "merged"
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Create new subgraph for chunk
|
||||
2. Initialize first frame pose in chunk's local coordinate system
|
||||
3. Mark chunk as active
|
||||
4. Return ChunkHandle
|
||||
|
||||
**Test Cases**:
|
||||
1. **Create chunk**: Returns ChunkHandle with is_active=True
|
||||
2. **Multiple chunks**: Can create multiple chunks simultaneously
|
||||
3. **Chunk isolation**: Factors added to chunk don't affect other chunks
|
||||
|
||||
---
|
||||
|
||||
### `get_chunk_for_frame(frame_id: int) -> Optional[ChunkHandle]`
|
||||
|
||||
**Description**: Gets the chunk containing the specified frame (low-level factor graph query).
|
||||
|
||||
**Called By**:
|
||||
- F12 Route Chunk Manager (for internal queries)
|
||||
- F07 Sequential VO (to determine chunk context for factor graph operations)
|
||||
|
||||
**Note**: This is a low-level method for factor graph operations. For high-level chunk queries, use F12.get_active_chunk(flight_id).
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
frame_id: int
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
Optional[ChunkHandle] # Active chunk or None if frame not in any chunk
|
||||
```
|
||||
|
||||
**Test Cases**:
|
||||
1. **Frame in active chunk**: Returns ChunkHandle
|
||||
2. **Frame not in chunk**: Returns None
|
||||
3. **Multiple chunks**: Returns correct chunk for frame
|
||||
|
||||
---
|
||||
|
||||
### `add_relative_factor_to_chunk(chunk_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool`
|
||||
|
||||
**Description**: Adds relative pose measurement to a specific chunk's subgraph.
|
||||
|
||||
**Called By**:
|
||||
- F07 Sequential VO (chunk-scoped operations)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_id: str
|
||||
frame_i: int # Previous frame ID
|
||||
frame_j: int # Current frame ID
|
||||
relative_pose: RelativePose
|
||||
covariance: np.ndarray # (6, 6)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if factor added successfully
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Verify chunk exists and is active
|
||||
2. Create BetweenFactor in chunk's subgraph
|
||||
3. Apply robust kernel (Huber)
|
||||
4. Add to chunk's factor graph
|
||||
5. Mark chunk as needing optimization
|
||||
|
||||
**Test Cases**:
|
||||
1. **Add to active chunk**: Factor added successfully
|
||||
2. **Add to inactive chunk**: Returns False
|
||||
3. **Multiple chunks**: Factors isolated to respective chunks
|
||||
|
||||
---
|
||||
|
||||
### `add_chunk_anchor(chunk_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray) -> bool`
|
||||
|
||||
**Description**: Adds absolute GPS anchor to a chunk, enabling global localization.
|
||||
|
||||
**Called By**:
|
||||
- F09 Metric Refinement (after chunk LiteSAM matching)
|
||||
- F11 Failure Recovery Coordinator (chunk matching)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_id: str
|
||||
frame_id: int # Frame within chunk to anchor
|
||||
gps: GPSPoint
|
||||
covariance: np.ndarray # (2, 2) or (3, 3)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if anchor added
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Convert GPS to ENU coordinates
|
||||
2. Create PriorFactor in chunk's subgraph
|
||||
3. Mark chunk as anchored
|
||||
4. Trigger chunk optimization
|
||||
5. Enable chunk merging
|
||||
|
||||
**Test Cases**:
|
||||
1. **Anchor unanchored chunk**: Anchor added, has_anchor=True
|
||||
2. **Anchor already anchored chunk**: Updates anchor
|
||||
3. **Chunk optimization**: Chunk optimized after anchor
|
||||
|
||||
---
|
||||
|
||||
### `merge_chunk_subgraphs(flight_id: str, new_chunk_id: str, main_chunk_id: str, transform: Sim3Transform) -> bool`
|
||||
|
||||
**Description**: Merges new_chunk INTO main_chunk using Sim(3) similarity transformation. Extends main_chunk with new_chunk's frames.
|
||||
|
||||
**Called By**:
|
||||
- F12 Route Chunk Manager (via merge_chunks method)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
flight_id: str # Flight identifier
|
||||
new_chunk_id: str # New chunk being merged (source, typically newer/recently anchored)
|
||||
main_chunk_id: str # Main chunk being extended (destination, typically older/established)
|
||||
transform: Sim3Transform:
|
||||
translation: np.ndarray # (3,)
|
||||
rotation: np.ndarray # (3, 3) or quaternion
|
||||
scale: float
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
bool: True if merge successful
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Verify both chunks exist and new_chunk is anchored
|
||||
2. Apply Sim(3) transform to all poses in new_chunk
|
||||
3. Merge new_chunk's subgraph into main_chunk's subgraph
|
||||
4. Update frame-to-chunk mapping (new_chunk frames now belong to main_chunk)
|
||||
5. Mark new_chunk subgraph as merged (F12 handles state updates)
|
||||
6. Optimize merged graph globally
|
||||
|
||||
**Sim(3) Transformation**:
|
||||
- Accounts for translation, rotation, and scale differences
|
||||
- Critical for merging chunks with different scales (monocular VO)
|
||||
- Preserves internal consistency of both chunks
|
||||
|
||||
**Test Cases**:
|
||||
1. **Merge anchored chunks**: new_chunk merged into main_chunk successfully
|
||||
2. **Merge unanchored chunk**: Returns False
|
||||
3. **Global consistency**: Merged trajectory is globally consistent
|
||||
|
||||
---
|
||||
|
||||
### `get_chunk_trajectory(chunk_id: str) -> Dict[int, Pose]`
|
||||
|
||||
**Description**: Retrieves optimized trajectory for a specific chunk.
|
||||
|
||||
**Called By**:
|
||||
- F12 Route Chunk Manager (chunk state queries)
|
||||
- F14 Result Manager (result publishing)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_id: str
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
Dict[int, Pose] # Frame ID -> Pose in chunk's local coordinate system
|
||||
```
|
||||
|
||||
**Test Cases**:
|
||||
1. **Get chunk trajectory**: Returns all poses in chunk
|
||||
2. **Empty chunk**: Returns empty dict
|
||||
3. **After optimization**: Returns optimized poses
|
||||
|
||||
---
|
||||
|
||||
### `get_all_chunks() -> List[ChunkHandle]`
|
||||
|
||||
**Description**: Retrieves all chunks in the factor graph.
|
||||
|
||||
**Called By**:
|
||||
- F11 Failure Recovery Coordinator (chunk matching coordination)
|
||||
- F12 Route Chunk Manager (chunk state queries)
|
||||
|
||||
**Input**: None
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
List[ChunkHandle] # All chunks (active and inactive)
|
||||
```
|
||||
|
||||
**Test Cases**:
|
||||
1. **Multiple chunks**: Returns all chunks
|
||||
2. **No chunks**: Returns empty list
|
||||
3. **Mixed states**: Returns chunks in various states (active, anchored, merged)
|
||||
|
||||
---
|
||||
|
||||
### `optimize_chunk(chunk_id: str, iterations: int) -> OptimizationResult`
|
||||
|
||||
**Description**: Optimizes a specific chunk's subgraph independently.
|
||||
|
||||
**Called By**:
|
||||
- F02 Flight Processor (after chunk anchor added)
|
||||
- F12 Route Chunk Manager (periodic chunk optimization)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
chunk_id: str
|
||||
iterations: int # Max iterations (typically 5-10 for incremental)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
OptimizationResult:
|
||||
converged: bool
|
||||
final_error: float
|
||||
iterations_used: int
|
||||
optimized_frames: List[int]
|
||||
mean_reprojection_error: float
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Extract chunk's subgraph
|
||||
2. Run Levenberg-Marquardt optimization
|
||||
3. Update poses in chunk's local coordinate system
|
||||
4. Return optimization result
|
||||
|
||||
**Test Cases**:
|
||||
1. **Optimize active chunk**: Chunk optimized successfully
|
||||
2. **Optimize anchored chunk**: Optimization improves consistency
|
||||
3. **Chunk isolation**: Other chunks unaffected
|
||||
|
||||
---
|
||||
|
||||
### `optimize_global(iterations: int) -> OptimizationResult`
|
||||
|
||||
**Description**: Optimizes all chunks and performs global merging.
|
||||
|
||||
**Called By**:
|
||||
- Background optimization task (periodic)
|
||||
- F11 Failure Recovery Coordinator (after chunk matching)
|
||||
|
||||
**Input**:
|
||||
```python
|
||||
iterations: int # Max iterations (typically 50-100 for global)
|
||||
```
|
||||
|
||||
**Output**:
|
||||
```python
|
||||
OptimizationResult:
|
||||
converged: bool
|
||||
final_error: float
|
||||
iterations_used: int
|
||||
optimized_frames: List[int] # All frames across all chunks
|
||||
mean_reprojection_error: float
|
||||
```
|
||||
|
||||
**Processing Flow**:
|
||||
1. Collect all chunks
|
||||
2. For anchored chunks, transform to global coordinate system
|
||||
3. Optimize merged global graph
|
||||
4. Update all chunk trajectories
|
||||
5. Return global optimization result
|
||||
|
||||
**Test Cases**:
|
||||
1. **Global optimization**: All chunks optimized together
|
||||
2. **Multiple anchored chunks**: Global consistency achieved
|
||||
3. **Performance**: Completes within acceptable time (<500ms)
|
||||
|
||||
## Integration Tests
|
||||
|
||||
### Test 1: Incremental Trajectory Building
|
||||
1. Initialize graph with first frame
|
||||
2. Add relative factors from VO × 100
|
||||
3. Add altitude priors × 100
|
||||
4. Optimize incrementally after each frame
|
||||
5. Verify smooth trajectory
|
||||
|
||||
### Test 2: Drift Correction with Absolute GPS
|
||||
1. Build trajectory with VO only (will drift)
|
||||
2. Add absolute GPS factor at frame 50
|
||||
3. Optimize → trajectory corrects
|
||||
4. Verify frames 1-49 also corrected (back-propagation)
|
||||
|
||||
### Test 3: Outlier Handling
|
||||
1. Add normal relative factors
|
||||
2. Add 350m outlier factor (tilt error)
|
||||
3. Optimize with robust kernel
|
||||
4. Verify outlier downweighted, trajectory smooth
|
||||
|
||||
### Test 4: User Anchor Integration
|
||||
1. Processing blocked at frame 237
|
||||
2. User provides anchor (high confidence)
|
||||
3. add_absolute_factor(is_user_anchor=True)
|
||||
4. Optimize → trajectory snaps to anchor
|
||||
|
||||
### Test 5: 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 6: Chunk Anchoring and Merging
|
||||
1. Create chunk_1 (frames 1-10), chunk_2 (frames 20-30)
|
||||
2. Add chunk_anchor to chunk_2 (frame 25)
|
||||
3. Optimize chunk_2 → local consistency improved
|
||||
4. Merge chunk_2 into chunk_1 with Sim(3) transform
|
||||
5. Optimize global → both chunks globally consistent
|
||||
6. Verify final trajectory coherent across chunks
|
||||
|
||||
### Test 7: 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
|
||||
|
||||
## Non-Functional Requirements
|
||||
|
||||
### Performance
|
||||
- **Incremental optimize**: < 100ms per frame (iSAM2)
|
||||
- **Batch optimize**: < 500ms for 100 frames
|
||||
- **get_trajectory**: < 10ms
|
||||
- Real-time capable: 10 FPS processing
|
||||
|
||||
### Accuracy
|
||||
- **Mean Reprojection Error (MRE)**: < 1.0 pixels
|
||||
- **GPS accuracy**: Meet 80% < 50m, 60% < 20m criteria
|
||||
- **Trajectory smoothness**: No sudden jumps (except user anchors)
|
||||
|
||||
### Reliability
|
||||
- Numerical stability for 2000+ frame trajectories
|
||||
- Graceful handling of degenerate configurations
|
||||
- Robust to missing/corrupted measurements
|
||||
|
||||
## Dependencies
|
||||
|
||||
### Internal Components
|
||||
- **H03 Robust Kernels**: For Huber/Cauchy loss functions
|
||||
- **H02 GSD Calculator**: For GSD computation and scale resolution
|
||||
|
||||
### External Dependencies
|
||||
- **GTSAM**: Graph optimization library
|
||||
- **numpy**: Matrix operations
|
||||
- **scipy**: Sparse matrix operations (optional)
|
||||
|
||||
## Data Models
|
||||
|
||||
### Pose
|
||||
```python
|
||||
class Pose(BaseModel):
|
||||
frame_id: int
|
||||
position: np.ndarray # (3,) - [x, y, z] in ENU
|
||||
orientation: np.ndarray # (4,) quaternion or (3,3) rotation matrix
|
||||
timestamp: datetime
|
||||
covariance: Optional[np.ndarray] # (6, 6)
|
||||
```
|
||||
|
||||
### RelativePose
|
||||
```python
|
||||
class RelativePose(BaseModel):
|
||||
translation: np.ndarray # (3,)
|
||||
rotation: np.ndarray # (3, 3) or (4,)
|
||||
covariance: np.ndarray # (6, 6)
|
||||
```
|
||||
|
||||
### OptimizationResult
|
||||
```python
|
||||
class OptimizationResult(BaseModel):
|
||||
converged: bool
|
||||
final_error: float
|
||||
iterations_used: int
|
||||
optimized_frames: List[int]
|
||||
mean_reprojection_error: float
|
||||
```
|
||||
|
||||
### FactorGraphConfig
|
||||
```python
|
||||
class FactorGraphConfig(BaseModel):
|
||||
robust_kernel_type: str = "Huber" # or "Cauchy"
|
||||
huber_threshold: float = 1.0 # pixels
|
||||
cauchy_k: float = 0.1
|
||||
isam2_relinearize_threshold: float = 0.1
|
||||
isam2_relinearize_skip: int = 1
|
||||
max_chunks: int = 100 # Maximum number of simultaneous chunks
|
||||
chunk_merge_threshold: float = 0.1 # Error threshold for chunk merging
|
||||
```
|
||||
|
||||
### ChunkHandle
|
||||
```python
|
||||
class ChunkHandle(BaseModel):
|
||||
chunk_id: str
|
||||
flight_id: str
|
||||
start_frame_id: int
|
||||
end_frame_id: Optional[int]
|
||||
frames: List[int]
|
||||
is_active: bool
|
||||
has_anchor: bool
|
||||
anchor_frame_id: Optional[int]
|
||||
anchor_gps: Optional[GPSPoint]
|
||||
matching_status: str # "unanchored", "matching", "anchored", "merged"
|
||||
```
|
||||
|
||||
### Sim3Transform
|
||||
```python
|
||||
class Sim3Transform(BaseModel):
|
||||
translation: np.ndarray # (3,) - translation vector
|
||||
rotation: np.ndarray # (3, 3) rotation matrix or (4,) quaternion
|
||||
scale: float # Scale factor
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user