add chunking

2026-04-23 02:46:35 +00:00 · 2025-11-27 03:43:19 +02:00
parent 4f8c18a066
commit 2037870f67
43 changed files with 7041 additions and 4135 deletions
@@ -0,0 +1,763 @@
+# Factor Graph Optimizer
+
+## Interface Definition
+
+**Interface Name**: `IFactorGraphOptimizer`
+
+### Interface Methods
+
+```python
+class IFactorGraphOptimizer(ABC):
+    @abstractmethod
+    def add_relative_factor(self, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
+        pass
+    
+    @abstractmethod
+    def add_absolute_factor(self, frame_id: int, gps: GPSPoint, covariance: np.ndarray, is_user_anchor: bool) -> bool:
+        pass
+    
+    @abstractmethod
+    def add_altitude_prior(self, frame_id: int, altitude: float, covariance: float) -> bool:
+        pass
+    
+    @abstractmethod
+    def optimize(self, iterations: int) -> OptimizationResult:
+        pass
+    
+    @abstractmethod
+    def get_trajectory(self) -> Dict[int, Pose]:
+        pass
+    
+    @abstractmethod
+    def get_marginal_covariance(self, frame_id: int) -> np.ndarray:
+        pass
+    
+    @abstractmethod
+    def create_new_chunk(self, chunk_id: str, start_frame_id: int) -> ChunkHandle:
+        pass
+    
+    @abstractmethod
+    def get_chunk_for_frame(self, frame_id: int) -> Optional[ChunkHandle]:
+        pass
+    
+    @abstractmethod
+    def add_relative_factor_to_chunk(self, chunk_id: str, frame_i: int, frame_j: int, relative_pose: RelativePose, covariance: np.ndarray) -> bool:
+        pass
+    
+    @abstractmethod
+    def add_chunk_anchor(self, chunk_id: str, frame_id: int, gps: GPSPoint, covariance: np.ndarray) -> bool:
+        pass
+    
+    @abstractmethod
+    def merge_chunks(self, chunk_id_1: str, chunk_id_2: str, transform: Sim3Transform) -> bool:
+        pass
+    
+    @abstractmethod
+    def get_chunk_trajectory(self, chunk_id: str) -> Dict[int, Pose]:
+        pass
+    
+    @abstractmethod
+    def get_all_chunks(self) -> List[ChunkHandle]:
+        pass
+    
+    @abstractmethod
+    def optimize_chunk(self, chunk_id: str, iterations: int) -> OptimizationResult:
+        pass
+    
+    @abstractmethod
+    def optimize_global(self, iterations: int) -> OptimizationResult:
+        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
+- **Native multi-chunk/multi-map support (Atlas architecture)**
+- **Chunk lifecycle management (creation, optimization, merging)**
+- **Sim(3) transformation for chunk merging**
+
+### 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**
+
+## 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 from H02 GSD Calculator:
+   - 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
+
+**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_chunks(chunk_id_1: str, chunk_id_2: str, transform: Sim3Transform) -> bool`
+
+**Description**: Merges two chunks using Sim(3) similarity transformation.
+
+**Called By**:
+- F11 Failure Recovery Coordinator (after chunk matching)
+- Background optimization task
+
+**Input**:
+```python
+chunk_id_1: str  # Source chunk (typically newer)
+chunk_id_2: str  # Target chunk (typically older, merged into)
+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 chunk_id_1 is anchored
+2. Apply Sim(3) transform to all poses in chunk_id_1
+3. Merge chunk_id_1's subgraph into chunk_id_2's subgraph
+4. Update frame-to-chunk mapping
+5. Mark chunk_id_1 as merged
+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**: Chunks merged 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 coordinate conversions
+
+### 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
+```
+