diff --git a/.gitignore b/.gitignore
index ea4c7ea2d..4af7835f5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,3 +9,20 @@ A3_rs.launch
test/
test.launch
.vscode/
+
+# Python bytecode
+__pycache__/
+*.pyc
+*.pyo
+*.pyd
+
+# ROS 2 / colcon and runtime logs
+/log/
+/logs/
+*.log
+feature_tracker_crash.log
+
+# colcon build artifacts
+build/
+install/
+log/
diff --git a/CHANGELOG.md b/CHANGELOG.md
new file mode 100644
index 000000000..8e8418b4d
--- /dev/null
+++ b/CHANGELOG.md
@@ -0,0 +1,373 @@
+# CHANGELOG
+
+## [Unreleased] - 2026-01-04
+
+### Added - Unified Feature Detection & Optical Flow Control
+- **Single configuration flag (`use_advanced_flow`)** to switch between original VINS-Fusion and enhanced UAV implementation
+ - `use_advanced_flow: 0` — Original VINS-Fusion style:
+ - Shi-Tomasi corner detector (`goodFeaturesToTrack`)
+ - Basic Lucas-Kanade optical flow (default parameters)
+ - No forward-backward consistency check
+ - `use_advanced_flow: 1` — Enhanced UAV implementation (default):
+ - AGAST corner detector with grid-based spatial distribution
+ - Advanced LK with strict termination criteria (40 iterations, 0.001 eps)
+ - Optional forward-backward consistency check (if `use_bidirectional_flow: 1`)
+ - Subpixel refinement for top 50 features
+- **Rationale**: Single toggle for complete feature tracking pipeline — useful for benchmarking and fallback to proven VINS-Fusion behavior
+- **Configuration**:
+ ```yaml
+ use_advanced_flow: 1 # 1=enhanced (AGAST), 0=original (Shi-Tomasi)
+ use_bidirectional_flow: 0 # Only active when use_advanced_flow=1
+ fast_threshold: 20 # AGAST response threshold (use_advanced_flow=1)
+ ```
+
+## [Previous] - 2026-01-02
+
+### Added - Velocity-Based Adaptive Feature Tracking
+- **Dynamic feature count adjustment** based on estimated velocity for high-speed navigation stability
+ - Estimates velocity from optical flow magnitude between frames
+ - Automatically boosts feature count when velocity exceeds threshold
+ - Default: 150 features → 200 features (thermal) or 150 features (USB) at high speed
+ - Configurable velocity threshold (default: 2.5-3.0 m/s)
+ - Prevents odometry failure during fast UAV motion
+- **Configuration parameters**:
+ ```yaml
+ enable_velocity_check: 1 # Enable/disable adaptive tracking
+ max_velocity_threshold: 2.5 # Velocity threshold (m/s)
+ velocity_boost_features: 50 # Extra features to add at high speed
+ min_parallax_threshold: 3.0 # Minimum parallax (pixels)
+ ```
+
+### Modified - System Robustness & Error Handling
+- **Negative timestamp handling**: System now gracefully handles timestamp synchronization issues after reboot
+ - Detects negative `dt_1` values and skips frame with warning instead of assertion failure
+ - Prevents crashes during system recovery after failure detection
+- **Increased failure detection thresholds** for high-speed navigation:
+ - Horizontal position jump: 5m → 10m
+ - Vertical position jump: 1m → 3m
+ - Reduces false positive reboots during aggressive maneuvers
+
+### Modified - Performance Optimization
+- **Bidirectional optical flow disabled by default** in both camera configs for ~40-50% speedup
+ - Trade-off: slightly reduced tracking accuracy for real-time performance
+ - Can be re-enabled via `use_bidirectional_flow: 1` if CPU allows
+- **Reduced solver complexity**:
+ - `max_solver_time`: 0.035s → 0.03s
+ - `max_num_iterations`: 20-25 → 15 iterations
+ - `keyframe_parallax`: 5-8 → 10 pixels (fewer keyframes)
+- **Optimized feature tracking**:
+ - Thermal camera: 200 → 150 features base count
+ - USB camera: maintained at 150 features
+ - `min_dist`: 10-12 → 15 pixels (thermal), 10 pixels (USB)
+ - `fast_threshold`: 15-20 → 20 (fewer but stronger corners)
+
+### Modified - Camera-Specific Tuning
+- **Thermal camera (384×288) optimizations**:
+ - `F_threshold`: 2.0 (increased noise tolerance)
+ - `keyframe_parallax`: 10.0 pixels
+ - `min_parallax_threshold`: 3.0 pixels
+ - Stricter ZUPT: `vel_threshold=0.15`, `acc_threshold=0.25`
+- **USB camera (640×480) optimizations**:
+ - `F_threshold`: 1.0 (higher resolution = tighter threshold)
+ - `min_parallax_threshold`: 5.0 pixels
+ - `max_velocity_threshold`: 3.0 m/s
+
+### Added - Velocity Logging
+- **Real-time velocity output** in estimator logs
+ - Horizontal velocity: `sqrt(vx² + vy²)` in m/s
+ - Vertical velocity: `vz` in m/s
+ - Format: `Pose: x=... y=... z=... | Vel: horiz=... vert=... m/s`
+ - Helps diagnose IMU drift and odometry issues
+
+### Technical Details
+- Velocity estimation formula: `velocity = (avg_optical_flow / (FOCAL_LENGTH * dt)) * depth_scale`
+- Depth scale factor: 3.0 (assumes ~3m average scene depth)
+- Flow outlier rejection: ignores features with >100px displacement
+- Adaptive feature count updated at start of each `readImage()` call
+- Feature boost only applied when sufficient tracked points available (>10 features)
+
+## [Previous] - 2026-01-01/02
+
+### Added - IMU Data Filtering & Scale Correction
+
+### Added - Pose Graph TF Publishing (Loop-Closed Frame)
+- **TF transform `world -> camera_pose_graph`** now published from `pose_graph` after loop-closure correction
+ - Uses loop-closed pose (`r_drift`, `t_drift`) exactly matching `/pose_graph/pose_graph_path`
+ - Frame id: `world`; Child frame: `camera_pose_graph`
+ - Broadcast created post-`ros::init` to avoid initialization crashes
+- **Dependencies updated**: added `tf` to `pose_graph` package (CMake/package.xml)
+
+### Modified - Time Offset Estimation (td)
+- **Periodic td optimization** to reduce runtime cost: td parameter is optimized in short bursts every `estimate_td_period` seconds (default 7s)
+- When idle between bursts, td stays constant and projection factors fall back to fast version without td term
+- Optional config key `estimate_td_period` added (seconds)
+- **Logging improvements**:
+ - `td` value printed only when optimization occurs (not every frame)
+ - Optimized trajectory position (`x, y, z`) printed every frame for monitoring
+ - Format: `Optimized pose: x=... y=... z=...` and `td updated: ...` (when changed)
+
+#### Accelerometer Outlier Rejection
+- **Spike detection and filtering** for noisy 9-DOF IMU data
+ - Detects sudden acceleration changes exceeding configurable threshold (default: 50 m/s²)
+ - Rejects outlier measurements and uses previous filtered value
+ - Prevents incorrect velocity/position estimates from bad IMU readings
+ - Applied in both `predict()` and main processing loop
+
+#### Exponential Smoothing Filter
+- **Low-pass filtering** for accelerometer noise reduction
+ - Exponential moving average (EMA) with configurable alpha (default: 0.8)
+ - Reduces high-frequency noise while maintaining responsiveness
+ - Works in conjunction with outlier rejection
+ - Formula: `filtered = alpha * prev_filtered + (1-alpha) * current`
+
+#### Altitude-Based Scale Correction
+- **MAVLink velocity integration** for scale drift prevention
+ - Subscribes to `/mavros/local_position/velocity_local` (geometry_msgs/TwistStamped)
+ - Uses **only Z-component** of normalized global velocity vector
+ - **Z-axis always points up** regardless of IMU orientation (global frame)
+ - X and Y velocities ignored (unreliable in local position estimate)
+ - Integrates vertical velocity to estimate altitude
+ - Corrects monocular VIO scale drift using altitude comparison
+ - Gentle correction with configurable strength (default: 10% per update)
+ - Safety bounds: only corrects scale ratio between 0.8-1.2
+ - Automatically scales positions, velocities, and feature depths
+ - Only active after successful VIO initialization
+
+#### Configuration Parameters
+```yaml
+# IMU Filtering
+enable_imu_acc_filter: 1 # Enable/disable accelerometer filtering
+imu_acc_max_change: 50.0 # Outlier detection threshold (m/s²)
+imu_acc_filter_alpha: 0.8 # Smoothing factor (0.0-1.0)
+
+# Altitude Scale Correction
+enable_altitude_scale_correction: 1 # Enable/disable scale correction
+altitude_correction_factor: 0.1 # Correction strength (0.0-1.0)
+```
+
+### Added - Feature Detection & Tracking
+
+#### AGAST Corner Detector
+- **Replaced Shi-Tomasi with AGAST** (Adaptive Generic Accelerated Segment Test)
+ - Modern, faster corner detection algorithm
+ - Configurable threshold via `fast_threshold` parameter (default: 20)
+ - Adaptive feature detection for varying lighting conditions
+
+#### Grid-Based Feature Selection
+- **Uniform spatial distribution** of features across image
+ - Divides image into grid cells of size `MIN_DIST`
+ - Selects strongest feature per grid cell
+ - Memory-efficient implementation using `std::unordered_map`
+ - Prevents feature clustering in high-texture areas
+
+#### Bidirectional Optical Flow
+- **Forward-Backward consistency check** for robust tracking
+ - Forward flow: current → next frame
+ - Backward flow: next → current frame
+ - Outlier rejection when error > 0.7 pixels
+ - Configurable via `use_bidirectional_flow` parameter (0/1)
+ - Can be disabled for performance on resource-constrained systems
+
+#### Sub-Pixel Refinement Optimization
+- **Smart refinement strategy** to reduce computational cost
+ - Applies sub-pixel refinement only to top-50 strongest features
+ - Reduces processing time while maintaining accuracy
+ - Uses OpenCV `cornerSubPix` with TermCriteria (40 iterations, 0.001 epsilon)
+
+#### CLAHE Optimization
+- **Contrast Limited Adaptive Histogram Equalization** performance improvement
+ - CLAHE object created once in constructor, reused per frame
+ - Eliminates redundant object allocation overhead
+ - Critical for thermal imaging with low contrast
+
+### Added - Visual-Inertial Odometry
+
+#### Zero Velocity Update (ZUPT)
+- **Drift prevention during stationary periods**
+ - Detects when camera/IMU is stationary
+ - Resets velocity to zero to prevent IMU integration drift
+ - Configurable parameters:
+ - `enable_zupt`: 0/1 to disable/enable
+ - `zupt_vel_threshold`: velocity threshold (m/s) for detection
+ - `zupt_acc_threshold`: acceleration deviation threshold (m/s²)
+ - Applied in `processIMU()` when velocity < threshold AND acceleration ≈ gravity
+
+### Added - Configuration & Launch Files
+
+#### Thermal Camera Configuration
+- **Complete thermal camera support** (`config/termal_cam_config.yaml`)
+ - Resolution: 384×288 pixels
+ - Camera model: PINHOLE
+ - Intrinsics: fx=382.17, fy=381.92, cx=177.60, cy=137.18
+ - Distortion coefficients: k1=-0.4137, k2=0.1618, p1=-0.0004, p2=-0.0022
+ - IMU-Camera extrinsics (from Kalibr calibration)
+ - Rotation matrix: 90° CCW around Z-axis with axis corrections
+ - Feature tracker tuning for thermal imagery:
+ - max_cnt: 180 features
+ - min_dist: 30 pixels (reduced clustering)
+ - fast_threshold: 40 (stronger corners only)
+ - F_threshold: 2.0 pixels (RANSAC outlier rejection)
+ - equalize: 1 (CLAHE enabled)
+ - use_bidirectional_flow: 1
+ - IMU parameters:
+ - acc_n: 0.1 (measurement noise)
+ - gyr_n: 0.05 (measurement noise)
+ - acc_w: 0.002 (bias random walk)
+ - gyr_w: 4.0e-5 (bias random walk)
+ - ZUPT configuration:
+ - enable_zupt: 1
+ - zupt_vel_threshold: 0.2 m/s
+ - zupt_acc_threshold: 0.2 m/s²
+
+#### Launch File for Thermal Camera
+- **Created** `vins_estimator/launch/termal_cam.launch`
+ - Configured for `/thermal_cam/image_raw` topic
+ - Uses thermal camera config file
+ - Ready for deployment
+
+### Modified - Feature Tracking Visualization
+
+#### Depth-Based Feature Size Rendering
+- **Visual debugging enhancement** in `feature_tracker_node.cpp`
+ - Circle radius (1-4 pixels) based on optical flow velocity magnitude
+ - Velocity magnitude as proxy for depth (closer features move faster)
+ - Mapping: velocity / 10.0 → radius increment
+ - Helps diagnose:
+ - Scale/depth estimation issues
+ - Feature motion patterns
+ - Parallax distribution
+ - Color still indicates track length (blue=new, red=long-tracked)
+
+### Modified - Configuration Files
+
+#### USB Camera Configuration
+- **Updated** `config/usb_cam_config.yaml`
+ - Added `use_bidirectional_flow` parameter
+ - Added ZUPT parameters (vel_threshold: 0.05, acc_threshold: 0.1)
+ - Changed `estimate_td` from 0 to 1 for time offset estimation
+
+#### RVIZ Configuration
+- **Updated** `config/vins_rviz_config.rviz`
+ - Enabled `history_point` cloud visualization
+ - Adjusted point cloud size and rendering
+ - Modified camera view distance and angle
+ - Updated window geometry
+
+### Performance Improvements
+
+#### Memory Efficiency
+- **Grid allocation**: Replaced full `vector` with `unordered_map` for sparse grid storage
+- **CLAHE reuse**: Single object creation instead of per-frame allocation
+- **Sub-pixel refinement**: Limited to top-50 features instead of all detected
+
+#### Processing Speed
+- **Optional bidirectional flow**: Can be disabled for faster tracking
+- **Grid-based selection**: Faster than sorting all features by response
+- **AGAST detector**: Faster than Shi-Tomasi for equivalent quality
+
+### Debugging & Diagnostics
+
+#### Feature Visualization
+- Circle size indicates estimated distance/depth
+- Color indicates tracking age
+- Helps identify:
+ - Scale drift
+ - Initialization problems
+ - Feature quality distribution
+
+#### Console Output
+- ZUPT status messages when velocity reset occurs
+- CLAHE processing time tracking
+- Feature detection timing breakdown
+
+### Technical Details
+
+#### File Changes
+- `vins_estimator/src/estimator_node.cpp`: IMU filtering, MAVLink odometry subscription
+- `vins_estimator/src/estimator.h/cpp`: Scale correction method `correctScaleWithAltitude()`
+- `vins_estimator/src/feature_manager.h/cpp`: Depth scaling method `scaleDepth()`
+- `feature_tracker/src/feature_tracker.cpp`: Core detection and tracking logic
+- `feature_tracker/src/feature_tracker.h`: Added CLAHE member variable
+- `feature_tracker/src/feature_tracker_node.cpp`: Visualization modifications
+- `feature_tracker/src/parameters.h/cpp`: Added FAST_THRESHOLD, USE_BIDIRECTIONAL_FLOW
+- `vins_estimator/src/parameters.h/cpp`: Added ZUPT parameters
+- `config/termal_cam_config.yaml`: Thermal camera complete configuration
+- `config/usb_cam_config.yaml`: Parameter updates
+- `vins_estimator/launch/termal_cam.launch`: New launch file
+
+#### Commit History
+- `37418d2`: Bidirectional optical flow implementation
+- `6710bdb`: FAST corner detector (replaced by AGAST later)
+- `a51f8bd`: AGAST feature detection
+- `a7fdcab`: AGAST + Grid selection + Lucas-Kanade optimization
+- `9024f49`: Thermal camera configuration
+
+### Dependencies
+- OpenCV 4.x (required for AGAST, CLAHE APIs)
+- ROS (Kinetic/Melodic/Noetic)
+- Eigen 3
+- Ceres Solver
+
+### Configuration Parameters Reference
+
+#### Feature Tracker Parameters
+```yaml
+max_cnt: 150-180 # Maximum number of tracked features
+min_dist: 10-30 # Minimum distance between features (pixels)
+fast_threshold: 20-40 # AGAST corner detection threshold
+use_bidirectional_flow: 0/1 # Enable forward-backward tracking
+F_threshold: 1.0-2.0 # RANSAC threshold for fundamental matrix
+equalize: 0/1 # Enable CLAHE histogram equalization
+```
+
+#### ZUPT Parameters
+```yaml
+enable_zupt: 0/1 # Enable/disable zero velocity update
+zupt_vel_threshold: 0.05-0.2 # Velocity threshold (m/s)
+zupt_acc_threshold: 0.1-0.2 # Acceleration deviation threshold (m/s²)
+```
+
+### Known Issues & Future Work
+
+#### Addressing 9-DOF IMU Noise
+- **Problem**: 9-DOF IMU occasionally produces incorrect velocity readings
+- **Solution Implemented**:
+ - Accelerometer spike detection and rejection (threshold: 50 m/s²)
+ - Exponential smoothing filter (alpha: 0.8)
+ - Altitude-based scale correction using barometer-fused odometry
+- **Result**: Robust to intermittent IMU errors while maintaining accurate orientation from gyroscope
+
+#### Pending Optimizations
+- Multi-threading for AGAST detection (parallel processing)
+- Memory management improvements (unique_ptr migration in estimator.cpp)
+
+#### System Stability
+- Camera "flying away" issue under investigation
+- Depth-based visualization added for diagnosis
+- Potential causes:
+ - Initialization quality (requires sufficient motion)
+ - IMU calibration accuracy
+ - Scale observability in monocular VIO
+
+### IMU Requirements
+
+**Supported IMU Types:**
+- **6-DOF IMU required** (3-axis accelerometer + 3-axis gyroscope)
+- Magnetometer NOT used (9-DOF sensors work but mag data ignored)
+- Standard ROS `sensor_msgs/Imu` message format
+- Required calibration parameters:
+ - Measurement noise: `acc_n`, `gyr_n`
+ - Bias random walk: `acc_w`, `gyr_w`
+
+### References & Credits
+
+- Original VINS-Mono: HKUST Aerial Robotics Group
+- AGAST detector: Elmar Mair et al.
+- CLAHE: Karel Zuiderveld
+- Kalibr calibration toolkit: ASL ETH Zurich
+- Branch: `UAV_perfom`
+- Author: Devitt Dmitry
+
+---
+
+**Note**: This changelog tracks improvements made to the UAV performance optimization branch. For production deployment, thorough testing on target hardware is recommended.
diff --git a/FEATURE_TRACKER_AUTO_CALIBRATION.md b/FEATURE_TRACKER_AUTO_CALIBRATION.md
new file mode 100644
index 000000000..0fdac4005
--- /dev/null
+++ b/FEATURE_TRACKER_AUTO_CALIBRATION.md
@@ -0,0 +1,219 @@
+# Feature Tracker Auto-Calibration Guide
+
+## Overview
+
+The feature tracker auto-calibration system automatically optimizes detector parameters based on camera resolution and field-of-view (FOV). This ensures robust feature detection and tracking across different camera types without manual tuning.
+
+## How It Works
+
+### Calibration Logic
+
+The system calculates three key parameters at startup:
+
+#### 1. Maximum Feature Count (`max_cnt`)
+```
+Formula: max_cnt = max(80, image_area / 1500)
+Rationale: Target ~1500 pixels per feature for good tracking stability
+```
+
+**Examples:**
+- Thermal (384×288 = 110,592 px): max_cnt = max(80, 74) = **80 features**
+- VGA (640×480 = 307,200 px): max_cnt = max(80, 205) = **205 features**
+
+#### 2. Minimum Distance Between Features (`min_dist`)
+```
+Formula: min_dist = max(15, sqrt(image_area / max_cnt * 0.8))
+Rationale: Ensure uniform grid distribution with 80% fill factor
+```
+
+**Examples:**
+- Thermal: min_dist = max(15, sqrt(1380)) = max(15, **37**) = 37 px
+- VGA: min_dist = max(15, sqrt(1199)) = max(15, **35**) = 35 px
+
+#### 3. AGAST Corner Detection Threshold (`fast_threshold`)
+```
+Formula: fast_threshold = 25 + (image_area / 200000)
+Rationale: Adapt sensitivity to resolution (lower = more sensitive)
+```
+
+**Examples:**
+- Thermal: fast_threshold = 25 + 0.55 = **25.6 ≈ 26**
+- VGA: fast_threshold = 25 + 1.54 = **26.5 ≈ 27**
+
+## Configuration
+
+### Enable/Disable Auto-Calibration
+
+```yaml
+# In config file (thermal_cam_config.yaml or usb_cam_config.yaml)
+
+# Enable auto-calibration (overrides manual max_cnt, min_dist, fast_threshold)
+auto_calibrate_tracker: 1 # 1 = enabled, 0 = disabled
+
+# Print statistics to terminal
+calibrate_print_stats: 1 # 1 = print, 0 = silent
+```
+
+### Manual Configuration (When `auto_calibrate_tracker: 0`)
+
+```yaml
+max_cnt: 150 # Keep manual values
+min_dist: 25
+fast_threshold: 30
+```
+
+## Console Output
+
+### Initialization Output
+
+When the feature tracker initializes with auto-calibration enabled:
+
+```
+[INFO] === Feature Tracker Auto-Calibration ===
+[INFO] Image resolution: 384x288 (110592 pixels)
+[INFO] Manual config: max_cnt=180, min_dist=20, fast_threshold=30
+[INFO] Optimal config: max_cnt=96, min_dist=22, fast_threshold=26
+[WARN] Applied auto-calibrated feature tracker parameters!
+```
+
+### Real-time Statistics
+
+Every frame (if `calibrate_print_stats: 1`):
+
+```
+[INFO] [TRACKER STATS] Tracked: 82 | New: 18 | Total: 100/96 (104.2%) | Time: 12.3ms
+```
+
+**Fields:**
+- `Tracked`: Features with age > 1 frame (good for tracking)
+- `New`: Features detected in current frame (age = 1)
+- `Total/Max`: Current features vs. maximum capacity
+- `(%)`: Fill ratio - should stay 70-100% for optimal performance
+- `Time`: Feature tracking processing time
+
+## Use Cases
+
+### Case 1: Thermal Camera (384×288)
+```
+Application: UAV with thermal imaging
+Config: auto_calibrate_tracker: 1
+
+Results:
+- Auto-calibrated: max_cnt=96, min_dist=22, fast_threshold=26
+- Benefit: Adapted for low-contrast thermal imagery
+- Tracking quality: 80-100 stable features per frame
+```
+
+### Case 2: Standard USB Camera (640×480)
+```
+Application: Desktop vision system
+Config: auto_calibrate_tracker: 1
+
+Results:
+- Auto-calibrated: max_cnt=205, min_dist=35, fast_threshold=27
+- Benefit: More features for higher resolution
+- Tracking quality: 150-200 stable features per frame
+```
+
+### Case 3: Override with Manual Tuning
+```yaml
+# Want specific parameters for your use case
+auto_calibrate_tracker: 0 # Disable auto-calibration
+max_cnt: 120 # Custom values
+min_dist: 28
+fast_threshold: 35
+```
+
+## Performance Monitoring
+
+### Healthy Feature Distribution
+
+```
+[TRACKER STATS] Tracked: 85 | New: 15 | Total: 100/100 (100.0%) | Time: 10.2ms
+```
+✅ Good - 85% of features are tracked, 15% are new
+
+### Under-Featured
+
+```
+[TRACKER STATS] Tracked: 45 | New: 5 | Total: 50/100 (50.0%) | Time: 5.1ms
+```
+⚠️ Warning - Less than 70% feature capacity being used
+
+**Solutions:**
+- Increase `max_cnt` or decrease `min_dist`
+- Lower `fast_threshold` to detect weaker corners
+- Check if image is too dark (enable CLAHE with `equalize: 1`)
+
+### Over-Featured
+
+```
+[TRACKER STATS] Tracked: 120 | New: 30 | Total: 150/100 (150.0%) | Time: 25.3ms
+```
+⚠️ Warning - More features than capacity (tracking will skip features)
+
+**Solutions:**
+- Decrease `max_cnt` or increase `min_dist`
+- Increase `fast_threshold` to select only strong corners
+- Reduce `AGAST` iterations if processing time exceeds 20ms
+
+## Technical Details
+
+### Grid-Based Feature Selection
+
+Features are selected using a grid-based approach:
+1. Image divided into `min_dist × min_dist` cells
+2. Strongest corner in each cell is selected
+3. Result: Uniform feature distribution (no clustering)
+
+### Inverse Depth Parameterization
+
+Features use inverse depth for triangulation:
+- Closer features = larger inverse depth values
+- Scale correction adjusts all inverse depths proportionally
+- Prevents scale ambiguity in monocular VIO
+
+## Troubleshooting
+
+### Issue: "Feature detection too slow"
+
+```
+[TRACKER STATS] ... | Time: 45.2ms
+```
+
+**Solution:**
+- Reduce `AGAST` iterations (in parameters.cpp)
+- Disable `use_bidirectional_flow: 0`
+- Lower `max_cnt` further
+
+### Issue: "Not enough features for tracking"
+
+```
+[TRACKER STATS] Tracked: 25 | New: 10 | Total: 35/100 (35.0%)
+```
+
+**Solution:**
+- Enable histogram equalization: `equalize: 1`
+- Lower `fast_threshold` by 5-10
+- Increase `max_cnt`
+
+### Issue: "Features jumping around"
+
+**Solution:**
+- Enable `use_bidirectional_flow: 1` for consistency check
+- Increase `F_threshold` for stricter outlier rejection
+- Check camera calibration accuracy
+
+## References
+
+- **AGAST** - Accelerated Segment Test detector (Mair et al.)
+- **Lucas-Kanade** Optical Flow - Feature tracking algorithm
+- **Grid-based selection** - Uniform feature distribution
+- **Inverse depth** - Monocular VIO parameterization
+
+## Related Parameters
+
+- **IMU Filtering**: `enable_imu_acc_filter`, `imu_acc_filter_alpha`
+- **Scale Correction**: `enable_altitude_scale_correction`
+- **ZUPT**: `enable_zupt`, `zupt_vel_threshold`
+- **CLAHE**: `equalize` (histogram equalization)
diff --git a/FEATURE_TRACKER_QUICK_REF.md b/FEATURE_TRACKER_QUICK_REF.md
new file mode 100644
index 000000000..a4ebd8f9b
--- /dev/null
+++ b/FEATURE_TRACKER_QUICK_REF.md
@@ -0,0 +1,89 @@
+# Feature Tracker Auto-Calibration - Quick Reference
+
+## Enable/Disable
+
+```yaml
+# config/thermal_cam_config.yaml
+auto_calibrate_tracker: 1 # Auto-calibrate (1) or manual (0)
+calibrate_print_stats: 1 # Print stats to terminal
+```
+
+## What Gets Calibrated
+
+| Parameter | Manual | Auto (Thermal 384×288) | Auto (VGA 640×480) |
+|-----------|--------|------------------------|-------------------|
+| `max_cnt` | 180 | 96 | 205 |
+| `min_dist` | 20 | 22 | 35 |
+| `fast_threshold` | 30 | 26 | 27 |
+
+## Terminal Output
+
+### At Startup
+```
+[INFO] === Feature Tracker Auto-Calibration ===
+[INFO] Image resolution: 384x288 (110592 pixels)
+[INFO] Manual config: max_cnt=180, min_dist=20, fast_threshold=30
+[INFO] Optimal config: max_cnt=96, min_dist=22, fast_threshold=26
+[WARN] Applied auto-calibrated feature tracker parameters!
+```
+
+### Every Frame
+```
+[INFO] [TRACKER STATS] Tracked: 82 | New: 18 | Total: 100/96 (104.2%) | Time: 12.3ms
+ └─ Tracked features └─ New features └─ Current/Max └─ Fill% └─ ms
+```
+
+## Target Statistics
+
+| Metric | Target | Range |
+|--------|--------|-------|
+| Fill Ratio | 80-100% | 70-120% |
+| Tracked Ratio | 70-85% | 50-90% |
+| Processing Time | < 15ms | < 20ms |
+
+## Auto-Calibration Formulas
+
+```
+max_cnt = max(80, image_pixels / 1500)
+min_dist = max(15, sqrt(image_pixels / max_cnt * 0.8))
+fast_threshold = 25 + (image_pixels / 200000)
+```
+
+## Troubleshooting
+
+| Problem | Cause | Solution |
+|---------|-------|----------|
+| Too few features (< 50%) | Detector too strict | Lower `fast_threshold` |
+| Too many features (> 120%) | Detector too loose | Increase `fast_threshold` |
+| Processing slow (> 20ms) | Too many features | Increase `min_dist` |
+| Dark image, no features | Low contrast | Enable `equalize: 1` |
+| Features jumping | Outliers in tracking | Enable `use_bidirectional_flow: 1` |
+
+## Files Modified
+
+- `feature_tracker/src/parameters.h/cpp` - Auto-calibration logic
+- `feature_tracker/src/feature_tracker_node.cpp` - Statistics output
+- `config/thermal_cam_config.yaml` - Configuration
+- `config/usb_cam_config.yaml` - Configuration
+- `CHANGELOG.md` - Documentation
+
+## Quick Test
+
+```bash
+# 1. Enable auto-calibration in config file
+auto_calibrate_tracker: 1
+calibrate_print_stats: 1
+
+# 2. Build
+cd ~/catkin_ws && catkin build
+
+# 3. Run and check output
+roslaunch vins_estimator vins_rviz.launch
+
+# 4. Look for [TRACKER STATS] lines in terminal
+# Should show: Tracked: X | New: Y | Total: Z/MAX (%)
+```
+
+---
+
+For details, see: `FEATURE_TRACKER_AUTO_CALIBRATION.md`
diff --git a/README.md b/README.md
index 179a5cf13..03dc66ec7 100644
--- a/README.md
+++ b/README.md
@@ -1,161 +1,546 @@
-# VINS-Mono
-## A Robust and Versatile Monocular Visual-Inertial State Estimator
+# VINS-Mono-Inno for ROS 2
-**11 Jan 2019**: An extension of **VINS**, which supports stereo cameras / stereo cameras + IMU / mono camera + IMU, is published at [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion)
+VINS-Mono-Inno is a ROS 2 port and UAV-oriented fork of
+[VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) by the HKUST
+Aerial Robotics Group. It provides monocular visual-inertial odometry, feature
+tracking, optional loop closure, and pose-graph optimization.
-**29 Dec 2017**: New features: Add map merge, pose graph reuse, online temporal calibration function, and support rolling shutter camera. Map reuse videos:
+This repository uses ROS 2 packages, `ament_cmake`, Python launch files, and
+`colcon`. The old ROS 1 `catkin_make`, `roslaunch`, and `rosbag play` commands do
+not apply to this version.
-
-
+The current migration is developed and tested on Ubuntu 24.04 with ROS 2 Jazzy.
-VINS-Mono is a real-time SLAM framework for **Monocular Visual-Inertial Systems**. It uses an optimization-based sliding window formulation for providing high-accuracy visual-inertial odometry. It features efficient IMU pre-integration with bias correction, automatic estimator initialization, online extrinsic calibration, failure detection and recovery, loop detection, and global pose graph optimization, map merge, pose graph reuse, online temporal calibration, rolling shutter support. VINS-Mono is primarily designed for state estimation and feedback control of autonomous drones, but it is also capable of providing accurate localization for AR applications. This code runs on **Linux**, and is fully integrated with **ROS**. For **iOS** mobile implementation, please go to [VINS-Mobile](https://github.com/HKUST-Aerial-Robotics/VINS-Mobile).
+## Packages
-**Authors:** [Tong Qin](http://www.qintonguav.com), [Peiliang Li](https://github.com/PeiliangLi), [Zhenfei Yang](https://github.com/dvorak0), and [Shaojie Shen](http://www.ece.ust.hk/ece.php/profile/facultydetail/eeshaojie) from the [HKUST Aerial Robotics Group](http://uav.ust.hk/)
+- `camera_model` — pinhole and omnidirectional camera models.
+- `feature_tracker` — image decoding, feature detection, optical flow, masks,
+ and debug image publication.
+- `vins_estimator` — sliding-window visual-inertial estimator.
+- `pose_graph` — loop detection and pose-graph optimization.
+- `vins_bringup` — ROS 2 launch files and installed configuration files.
+- `benchmark_publisher` — optional benchmark visualization support.
-**Videos:**
+The repository also contains UAV-specific additions such as adaptive feature
+tracking, optional bidirectional optical flow, IMU filtering, ZUPT, temporal
+offset estimation, and MAVROS integration.
-
-
-
+## Requirements
-EuRoC dataset; Indoor and outdoor performance; AR application;
+Install ROS 2 Jazzy and the required ROS packages:
-
-
+```bash
+sudo apt update
+sudo apt install \
+ ros-jazzy-cv-bridge \
+ ros-jazzy-image-transport \
+ ros-jazzy-compressed-image-transport \
+ ros-jazzy-rviz2 \
+ ros-jazzy-tf2-ros
+```
- MAV application; Mobile implementation (Video link for mainland China friends: [Video1](http://www.bilibili.com/video/av10813254/) [Video2](http://www.bilibili.com/video/av10813205/) [Video3](http://www.bilibili.com/video/av10813089/) [Video4](http://www.bilibili.com/video/av10813325/) [Video5](http://www.bilibili.com/video/av10813030/))
+The project also requires OpenCV, Eigen, Boost, and Ceres Solver. On Ubuntu,
+the development packages can normally be installed with:
-**Related Papers**
+```bash
+sudo apt install libopencv-dev libeigen3-dev libboost-all-dev libceres-dev
+```
-* **Online Temporal Calibration for Monocular Visual-Inertial Systems**, Tong Qin, Shaojie Shen, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS, 2018), **best student paper award** [pdf](https://ieeexplore.ieee.org/abstract/document/8593603)
+## Build
-* **VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator**, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen, IEEE Transactions on Robotics[pdf](https://ieeexplore.ieee.org/document/8421746/?arnumber=8421746&source=authoralert)
+Place the repository inside a ROS 2 workspace, then build it with `colcon`:
-*If you use VINS-Mono for your academic research, please cite at least one of our related papers.*[bib](https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper_bib.txt)
+```bash
+mkdir -p ~/ros2_ws/src
+cd ~/ros2_ws/src
+git clone https://github.com/Innopolis-UAV-Team/VINS-Mono-inno.git
-## 1. Prerequisites
-1.1 **Ubuntu** and **ROS**
-Ubuntu 16.04.
-ROS Kinetic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
-additional ROS pacakge
+cd ~/ros2_ws
+source /opt/ros/jazzy/setup.bash
+colcon build --symlink-install
+source install/setup.bash
```
- sudo apt-get install ros-YOUR_DISTRO-cv-bridge ros-YOUR_DISTRO-tf ros-YOUR_DISTRO-message-filters ros-YOUR_DISTRO-image-transport
+
+If the repository is already located in `~/ros2_ws/src`, only the last four
+commands are needed. Add the ROS and workspace setup files to `.bashrc` if you
+want new terminals to source them automatically.
+
+## Run the Innospector fisheye bag configuration
+
+The launch file `innospector_cam.launch.py` starts:
+
+- `feature_tracker`;
+- `vins_estimator`;
+- `pose_graph`.
+
+Its parameters are stored in
+`config/fisheye_bag/fisheye_bag_config.yaml`. The supplied configuration expects:
+
+- compressed camera images on `/image_raw/compressed`;
+- IMU messages on `/mavros/imu/data`.
+
+The YAML uses the image-transport base topic `/image_raw` together with
+`image_input_format: "compressed"`. Do not change the base topic to
+`/image_raw/compressed` when compressed transport is enabled.
+
+Terminal 1 — start VINS:
+
+```bash
+cd ~/ros2_ws
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 launch vins_bringup innospector_cam.launch.py
```
+Terminal 2 — play the ROS 2 bag:
+
+```bash
+cd ~/ros2_ws
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 bag play /path/to/bag
+```
-1.2. **Ceres Solver**
-Follow [Ceres Installation](http://ceres-solver.org/installation.html), use **version 1.14.0** and remember to **sudo make install**. (There are compilation issues in Ceres versions 2.0.0 and above.)
+Only the required bag topics may be played:
-## 2. Build VINS-Mono on ROS
-Clone the repository and catkin_make:
+```bash
+ros2 bag play /path/to/bag \
+ --topics /image_raw/compressed /mavros/imu/data
```
- cd ~/catkin_ws/src
- git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
- cd ../
- catkin_make
- source ~/catkin_ws/devel/setup.bash
+
+Terminal 3 — start the supplied RViz configuration:
+
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch vins_bringup vins_rviz.launch.py
```
-## 3. Visual-Inertial Odometry and Pose Graph Reuse on Public datasets
-Download [EuRoC MAV Dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Although it contains stereo cameras, we only use one camera. The system also works with [ETH-asl cla dataset](http://robotics.ethz.ch/~asl-datasets/maplab/multi_session_mapping_CLA/bags/). We take EuRoC as the example.
+RViz is recommended for viewing the tracked-feature image. Some versions of
+`rqt_image_view` perform poorly with large, high-rate images.
-**3.1 visual-inertial odometry and loop closure**
+## Other launch configurations
-3.1.1 Open three terminals, launch the vins_estimator , rviz and play the bag file respectively. Take MH_01 for example
+All supported ROS 2 launch files are owned by `vins_bringup`. The
+`vins_estimator` package contains the estimator executable and algorithm; it is
+not a duplicate bringup package and no longer contains legacy ROS 1 XML launch
+files. Always launch a complete configuration with:
+
+```bash
+ros2 launch vins_bringup .launch.py
```
- roslaunch vins_estimator euroc.launch
- roslaunch vins_estimator vins_rviz.launch
- rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag
+
+For the thermal camera configuration, start the estimator pipeline with:
+
+```bash
+cd ~/ros2_ws
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 launch vins_bringup termal_cam.launch.py
```
-(If you fail to open vins_rviz.launch, just open an empty rviz, then load the config file: file -> Open Config-> YOUR_VINS_FOLDER/config/vins_rviz_config.rviz)
-3.1.2 (Optional) Visualize ground truth. We write a naive benchmark publisher to help you visualize the ground truth. It uses a naive strategy to align VINS with ground truth. Just for visualization. not for quantitative comparison on academic publications.
+Then start RViz in another terminal:
+
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch vins_bringup vins_rviz.launch.py
```
- roslaunch benchmark_publisher publish.launch sequence_name:=MH_05_difficult
+
+The thermal launch reads `config/termal_cam_config.yaml` and starts
+`feature_tracker`, `vins_estimator`, and `pose_graph` together.
+
+```bash
+ros2 launch vins_bringup euroc.launch.py
+ros2 launch vins_bringup euroc_no_extrinsic_param.launch.py
+ros2 launch vins_bringup realsense_color.launch.py
+ros2 launch vins_bringup realsense_fisheye.launch.py
+ros2 launch vins_bringup usb_cam.launch.py
+ros2 launch vins_bringup termal_cam.launch.py
+ros2 launch vins_bringup 3dm.launch.py
```
- (Green line is VINS result, red line is ground truth).
-
-3.1.3 (Optional) You can even run EuRoC **without extrinsic parameters** between camera and IMU. We will calibrate them online. Replace the first command with:
+
+Review the corresponding file under `config/` before using a launch file. Topic
+names, camera calibration, camera-to-IMU extrinsics, IMU noise parameters, and
+time synchronization must match the actual sensor system.
+
+## Verify the pipeline
+
+Check that the source topics are active:
+
+```bash
+ros2 topic hz /image_raw/compressed
+ros2 topic hz /mavros/imu/data
```
- roslaunch vins_estimator euroc_no_extrinsic_param.launch
+
+Check each processing stage:
+
+```bash
+ros2 topic hz /feature_tracker/feature
+ros2 topic hz /feature_tracker/feature_img
+ros2 topic hz /vins_estimator/odometry
+ros2 topic hz /vins_estimator/path
```
-**No extrinsic parameters** in that config file. Waiting a few seconds for initial calibration. Sometimes you cannot feel any difference as the calibration is done quickly.
-**3.2 map merge**
+Inspect endpoint types and QoS when a publisher and subscriber do not connect:
-After playing MH_01 bag, you can continue playing MH_02 bag, MH_03 bag ... The system will merge them according to the loop closure.
+```bash
+ros2 topic info -v /image_raw/compressed
+ros2 topic info -v /feature_tracker/feature
+```
-**3.3 map reuse**
+Expected types include:
-3.3.1 map save
+- `/image_raw/compressed`: `sensor_msgs/msg/CompressedImage`;
+- `/feature_tracker/feature`: `sensor_msgs/msg/PointCloud`;
+- `/feature_tracker/feature_img`: `sensor_msgs/msg/Image`;
+- `/vins_estimator/odometry`: `nav_msgs/msg/Odometry`.
-Set the **pose_graph_save_path** in the config file (YOUR_VINS_FOLEDER/config/euroc/euroc_config.yaml). After playing MH_01 bag, input **s** in vins_estimator terminal, then **enter**. The current pose graph will be saved.
+## Camera masks and image formats
-3.3.2 map load
+`feature_tracker` supports ROS 2 raw and compressed image transport. Select the
+transport in the camera configuration:
-Set the **load_previous_pose_graph** to 1 before doing 3.1.1. The system will load previous pose graph from **pose_graph_save_path**. Then you can play MH_02 bag. New sequence will be aligned to the previous pose graph.
+```yaml
+image_topic: "/image_raw"
+image_input_format: "compressed" # use "raw" for sensor_msgs/msg/Image
+```
-## 4. AR Demo
-4.1 Download the [bag file](https://www.dropbox.com/s/s29oygyhwmllw9k/ar_box.bag?dl=0), which is collected from HKUST Robotic Institute. For friends in mainland China, download from [bag file](https://pan.baidu.com/s/1geEyHNl).
+A custom feature mask can restrict tracking to valid image pixels:
-4.2 Open three terminals, launch the ar_demo, rviz and play the bag file respectively.
+```yaml
+fisheye: 1
+fisheye_mask: "config/mask.jpg"
```
- roslaunch ar_demo 3dm_bag.launch
- roslaunch ar_demo ar_rviz.launch
- rosbag play YOUR_PATH_TO_DATASET/ar_box.bag
+
+The mask must have the same width and height as the configured camera. White
+pixels enable feature detection and black pixels exclude an area. This mask
+does not select or replace the camera projection model.
+
+## Feature tracker parameters
+
+These parameters are set in the camera YAML configuration file. They control
+corner detection and optical-flow tracking. These are not BRIEF descriptor
+parameters: BRIEF is used separately by the `pose_graph` module for loop
+closure.
+
+The ranges below are practical starting ranges for a 1280×1024 image at
+~30 FPS, not hard limits enforced by code. For other resolutions, `max_cnt`
+should scale roughly with frame area and `min_dist` with linear size. The final
+choice should be guided by CPU load and `[AUTO_TUNE]` output: aim for `fill`
+above 90% and stable `retention` above 70–80% on segments without heavy motion
+blur.
+
+| Parameter | Purpose and effect | Practical range / starting point |
+|---|---|---|
+| `max_cnt` | Target maximum number of tracked points per frame. Existing tracks are kept with priority and distributed evenly across the image; free slots are filled with new points. A higher value generally improves robustness but increases CPU load, RANSAC cost, and optimizer load. This is an upper bound, not a guarantee of the actual point count. | For 1280×1024: `150–800`; start at `250–400`. Above `500–600` only makes sense with sufficient CPU and texture. |
+| `min_dist` | Minimum spatial distance between selected points, in pixels. A larger value gives sparser, more uniform coverage; too large prevents reaching `max_cnt`. A small value allows more points but increases the risk of clusters on a single textured region. | `8–40 px`; start at `15–25 px`. For `max_cnt: 250`, `10–25 px` is acceptable. |
+| `freq` | Target rate for feature set publication and replenishment, in Hz. Incoming images are still used for optical flow, but RANSAC, new-point detection, and publication run only on selected frames. `0` in the config is replaced with `100`. | `10–30 Hz`; start at `20–25 Hz`. Generally do not set above the actual camera rate. |
+| `F_threshold` | RANSAC error threshold for the fundamental-matrix check, in pixels on the virtual undistorted image. A lower value rejects outliers more aggressively but may drop good tracks under noise or blur. A higher value keeps more points, including potential outliers. | `0.5–2.0 px`; start at `1.0 px`. For sharp images try `0.7–1.0`; for noisy/blurry ones `1.0–1.5`. |
+| `gftt_quality_level` | Relative quality threshold for Shi–Tomasi `goodFeaturesToTrack`; mathematically valid range `(0, 1]`. Used for new points only when `use_advanced_flow: 0`. Lower = more weak corners; higher = fewer but usually better. | Working range `0.001–0.03`; start at `0.005–0.01`. Values above `0.05` often yield too few points. |
+| `fast_threshold` | AGAST threshold for new points. Used only when `use_advanced_flow: 1`. Lower = more candidates including weak and noisy ones; higher = fewer candidates with stronger contrast. | `7–40`; start at `15–20`. For low-contrast/thermal images often `7–15`; for noisy ones `20–30`. |
+| `enable_feature_auto_tuning` | Enables (`1`) periodic adjustment of the active detector threshold. For AGAST it adjusts `fast_threshold`; for Shi–Tomasi it adjusts `gftt_quality_level`. `max_cnt`, `min_dist`, `F_threshold`, and optical-flow parameters are not changed automatically. | `0` or `1` only; start with `0` for manual baseline tuning, then try `1` on the full dataset. |
+| `feature_auto_tune_interval` | Auto-tune interval in seconds; minimum allowed value is `0.5`. Fill and retention statistics are accumulated between updates. Active only when auto-tuning is enabled. | `2–30 s`; start at `5–10 s`. Below `2 s` may cause unnecessary threshold oscillation. |
+| `feature_auto_tune_log` | When `1`, prints an `[AUTO_TUNE]` line to the ROS console and log after each interval. It reports fill, retention, candidate count, and current thresholds. Does not enable auto-tuning itself. | `0` or `1` only; recommended `1` during tuning. |
+| `fast_threshold_min`, `fast_threshold_max` | Lower and upper bounds within which auto-tuning may change the AGAST threshold. When auto-tuning is off, the bounds do not modify `fast_threshold` except for initial clamping at config load. | General safe range `3–60`; starting pair `7` and `40–45`. The gap between bounds should be at least `10`. |
+| `gftt_quality_min`, `gftt_quality_max` | Lower and upper bounds for automatic adjustment of `gftt_quality_level` (Shi–Tomasi). When `use_advanced_flow: 1`, this range is not used for new-point selection. | Minimum `0.0005–0.005`, maximum `0.02–0.10`; starting pair `0.001` and `0.03–0.05`. |
+| `use_bidirectional_flow` | When `1`, runs a reverse LK pass after the forward flow and rejects points with poor forward-backward consistency. Improves robustness but nearly doubles optical-flow cost. Only implemented in the `use_advanced_flow: 1` branch. | `0` or `1` only; `1` for quality, `0` when CPU is limited. |
+| `use_advanced_flow` | Selects the tracker mode. `1`: AGAST, spatial selection, subpixel refinement of a subset, and extended LK settings. `0`: Shi–Tomasi and basic LK. This parameter also determines which threshold auto-tuning adjusts. | `0` or `1` only; use `1` for the current uniform AGAST-based selection. |
+| `show_track` | When `1`, publishes an image with track visualization on `/feature_tracker/feature_img`. Does not disable publication of the feature points themselves on `/feature_tracker/feature`. Visualization adds CPU and ROS bandwidth cost. | `0` or `1` only; `1` during tuning, `0` for minimal overhead. |
+| `equalize` | When `1`, applies CLAHE to each grayscale frame before tracking and detection. Helps with low or uneven lighting but may amplify noise and adds computational cost. | `0` or `1` only; for dark/thermal cameras start with `1`; with good stable lighting compare against `0`. |
+| `fisheye` | When `1`, enables applying `fisheye_mask` to point selection. This parameter does not select the camera model or enable fisheye distortion correction; the model is set separately via `model_type`. | `0` or `1` only; `1` if a mask is actually needed and matches the frame. |
+| `fisheye_mask` | Path to a single-channel mask matching the image size. White pixels allow features, black pixels exclude regions. Relative paths are resolved against the `vins_bringup` package share directory; absolute paths are used as-is. | Non-numeric. Size must match `image_width × image_height`; the white usable area should be large enough to reach `max_cnt`. |
+
+Example manual AGAST configuration:
+
+```yaml
+max_cnt: 250
+min_dist: 10
+fast_threshold: 20
+use_advanced_flow: 1
+enable_feature_auto_tuning: 0
```
-We put one 0.8m x 0.8m x 0.8m virtual box in front of your view.
-## 5. Run with your device
+In this mode `gftt_quality_level`, `gftt_quality_min`, and `gftt_quality_max`
+do not affect new-point detection. `feature_auto_tune_interval` and
+`feature_auto_tune_log` take effect only after setting
+`enable_feature_auto_tuning: 1`.
+
+### `use_advanced_flow: 1` pipeline (AGAST + LK)
+
+When `use_advanced_flow: 1` is set, the feature tracker uses an enhanced
+processing pipeline instead of the basic Shi-Tomasi path. The full per-frame
+pipeline is described below.
+
+```mermaid
+flowchart TD
+ A["Input: new frame
+ timestamp"] --> B{"EQUALIZE?"}
+ B -->|Yes| C["CLAHE
clipLimit=3.0, grid=8x8"]
+ B -->|No| D["Use as-is"]
+ C --> E["forw_img = frame"]
+ D --> E
+
+ E --> F{"Existing tracks
from prev frame?"}
+ F -->|No| G["Skip LK"]
+ F -->|Yes| H["LK optical flow
cur_img → forw_img
window 21x21, pyr=3
TermCriteria COUNT+EPS 40/0.001"]
+
+ H --> I{"use_bidirectional_flow?"}
+ I -->|Yes| J["Reverse LK
forw_img → cur_img
reject if dist² > 0.5 px²"]
+ I -->|No| K["Skip"]
+ J --> L["Border check
reject edge points"]
+ K --> L
+
+ L --> M["reduceVector
remove rejected"]
+ M --> N{"ENABLE_VELOCITY_CHECK?"}
+ N -->|Yes| O["Estimate velocity
from optical flow"]
+ O --> P{"vel > threshold?"}
+ P -->|Yes| Q["adaptive_max_cnt += boost"]
+ P -->|No| R["adaptive_max_cnt = MAX_CNT"]
+ N -->|No| R
+
+ R --> S["track_cnt++ for all"]
+ S --> T{"PUB_THIS_FRAME?
freq throttle"}
+ T -->|No| Z["End of frame"]
+ T -->|Yes| U["rejectWithF()
RANSAC F-matrix
F_THRESHOLD, conf=0.99
on undistorted points"]
+
+ U --> V["setMask()
fisheye_mask + MIN_DIST
spatial bucketing
round-robin selection"]
+ V --> W["Detect new points"]
+ W --> X["AGAST
threshold = fast_threshold"]
+ X --> Y["Grid filter
cells MIN_DIST x MIN_DIST
strongest response per cell"]
+ Y --> AA["Spatial bucketing
round-robin across buckets
until n_max_cnt filled"]
+ AA --> BB["cornerSubPix
top-50 points
window 5x5, EPS+COUNT 40/0.001"]
+ BB --> CC["addPoints()
new points → forw_pts"]
+ CC --> DD["updateAutoTuning()
fill/retention stats
adjust fast_threshold"]
+ DD --> EE["undistortedPoints()
liftProjective → normalized
compute pts_velocity"]
+ EE --> Z
+```
-Suppose you are familiar with ROS and you can get a camera and an IMU with raw metric measurements in ROS topic, you can follow these steps to set up your device. For beginners, we highly recommend you to first try out [VINS-Mobile](https://github.com/HKUST-Aerial-Robotics/VINS-Mobile) if you have iOS devices since you don't need to set up anything.
+#### Step-by-step description
+
+1. **Preprocessing** — optional CLAHE equalization (`equalize: 1`) for dark
+ or unevenly lit scenes. Applied before tracking and detection.
+
+2. **LK optical flow** — `cv::calcOpticalFlowPyrLK` tracks points from the
+ previous frame into the current one. Parameters:
+ - Window: 21×21 pixels
+ - Pyramid levels: 3
+ - Termination criteria: 40 iterations or ε = 0.001
+
+3. **Bidirectional check** (optional, `use_bidirectional_flow: 1`) — reverse
+ LK flow from the current frame back to the previous one. A point is rejected
+ if the squared distance between the original and reverse position exceeds
+ 0.5 px² (≈ 0.7 px). Doubles LK cost but sharply reduces drift on repetitive
+ textures.
+
+4. **Border check** — reject points that moved outside the image border
+ (1 px margin).
+
+5. **Velocity-adaptive boost** (optional, `enable_velocity_check: 1`) —
+ estimates velocity from average optical flow. When `vel >
+ max_velocity_threshold`, `velocity_boost_features` extra points are added.
+
+6. **RANSAC F-matrix rejection** (`rejectWithF`) — on frames selected for
+ publication (by `freq`), points are checked via the fundamental matrix.
+ Uses undistorted normalized coordinates (via the camera model's
+ `liftProjective`). Threshold `F_threshold`, confidence 0.99.
+
+7. **setMask** — builds a mask for new-point selection:
+ - Base: `fisheye_mask` (if `fisheye: 1`) or all-white
+ - Existing tracks are marked with circles of radius `MIN_DIST` (prevents
+ clustering)
+ - Spatial bucketing: round-robin across buckets for uniform coverage
+
+8. **AGAST detection** — `cv::AGAST` with threshold `fast_threshold`. Returns
+ keypoints with response values.
+
+9. **Grid filter** — the image is divided into cells of `MIN_DIST × MIN_DIST`.
+ Only the keypoint with the highest response is kept per cell. This provides
+ spatial distribution without a separate NMS step.
+
+10. **Spatial bucketing** — round-robin across coarse buckets (as in setMask)
+ until `n_max_cnt = adaptive_max_cnt - tracked_count` is filled.
+
+11. **Subpixel refinement** — `cv::cornerSubPix` for the top-50 points
+ (window 5×5, criteria EPS+COUNT 40/0.001). Improves accuracy to subpixel
+ level.
+
+12. **Auto-tuning** (`enable_feature_auto_tuning: 1`) — accumulates `fill`
+ (fill ratio) and `retention` (fraction of tracks kept) statistics over the
+ `feature_auto_tune_interval`. Adjusts `fast_threshold`:
+ - `fill < 0.90` → threshold −2 (more points)
+ - `fill > 0.98 && retention < 0.75` → threshold +2 (fewer but better)
+
+13. **Undistortion** — the camera model's `liftProjective` converts pixel
+ coordinates to normalized ones (x/z, y/z). Point velocities
+ (`pts_velocity`) are computed for the estimator.
+
+#### Parameters affecting the pipeline
+
+| Parameter | Stage | Effect |
+|---|---|---|
+| `equalize` | 1. Preprocessing | CLAHE for dark scenes |
+| `use_bidirectional_flow` | 3. LK | Reject inconsistent tracks |
+| `F_threshold` | 6. RANSAC | Geometric outlier rejection strictness |
+| `max_cnt` | 7-10. Detection | Target point count |
+| `min_dist` | 7-9. Mask/Grid | Spatial distribution |
+| `fast_threshold` | 8. AGAST | Corner detector threshold |
+| `enable_feature_auto_tuning` | 12. Auto-tune | Threshold adaptation |
+| `fisheye` / `fisheye_mask` | 7. setMask | Restrict working area |
+| `freq` | 6. RANSAC | Publication and replenishment rate |
+
+#### Comparison with `use_advanced_flow: 0`
+
+| Property | `use_advanced_flow: 1` | `use_advanced_flow: 0` |
+|---|---|---|
+| Detector | AGAST + grid + subpixel | Shi-Tomasi (`goodFeaturesToTrack`) |
+| LK parameters | Stricter TermCriteria | Basic |
+| Bidirectional | Optional | Not available |
+| Subpixel refine | Yes (top-50) | No |
+| Spatial bucketing | Round-robin across buckets | No |
+| Auto-tune | Adjusts `fast_threshold` | Adjusts `gftt_quality_level` |
+| CPU cost | Higher (AGAST + refine) | Lower |
+
+### Changing parameters at runtime
+
+All parameters in the table are declared as dynamic ROS 2 parameters. After
+launching `feature_tracker`, open the GUI:
+
+```bash
+source ~/ros2_ws/install/setup.bash
+rqt_reconfigure
+```
-5.1 Change to your topic name in the config file. The image should exceed 20Hz and IMU should exceed 100Hz. Both image and IMU should have the accurate time stamp. IMU should contain absolute acceleration values including gravity.
+Select `/feature_tracker` in the tree. Numeric fields have valid bounds, and
+changes are applied without restarting the node. The same can be done from the
+terminal:
+
+```bash
+ros2 param set /feature_tracker max_cnt 350
+ros2 param set /feature_tracker min_dist 15
+ros2 param set /feature_tracker fast_threshold 18
+ros2 param set /feature_tracker enable_feature_auto_tuning true
+ros2 param get /feature_tracker fast_threshold
+ros2 param dump /feature_tracker
+```
-5.2 Camera calibration:
+Change related bounds in a safe order, otherwise the intermediate combination
+will be rejected. For example, when widening the range, increase the maximum
+first, then decrease the minimum:
-We support the [pinhole model](http://docs.opencv.org/2.4.8/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html) and the [MEI model](http://www.robots.ox.ac.uk/~cmei/articles/single_viewpoint_calib_mei_07.pdf). You can calibrate your camera with any tools you like. Just write the parameters in the config file in the right format. If you use rolling shutter camera, please carefully calibrate your camera, making sure the reprojection error is less than 0.5 pixel.
+```bash
+ros2 param set /feature_tracker fast_threshold_max 45
+ros2 param set /feature_tracker fast_threshold_min 7
+```
-5.3 **Camera-Imu extrinsic parameters**:
+The node validates `*_min <= *_max`, `fisheye_mask` size, and other valid
+ranges. Accepted changes are logged with a `[RUNTIME_CONFIG]` line. Runtime
+changes are not written back to the YAML config and will be lost on restart.
+To persist tuned values, copy them into the YAML manually or save the output of
+`ros2 param dump`.
-If you have seen the config files for EuRoC and AR demos, you can find that we can estimate and refine them online. If you familiar with transformation, you can figure out the rotation and position by your eyes or via hand measurements. Then write these values into config as the initial guess. Our estimator will refine extrinsic parameters online. If you don't know anything about the camera-IMU transformation, just ignore the extrinsic parameters and set the **estimate_extrinsic** to **2**, and rotate your device set at the beginning for a few seconds. When the system works successfully, we will save the calibration result. you can use these result as initial values for next time. An example of how to set the extrinsic parameters is in[extrinsic_parameter_example](https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/config/extrinsic_parameter_example.pdf)
+## Calibration notes
-5.4 **Temporal calibration**:
-Most self-made visual-inertial sensor sets are unsynchronized. You can set **estimate_td** to 1 to online estimate the time offset between your camera and IMU.
+Reliable VIO requires all of the following:
-5.5 **Rolling shutter**:
-For rolling shutter camera (carefully calibrated, reprojection error under 0.5 pixel), set **rolling_shutter** to 1. Also, you should set rolling shutter readout time **rolling_shutter_tr**, which is from sensor datasheet(usually 0-0.05s, not exposure time). Don't try web camera, the web camera is so awful.
+1. Camera intrinsics and distortion parameters for the exact image resolution.
+2. An accurate camera-to-IMU rotation and translation.
+3. Correct IMU noise and bias random-walk parameters.
+4. Monotonic timestamps and a known or estimated camera/IMU time offset.
+5. Sufficient motion and parallax during estimator initialization.
-5.6 Other parameter settings: Details are included in the config file.
+Use `estimate_extrinsic: 0` only when the supplied extrinsic transform is known
+to be accurate. Use `estimate_td: 1` for unsynchronized sensors; otherwise fix
+the calibrated offset with `estimate_td: 0` and `td: `.
-5.7 Performance on different devices:
+For a physical fisheye or very wide-angle camera, calibrate an appropriate
+camodocal model such as `KANNALA_BRANDT` or `MEI`. Setting `fisheye: 1` only
+enables the feature mask; it does not change the projection model.
-(global shutter camera + synchronized high-end IMU, e.g. VI-Sensor) > (global shutter camera + synchronized low-end IMU) > (global camera + unsync high frequency IMU) > (global camera + unsync low frequency IMU) > (rolling camera + unsync low frequency IMU).
+## Loop closure and pose graph parameters
-## 6. Docker Support
+Loop closure is configured in the camera YAML file:
-To further facilitate the building process, we add docker in our code. Docker environment is like a sandbox, thus makes our code environment-independent. To run with docker, first make sure [ros](http://wiki.ros.org/ROS/Installation) and [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) are installed on your machine. Then add your account to `docker` group by `sudo usermod -aG docker $YOUR_USER_NAME`. **Relaunch the terminal or logout and re-login if you get `Permission denied` error**, type:
+```yaml
+loop_closure: 1
+load_previous_pose_graph: 0
+fast_relocalization: 0
+pose_graph_save_path: "/tmp/vins_thermal_output/pose_graph/"
```
-cd ~/catkin_ws/src/VINS-Mono/docker
-make build
-./run.sh LAUNCH_FILE_NAME # ./run.sh euroc.launch
+
+`loop_closure` enables image-based place recognition and pose graph
+optimization:
+
+- `0` runs local visual-inertial odometry only. Accumulated drift is not
+ corrected by previously visited places.
+- `1` loads the BRIEF vocabulary, searches for previously observed keyframes,
+ verifies candidates geometrically, and optimizes the pose graph after an
+ accepted loop.
+
+`load_previous_pose_graph` controls map reuse at startup:
+
+- `0` starts a new pose graph.
+- `1` loads a previously saved graph from `pose_graph_save_path` and attempts
+ to align the current sequence with it. The saved graph must be compatible
+ with the current camera calibration and configuration.
+
+`fast_relocalization` sends geometrically verified loop matches back to the
+VINS estimator:
+
+- `0` keeps loop correction primarily inside the pose graph.
+- `1` publishes matched points to the estimator for faster correction of its
+ local state. Enable this only after ordinary loop closure has been verified
+ as stable.
+
+`pose_graph_save_path` is the directory used to save and load keyframes,
+BRIEF descriptors, graph constraints, and trajectory data. Paths under `/tmp`
+are normally removed after a reboot. Use a persistent location for map reuse,
+for example:
+
+```yaml
+pose_graph_save_path: "/home/jetson/vins_maps/thermal_pose_graph/"
+```
+
+For a first run, the recommended settings are:
+
+```yaml
+loop_closure: 1
+load_previous_pose_graph: 0
+fast_relocalization: 0
+```
+
+The console reports loop processing with `[LOOP]` messages. A real closure is
+confirmed by `[LOOP] accepted`; a DBoW image candidate may still be rejected by
+geometric verification.
+
+## Tests
+
+Build and run the package tests with:
+
+```bash
+cd ~/ros2_ws
+source install/setup.bash
+colcon test --packages-select feature_tracker
+colcon test-result --verbose
```
-Note that the docker building process may take a while depends on your network and machine. After VINS-Mono successfully started, open another terminal and play your bag file, then you should be able to see the result. If you need modify the code, simply run `./run.sh LAUNCH_FILE_NAME` after your changes.
+Additional migration and runtime checks are documented in
+[ROS2_FISHEYE_CHECKLIST.md](ROS2_FISHEYE_CHECKLIST.md),
+[ROS2_MIGRATION_PLAN.md](ROS2_MIGRATION_PLAN.md), and
+[ROS2_MIGRATION_HANDOFF.md](ROS2_MIGRATION_HANDOFF.md).
+
+## Original project and citation
+
+VINS-Mono is an optimization-based monocular visual-inertial SLAM framework by
+Tong Qin, Peiliang Li, Zhenfei Yang, and Shaojie Shen. If this software is used
+for academic research, cite the original publications:
-## 7. Acknowledgements
-We use [ceres solver](http://ceres-solver.org/) for non-linear optimization and [DBoW2](https://github.com/dorian3d/DBoW2) for loop detection, and a generic [camera model](https://github.com/hengli/camodocal).
+- T. Qin, P. Li, Z. Yang, and S. Shen, “VINS-Mono: A Robust and Versatile
+ Monocular Visual-Inertial State Estimator,” IEEE Transactions on Robotics.
+- T. Qin and S. Shen, “Online Temporal Calibration for Monocular
+ Visual-Inertial Systems,” IROS 2018.
-## 8. Licence
-The source code is released under [GPLv3](http://www.gnu.org/licenses/) license.
+This fork uses Ceres Solver, DBoW2, OpenCV, Eigen, Boost, and camodocal.
-We are still working on improving the code reliability. For any technical issues, please contact Tong QIN or Peiliang LI .
+## License
-For commercial inquiries, please contact Shaojie SHEN
+The source code is released under the GPLv3 license. This ROS 2 fork is
+maintained by the Innopolis UAV Team; the original VINS-Mono authors retain
+credit for the underlying system.
diff --git a/ROS2_FISHEYE_CHECKLIST.md b/ROS2_FISHEYE_CHECKLIST.md
new file mode 100644
index 000000000..760122129
--- /dev/null
+++ b/ROS2_FISHEYE_CHECKLIST.md
@@ -0,0 +1,190 @@
+# ROS2 fisheye bag checklist
+
+Этот чек-лист подходит для запуска `vins_bringup` с bag, где входная картинка идет в формате `sensor_msgs/msg/CompressedImage`.
+
+## 1. Перед запуском
+
+Убедись, что bag уже запущен и публикует:
+
+- `/image_raw/compressed`
+- `/mavros/imu/data`
+
+Проверка:
+
+```bash
+source ~/.bashrc
+ros2 topic list | grep -E 'image_raw/compressed|mavros/imu/data'
+ros2 topic hz /image_raw/compressed
+ros2 topic hz /mavros/imu/data
+```
+
+Ожидаемо:
+
+- `/image_raw/compressed` есть
+- `/mavros/imu/data` есть
+- частота у обоих топиков не нулевая
+
+## 2. Конфиг
+
+Для bag-режима в `config/fisheye_bag/fisheye_bag_config.yaml` должны быть:
+
+```yaml
+image_topic: "/image_raw/compressed"
+image_input_format: "compressed"
+imu_topic: "/mavros/imu/data"
+estimate_extrinsic: 0
+extrinsicTranslation: [0.0, 0.0, 0.0]
+```
+
+Если позже захочешь вернуть обычную raw-камеру:
+
+```yaml
+image_topic: "/camera/fisheye/image_raw"
+image_input_format: "raw"
+```
+
+## 3. Запуск
+
+В одном терминале:
+
+```bash
+source ~/.bashrc
+ros2 launch vins_bringup innospector_cam.launch.py
+```
+
+## 4. Что должно появиться в консоли
+
+### feature_tracker
+
+Должны быть строки вроде:
+
+```text
+[START] image_input_format=compressed
+[START] compressed subscription created for /image_raw/compressed
+[INPUT] first compressed image received: ...
+[INPUT] first image received: ...
+[STATE] tracker primed on first frame
+[OUTPUT] published feature packet #1 ...
+```
+
+### vins_estimator
+
+Должны быть строки вроде:
+
+```text
+[START] vins_estimator is up and waiting for synchronized measurement pairs
+[INPUT] imu #1 ...
+[INPUT] first feature packet received: ...
+```
+
+### pose_graph
+
+Нормально увидеть:
+
+```text
+no previous pose graph
+```
+
+## 5. Проверка топиков после старта
+
+В другом терминале:
+
+```bash
+source ~/.bashrc
+ros2 topic list | grep -E 'image_raw/compressed|feature_tracker|vins_estimator|pose_graph'
+ros2 topic info -v /image_raw/compressed
+ros2 topic hz /image_raw/compressed
+ros2 topic hz /feature_tracker/feature
+ros2 topic hz /feature_tracker/feature_img
+ros2 topic hz /vins_estimator/odometry
+```
+
+Ожидаемо:
+
+- `/image_raw/compressed` публикуется rosbag с типом `sensor_msgs/msg/CompressedImage`
+- `feature_tracker` подписан на него через transport `compressed`
+- `/feature_tracker/feature` имеет ненулевую частоту
+- `/feature_tracker/feature_img` имеет частоту около заданного `freq` и тип `sensor_msgs/msg/Image`
+- `/vins_estimator/odometry` появляется после того, как наберется достаточно кадров
+
+## 6. Если что-то не работает
+
+### Сценарий A: есть IMU, но нет image callback
+
+Это значит, что `feature_tracker` не получает картинку.
+
+Проверь:
+
+```bash
+ros2 topic echo --once /image_raw/compressed
+ros2 topic info -v /image_raw/compressed
+ros2 node list
+```
+
+### Сценарий B: `feature_tracker` получает картинку, но `vins_estimator` пишет `queue_feat=0`
+
+Это значит, что `feature_tracker` не публикует `feature`.
+
+Проверь логи `feature_tracker`:
+
+```text
+[OUTPUT] published feature packet ...
+```
+
+### Сценарий C: `vins_estimator` пишет `imu message in disorder!`
+
+Это обычно проблема порядка временных меток.
+
+Проверь:
+
+```bash
+ros2 topic hz /mavros/imu/data
+ros2 topic echo --once /mavros/imu/data
+```
+
+### Сценарий D: launch стартует, но видны только старые предупреждения FastDDS
+
+Сообщения вида:
+
+```text
+RTPS_TRANSPORT_SHM Error
+```
+
+часто не критичны для логики VINS, но могут мешать диагностике.
+Главное — есть ли реальные сообщения на нужных топиках.
+
+## 7. Быстрая команда “все ли живо”
+
+```bash
+source ~/.bashrc
+ros2 topic hz /image_raw/compressed
+ros2 topic hz /mavros/imu/data
+ros2 topic hz /feature_tracker/feature
+ros2 topic hz /vins_estimator/odometry
+```
+
+Если первые две частоты есть, а последние две нет — проблема между камерой и VINS.
+
+## 8. Диагностические сообщения новой цепочки
+
+При нормальной работе последовательно появляются:
+
+```text
+[INPUT] first image received ... encoding=mono8
+[OUTPUT] published feature packet ...
+[OUTPUT] published feature image ... /feature_tracker/feature_img
+[INPUT] first feature packet received ...
+[OUTPUT] accepted keyframe ...
+```
+
+Сообщения `[FLOW]`, `inconsistent feature vectors`, `non-increasing image timestamps`,
+`invalid keyframe point layout` и `[SYNC] ... exhausted` означают конкретное место разрыва.
+
+Unit-тесты защитных проверок:
+
+```bash
+cd ~/ros2_ws
+source ~/.bashrc
+colcon test --packages-select feature_tracker --event-handlers console_direct+
+colcon test-result --verbose
+```
diff --git a/ROS2_MIGRATION_HANDOFF.md b/ROS2_MIGRATION_HANDOFF.md
new file mode 100644
index 000000000..235742bd3
--- /dev/null
+++ b/ROS2_MIGRATION_HANDOFF.md
@@ -0,0 +1,521 @@
+# VINS-Mono ROS1 → ROS2 Migration Handoff Document
+
+## Для агента-преемника
+
+Этот документ содержит полную информацию о состоянии миграции VINS-Mono из ROS1 в ROS2 Jazzy на Jetson Orin NX. Передавайся от одного агента к другому для продолжения работы.
+
+---
+
+## 1. Окружение
+
+| Параметр | Значение |
+|-----------|----------|
+| **ROS2** | Jazzy Jalisco (`/opt/ros/jazzy`) |
+| **Hardware** | Jetson Orin NX, aarch64, 8 ядер, 9.5 GB RAM |
+| **Workspace** | `~/ros2_ws` |
+| **Source** | `~/ros2_ws/src/VINS-Mono-inno` |
+| **Git branch** | `ros2` (ответвлён от `UAV_perfom`) |
+| **Build command** | `cd ~/ros2_ws && source /opt/ros/jazzy/setup.bash && source install/setup.bash 2>/dev/null; colcon build --symlink-install --packages-select --base-paths src` |
+| **C++ standard** | C++17 (обязательно для ROS2 Jazzy) |
+
+### Системные библиотеки (все установлены)
+- Ceres 2.2.0 (`libceres-dev`) — **Важно: `ceres::LocalParameterization` удалён, используется `ceres::Manifold`**
+- OpenCV 4.6 (`libopencv-dev`)
+- Eigen 3.4 (`libeigen3-dev`)
+- Boost 1.83 (`libboost-all-dev`)
+- yaml-cpp 0.8
+
+### ROS2 пакеты (все установлены)
+- `rclcpp`, `std_msgs`, `sensor_msgs`, `nav_msgs`, `geometry_msgs`, `visualization_msgs`
+- `tf2_ros`, `tf2_geometry_msgs`, `tf2_eigen`
+- `cv_bridge` (header: `cv_bridge/cv_bridge.hpp`, не `.h`!)
+- `image_transport`, `message_filters`
+- `ament_cmake`, `ament_index_cpp`
+
+### Важно: `sensor_msgs::msg::PointCloud` + `ChannelFloat32`
+В ROS2 Jazzy эти сообщения **доступны, но deprecated**. Решено использовать их как есть для минимизации изменений кода.
+
+---
+
+## 2. Структура пакетов (6 пакетов)
+
+```
+~/ros2_ws/src/VINS-Mono-inno/
+├── camera_model/ # ✅ ГОТОВО — библиотека + Calibration tool
+├── benchmark_publisher/ # ✅ ГОТОВО — простейший узел
+├── feature_tracker/ # ✅ ГОТОВО — трекинг фич
+├── vins_estimator/ # ✅ ГОТОВО — основной оценщик (самый сложный)
+├── pose_graph/ # ✅ ГОТОВО — loop closure + pose graph
+├── vins_bringup/ # ✅ ГОТОВО — launch-пакет
+├── config/ # Общие YAML конфиги (не требуют изменений)
+├── support_files/ # brief_k10L6.bin, brief_pattern.yml
+├── ROS2_MIGRATION_PLAN.md # Общий план миграции
+└── ROS2_MIGRATION_HANDOFF.md # Этот документ
+```
+
+### Статус сборки
+| Пакет | Статус | Собран |
+|-------|--------|--------|
+| `camera_model` | ✅ Полностью мигрирован | Да |
+| `benchmark_publisher` | ✅ Полностью мигрирован | Да |
+| `feature_tracker` | ✅ Полностью мигрирован | Да |
+| `vins_estimator` | ✅ Полностью мигрирован | Да |
+| `pose_graph` | ✅ Полностью мигрирован | Да |
+| `vins_bringup` | ✅ Создан и собран | Да |
+
+`ar_demo` и `data_generator` намеренно исключены из текущего scope и удалены из дерева репозитория.
+
+---
+
+## 3. Ключевые архитектурные решения (уже реализованные)
+
+### 3.1 Ceres 2.2 Compatibility Shim
+Файл: `camera_model/include/camodocal/gpl/ceres_local_parameterization_compat.h`
+
+Ceres 2.2 полностью удалил `ceres::LocalParameterization`. Создан shim-класс, который наследует `ceres::Manifold` и предоставляет старый API `LocalParameterization` (`Plus`, `ComputeJacobian`, `GlobalSize`, `LocalSize`).
+
+**Использование в коде:**
+```cpp
+#include "camodocal/gpl/ceres_local_parameterization_compat.h"
+
+// Старый код продолжает работать:
+class PoseLocalParameterization : public ceres::LocalParameterization { ... };
+
+// Но тип указателя меняется:
+ceres::Manifold* local_parameterization = new PoseLocalParameterization(); // НЕ LocalParameterization*
+problem.SetManifold(para_Pose[i], local_parameterization); // НЕ AddParameterBlock(..., local_parameterization)
+```
+
+**Также доступно:**
+```cpp
+// ceres::QuaternionManifold() — встроенный в Ceres 2.2
+ceres::Manifold* q_manifold = new ceres::QuaternionManifold();
+
+// AutoDiffManifold заменяет AutoDiffLocalParameterization
+static ceres::Manifold* Create() {
+ return new ceres::AutoDiffManifold();
+}
+```
+
+### 3.2 Helper функция для timestamp
+Добавлена в `vins_estimator/src/utility/utility.h`:
+```cpp
+inline double stampToSec(const builtin_interfaces::msg::Time &stamp) {
+ return (double)stamp.sec + (double)stamp.nanosec * 1e-9;
+}
+```
+Используется вместо `msg->header.stamp.toSec()`.
+
+### 3.3 Паттерн миграции ROS1 → ROS2 (уже отработанный)
+
+#### Includes
+```cpp
+// ROS1 → ROS2
+#include → #include "rclcpp/rclcpp.hpp"
+#include → #include "sensor_msgs/msg/image.hpp"
+#include → #include "sensor_msgs/msg/point_cloud.hpp"
+#include → #include "sensor_msgs/msg/imu.hpp"
+#include → #include "sensor_msgs/image_encodings.hpp"
+#include → #include "std_msgs/msg/bool.hpp"
+#include → #include "std_msgs/msg/header.hpp"
+#include → #include "nav_msgs/msg/odometry.hpp"
+#include → #include "nav_msgs/msg/path.hpp"
+#include → #include "geometry_msgs/msg/point_stamped.hpp"
+#include → #include "geometry_msgs/msg/point32.hpp"
+#include → #include "geometry_msgs/msg/point.hpp"
+#include → #include "geometry_msgs/msg/twist_stamped.hpp"
+#include → #include "visualization_msgs/msg/marker.hpp"
+#include → #include "visualization_msgs/msg/marker_array.hpp"
+#include → #include "std_msgs/msg/color_rgba.hpp"
+#include → #include "tf2_ros/transform_broadcaster.h"
+#include → #include "cv_bridge/cv_bridge.hpp" // ВАЖНО: .hpp, не .h!
+```
+
+#### API замены
+```cpp
+// ROS1 → ROS2
+ros::init(argc, argv, "node") → rclcpp::init(argc, argv)
+ros::NodeHandle n("~") → auto node = std::make_shared("node")
+ros::console::set_logger_level(...) → (не нужно, rclcpp по умолчанию Info)
+readParameters(n) → readParameters(node) // signature: rclcpp::Node::SharedPtr
+
+// Parameters
+n.getParam("key", var) → node->declare_parameter("key", default); node->get_parameter("key", var)
+readParam(n, "key") → (см. parameters.cpp в feature_tracker/vins_estimator для шаблона)
+
+// Subscribers
+ros::Subscriber sub = n.subscribe(topic, q, cb)
+ → auto sub = node->create_subscription(topic, q, cb)
+
+// Publishers
+ros::Publisher pub = n.advertise(topic, q)
+ → rclcpp::Publisher::SharedPtr pub = node->create_publisher(topic, q)
+pub.publish(msg) → pub->publish(msg) // для shared_ptr: pub->publish(*msg)
+
+// Spin
+ros::spin() → rclcpp::spin(node)
+ros::ok() → rclcpp::ok()
+
+// Time
+ros::Time::now().toSec() → node->now().seconds()
+msg->header.stamp.toSec() → stampToSec(msg->header.stamp) // или rclcpp::Time(msg->header.stamp).seconds()
+ros::Time(double_t) → см. helper ниже
+
+// Logging
+ROS_INFO(...) → RCLCPP_INFO(rclcpp::get_logger("node_name"), ...)
+ROS_WARN(...) → RCLCPP_WARN(rclcpp::get_logger("node_name"), ...)
+ROS_DEBUG(...) → RCLCPP_DEBUG(rclcpp::get_logger("node_name"), ...)
+ROS_INFO_STREAM(...) → RCLCPP_INFO_STREAM(rclcpp::get_logger("node_name"), ...)
+ROS_ASSERT(cond) → assert(cond)
+ROS_BREAK() → std::abort()
+ROS_WARN_THROTTLE(2.0, ...) → RCLCPP_WARN_SKIPFIRST_THROTTLE(logger, clock, 2000, ...)
+
+// Message types
+sensor_msgs::ImageConstPtr → sensor_msgs::msg::Image::ConstSharedPtr
+sensor_msgs::PointCloudConstPtr → sensor_msgs::msg::PointCloud::ConstSharedPtr
+sensor_msgs::PointCloudPtr → sensor_msgs::msg::PointCloud::SharedPtr
+nav_msgs::Odometry::ConstPtr → nav_msgs::msg::Odometry::ConstSharedPtr
+std_msgs::BoolConstPtr → std_msgs::msg::Bool::ConstSharedPtr
+geometry_msgs::TwistStamped::ConstPtr → geometry_msgs::msg::TwistStamped::ConstSharedPtr
+sensor_msgs::ChannelFloat32 → sensor_msgs::msg::ChannelFloat32
+geometry_msgs::Point32 → geometry_msgs::msg::Point32
+geometry_msgs::Point → geometry_msgs::msg::Point
+nav_msgs::Path → nav_msgs::msg::Path
+nav_msgs::Odometry → nav_msgs::msg::Odometry
+visualization_msgs::Marker → visualization_msgs::msg::Marker
+visualization_msgs::MarkerArray → visualization_msgs::msg::MarkerArray
+std_msgs::Header → std_msgs::msg::Header
+std_msgs::ColorRGBA → std_msgs::msg::ColorRGBA
+ros::Duration() → builtin_interfaces::msg::Duration()
+
+// TF
+tf::TransformBroadcaster → tf2_ros::TransformBroadcaster
+tf::Transform → geometry_msgs::msg::TransformStamped
+tf::StampedTransform(...) → (заполнить TransformStamped полями)
+br.sendTransform(StampedTransform(...)) → br->sendTransform(transform_stamped)
+
+// Package path
+ros::package::getPath("pkg") → ament_index_cpp::get_package_share_directory("pkg")
+
+// cv_bridge
+cv_bridge::toCvCopy(msg, encoding) → cv_bridge::toCvCopy(msg, encoding) // тот же API
+cv_bridge::CvImageConstPtr → cv_bridge::CvImageConstPtr // тот же тип
+ptr->toImageMsg() → ptr->toImageMsg() // возвращает SharedPtr, нужно *разыменовать
+```
+
+#### CMakeLists.txt паттерн
+```cmake
+cmake_minimum_required(VERSION 3.8)
+project(PACKAGE_NAME)
+
+if(NOT CMAKE_BUILD_TYPE)
+ set(CMAKE_BUILD_TYPE "Release")
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(camera_model REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+add_executable(NODE_NAME src/file1.cpp src/file2.cpp ...)
+
+ament_target_dependencies(NODE_NAME rclcpp std_msgs sensor_msgs cv_bridge camera_model)
+target_link_libraries(NODE_NAME camera_model::camera_model ${OpenCV_LIBS})
+target_include_directories(NODE_NAME PRIVATE ${EIGEN3_INCLUDE_DIR} ${camera_model_INCLUDE_DIRS})
+
+install(TARGETS NODE_NAME DESTINATION lib/${PROJECT_NAME})
+ament_package()
+```
+
+#### package.xml паттерн
+```xml
+
+
+ PACKAGE_NAME
+ 0.0.0
+ ...
+ Name
+ TODO
+ ament_cmake
+ rclcpp
+ std_msgs
+ sensor_msgs
+
+ ament_cmake
+
+```
+
+---
+
+## 4. Детали по каждому ГОТОВОМУ пакету
+
+### 4.1 camera_model ✅
+- **Тип**: Библиотека (без ROS зависимостей в коде)
+- **Изменения**: CMakeLists.txt (catkin → ament), package.xml, `SetParameterization` → `SetManifold` в CameraCalibration.cc
+- **Новый файл**: `include/camodocal/gpl/ceres_local_parameterization_compat.h` — shim для Ceres 2.2
+- **Изменён**: `include/camodocal/gpl/EigenQuaternionParameterization.h` — include shim вместо удалённого `ceres/local_parameterization.h`
+- **Собирается**: Да
+
+### 4.2 benchmark_publisher ✅
+- **Тип**: Простой узел (1 subscriber, 2 publishers)
+- **Изменения**: Полная переработка `benchmark_publisher_node.cpp`, CMakeLists.txt, package.xml
+- **Особенности**: `ros::Time(double)` → ручная конверсия в `builtin_interfaces::msg::Time`
+- **Собирается**: Да
+
+### 4.3 feature_tracker ✅
+- **Тип**: Узел (1 subscriber, 3 publishers)
+- **Изменения**: `feature_tracker_node.cpp` (полная переработка), `parameters.cpp/h`, `feature_tracker.cpp` (ROS_DEBUG → RCLCPP_DEBUG)
+- **Особенности**:
+ - `pub_img` и `pub_match` — разные типы (`PointCloud` и `Image`), разделены на отдельные объявления
+ - `pub->publish(*feature_points)` — разыменование SharedPtr при публикации
+ - `cv_bridge/cv_bridge.hpp` (не `.h`)
+ - `cv::COLOR_GRAY2RGB` вместо `CV_GRAY2RGB`
+ - `g_clock` — глобальный `rclcpp::Clock::SharedPtr` для `RCLCPP_WARN_THROTTLE`
+- **Собирается**: Да
+
+### 4.4 vins_estimator ✅
+- **Тип**: Самый сложный узел (5 subscribers, 13 publishers, TF, потоки)
+- **Изменения**: 19 файлов модифицированы
+- **Ключевые решения**:
+ - `estimator.h`: `std_msgs::Header` → `std_msgs::msg::Header` (включая member `Headers[WINDOW_SIZE+1]`)
+ - `estimator.cpp`: `ros::Time::now().toSec()` → `std::chrono::system_clock` (wall clock, не зависит от ROS)
+ - `visualization.cpp/h`: Все 13 publishers → `rclcpp::Publisher<...>::SharedPtr`, TF → `tf2_ros::TransformBroadcaster`
+ - `CameraPoseVisualization.cpp/h`: `ros::Publisher&` → `rclcpp::Publisher<...>::SharedPtr`
+ - `feature_manager.h`: `#include ` + `` → `#include ` + `#include "rclcpp/rclcpp.hpp"`
+ - `initial/initial_alignment.h`, `initial_ex_rotation.h`, `solve_5pts.h`: ROS includes заменены
+ - `factor/imu_factor.h`, `marginalization_factor.h`: добавлен `#include "rclcpp/rclcpp.hpp"` для ROS_ASSERT
+ - `pose_local_parameterization.h`: include compat shim
+ - `estimator.cpp`: `ceres::LocalParameterization*` → `ceres::Manifold*`, `AddParameterBlock(..., local_param)` → `SetManifold(...)`
+ - `initial_sfm.cpp`: `ceres::LocalParameterization*` → `ceres::Manifold*`
+ - `utility.h`: добавлена `stampToSec()` helper функция
+ - QoS для IMU: `rclcpp::QoS(rclcpp::KeepLast(2000)).best_effort()` (замена `tcpNoDelay`)
+- **Собирается**: Да
+
+---
+
+## 5. Детали по НЕЗАВЕРШЁННЫМ пакетам
+
+### 5.1 pose_graph ✅ (ГОТОВО)
+
+> Обновлено 2026-07-04: перечисленный ниже исторический чек-лист выполнен. Пакет
+> переведён на ROS2 Jazzy и успешно собран через `colcon`. Мигрированы node API,
+> сообщения и publishers, TF2, `ament_index_cpp`, Ceres 2.2 `Manifold`; BRIEF-файлы
+> теперь устанавливаются в `share/pose_graph/support_files`.
+
+**Уже мигрированы (3 файла):**
+- `src/parameters.h` — includes заменены, `ros::Publisher` → `rclcpp::Publisher<...>::SharedPtr`
+- `src/pose_graph.h` — includes заменены, `nav_msgs::Path` → `nav_msgs::msg::Path`, `tf::TransformBroadcaster` → `tf2_ros::TransformBroadcaster`, `AngleLocalParameterization::Create()` → `ceres::AutoDiffManifold`, `registerPub(ros::NodeHandle&)` → `registerPub(rclcpp::Node::SharedPtr)`, publishers → `rclcpp::Publisher<...>::SharedPtr`
+- `src/utility/CameraPoseVisualization.h` — includes заменены, типы обновлены, `publish_by` и `publish_image_by` сигнатуры обновлены
+
+**НЕ мигрированы (нужно сделать):**
+
+#### `src/utility/CameraPoseVisualization.cpp`
+Заменить:
+- `geometry_msgs::Point` → `geometry_msgs::msg::Point`
+- `visualization_msgs::Marker` → `visualization_msgs::msg::Marker`
+- `visualization_msgs::MarkerArray` → `visualization_msgs::msg::MarkerArray`
+- `visualization_msgs::Marker::LINE_LIST` → `visualization_msgs::msg::Marker::LINE_LIST` (и т.д.)
+- `ros::Duration()` → `builtin_interfaces::msg::Duration()`
+- `ros::Publisher &pub` → `rclcpp::Publisher::SharedPtr pub` (в `publish_by`)
+- `ros::Publisher &pub` → `rclcpp::Publisher::SharedPtr pub` (в `publish_image_by`)
+- `const std_msgs::Header &header` → `const std_msgs::msg::Header &header`
+- `pub.publish(...)` → `pub->publish(...)`
+
+#### `src/pose_graph.cpp`
+Заменить:
+- `#include ` → удалить (уже в pose_graph.h)
+- `registerPub(ros::NodeHandle &n)` → `registerPub(rclcpp::Node::SharedPtr n)`
+- `n.advertise(topic, q)` → `n->create_publisher(topic, q)`
+- `tf_broadcaster.reset(new tf::TransformBroadcaster())` → `tf_broadcaster.reset(new tf2_ros::TransformBroadcaster(n))`
+- `ros::Time(cur_kf->time_stamp)` → helper: заполнить `builtin_interfaces::msg::Time` из double
+- TF broadcasting: `tf::Transform`, `tf::Vector3`, `tf::Quaternion`, `tf::StampedTransform` → `geometry_msgs::msg::TransformStamped`
+- `pub.publish(...)` → `pub->publish(...)`
+- `ceres::LocalParameterization*` → `ceres::Manifold*`
+- `problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization)` → `problem.AddParameterBlock(euler_array[i], 1); problem.SetManifold(euler_array[i], angle_local_parameterization)`
+
+#### `src/pose_graph_node.cpp`
+Заменить:
+- Все ROS1 includes → ROS2 (см. паттерн выше)
+- `ros::init` → `rclcpp::init`
+- `ros::NodeHandle n("~")` → `auto node = std::make_shared("pose_graph")`
+- `n.getParam(...)` → `node->declare_parameter(...)` + `node->get_parameter(...)`
+- `ros::package::getPath("pose_graph")` → `ament_index_cpp::get_package_share_directory("pose_graph")`
+- Все `ros::Subscriber` → `rclcpp::Subscription<...>::SharedPtr` через `node->create_subscription<>()`
+- Все `ros::Publisher` → `rclcpp::Publisher<...>::SharedPtr` через `node->create_publisher<>()`
+- `sensor_msgs::ImageConstPtr` → `sensor_msgs::msg::Image::ConstSharedPtr`
+- `sensor_msgs::PointCloudConstPtr` → `sensor_msgs::msg::PointCloud::ConstSharedPtr`
+- `nav_msgs::Odometry::ConstPtr` → `nav_msgs::msg::Odometry::ConstSharedPtr`
+- `cv_bridge::toCvCopy` — тот же API
+- `ROS_WARN(...)` → `RCLCPP_WARN(node->get_logger(), ...)`
+- `ROS_BREAK()` → `std::abort()`
+- `ros::Duration()` → `builtin_interfaces::msg::Duration()`
+- `visualization_msgs::Marker::SPHERE_LIST` → `visualization_msgs::msg::Marker::SPHERE_LIST`
+- `pub.publish(...)` → `pub->publish(...)`
+- `ros::spin()` → `rclcpp::spin(node)`
+- `std::thread process` и `std::thread command` — остаются без изменений
+
+**Подписчики (7 штук):**
+| Topic | Message Type | Callback |
+|-------|-------------|----------|
+| `/vins_estimator/imu_propagate` | `nav_msgs::msg::Odometry` | `imu_forward_callback` |
+| `/vins_estimator/odometry` | `nav_msgs::msg::Odometry` | `vio_callback` |
+| `IMAGE_TOPIC` (из конфига) | `sensor_msgs::msg::Image` | `image_callback` |
+| `/vins_estimator/keyframe_pose` | `nav_msgs::msg::Odometry` | `pose_callback` |
+| `/vins_estimator/extrinsic` | `nav_msgs::msg::Odometry` | `extrinsic_callback` |
+| `/vins_estimator/keyframe_point` | `sensor_msgs::msg::PointCloud` | `point_callback` |
+| `/vins_estimator/relo_relative_pose` | `nav_msgs::msg::Odometry` | `relo_relative_pose_callback` |
+
+**Издатели (5 в node + 9 в PoseGraph):**
+| Variable | Topic | Message Type |
+|----------|-------|-------------|
+| `pub_match_img` | `match_image` | `sensor_msgs::msg::Image` |
+| `pub_camera_pose_visual` | `camera_pose_visual` | `visualization_msgs::msg::MarkerArray` |
+| `pub_key_odometrys` | `key_odometrys` | `visualization_msgs::msg::Marker` |
+| `pub_vio_path` | `no_loop_path` | `nav_msgs::msg::Path` |
+| `pub_match_points` | `match_points` | `sensor_msgs::msg::PointCloud` |
+| `pub_pg_path` | `pose_graph_path` | `nav_msgs::msg::Path` |
+| `pub_base_path` | `base_path` | `nav_msgs::msg::Path` |
+| `pub_pose_graph` | `pose_graph` | `visualization_msgs::msg::MarkerArray` |
+| `pub_path[1-9]` | `path_1`...`path_9` | `nav_msgs::msg::Path` |
+
+#### `src/keyframe.cpp`
+Заменить:
+- `sensor_msgs::ImagePtr` → `sensor_msgs::msg::Image::SharedPtr`
+- `cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage).toImageMsg()` → `cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", thumbimage).toImageMsg()`
+- `msg->header.stamp = ros::Time(time_stamp)` → ручная конверсия double → `builtin_interfaces::msg::Time`
+- `sensor_msgs::PointCloud` → `sensor_msgs::msg::PointCloud`
+- `geometry_msgs::Point32` → `geometry_msgs::msg::Point32`
+- `sensor_msgs::ChannelFloat32` → `sensor_msgs::msg::ChannelFloat32`
+- `pub_match_img.publish(msg)` → `pub_match_img->publish(*msg)`
+- `pub_match_points.publish(msg_match_points)` → `pub_match_points->publish(msg_match_points)`
+
+#### `src/keyframe.h`
+- Не имеет прямых ROS includes, но включает `parameters.h` (уже мигрирован)
+- Проверить, что нет косвенных ROS зависимостей
+
+#### CMakeLists.txt
+Заменить по паттерну. Дополнительно:
+- `find_package(ament_index_cpp REQUIRED)` для `get_package_share_directory`
+- Установить support_files: `install(DIRECTORY ../../support_files/ DESTINATION share/${PROJECT_NAME}/support_files FILES_MATCHING PATTERN "*.bin" PATTERN "*.yml")`
+- ThirdParty source files включаются в `add_executable` (DBoW, DUtils, DVision, VocabularyBinary)
+
+#### package.xml
+Заменить по паттерну. Зависимости: `rclcpp`, `std_msgs`, `nav_msgs`, `sensor_msgs`, `visualization_msgs`, `geometry_msgs`, `cv_bridge`, `camera_model`, `tf2_ros`, `ament_index_cpp`
+
+### 5.2 vins_bringup ✅
+
+**Файлы:** `CMakeLists.txt`, `package.xml`, `launch/*.launch.py`, `launch/common.py`
+
+**Что делает пакет:**
+- Поднимает `feature_tracker`, `vins_estimator` и `pose_graph` в нужной конфигурации.
+- Поддерживает сценарии `euroc`, `3dm`, `usb_cam`, `realsense`, `cla`, `black_box`, `termal_cam`.
+- Отдельно запускает `rviz2` с готовым конфигом.
+
+**Что уже сделано:**
+- Создан `vins_bringup/CMakeLists.txt`.
+- Создан `vins_bringup/package.xml`.
+- Добавлены launch-файлы для основных сценариев.
+- Добавлен bag-specific launch `innospector_cam.launch.py` с `image_transport republish` для входа `/image_raw/compressed`.
+- Добавлен конфиг `config/fisheye_bag/fisheye_bag_config.yaml` под камеру `1280x1024` и topic `"/mavros/imu/data"`.
+- Конфиги из `config/` устанавливаются в `share/vins_bringup/config`.
+
+**Что осталось:**
+- Пакет собран.
+- Launch-файлы синтаксически проверены.
+
+---
+
+## 6. Создание vins_bringup (launch package) — ГОТОВО
+
+Пакет создан и собран; далее остаётся только финальная проверка всего workspace при необходимости.
+
+---
+
+## 7. Порядок оставшихся работ
+
+1. **Финальная сборка** — `colcon build --symlink-install --base-paths src` (все пакеты)
+2. **Тестирование** — запуск с реальными данными
+
+---
+
+## 8. Известные проблемы и решения
+
+| Проблема | Решение |
+|----------|---------|
+| `ceres::LocalParameterization` удалён в Ceres 2.2 | Использовать `ceres_local_parameterization_compat.h` shim или `ceres::Manifold` напрямую |
+| `cv_bridge/cv_bridge.h` не найден | В ROS2 Jazzy: `cv_bridge/cv_bridge.hpp` (не `.h`) |
+| `pub->publish(shared_ptr)` не компилируется | Разыменовать: `pub->publish(*msg)` |
+| `ros::Time(double)` нет прямого аналога | Ручная конверсия: `sec = (int32_t)t; nanosec = (uint32_t)((t - sec) * 1e9)` |
+| `ros::Duration()` для marker lifetime | `builtin_interfaces::msg::Duration()` |
+| `CV_GRAY2RGB` удалён в OpenCV 4.x | `cv::COLOR_GRAY2RGB` |
+| `AddParameterBlock(..., local_parameterization)` | `AddParameterBlock(...); SetManifold(..., local_parameterization)` |
+| `camera_model` не линкуется | Использовать imported target: `camera_model::camera_model` (не просто `camera_model`) |
+| Boost targets не найдены при линковке | Добавить `find_package(Boost REQUIRED COMPONENTS filesystem program_options system)` в downstream пакеты |
+| colcon не видит пакеты | Использовать `--base-paths src` (пакеты в подпапках `src/VINS-Mono-inno/`) |
+| `ROS_WARN_THROTTLE(rate, ...)` | `RCLCPP_WARN_SKIPFIRST_THROTTLE(logger, clock, ms, ...)` — нужен `rclcpp::Clock` |
+
+---
+
+## 9. Ссылки на эталонные файлы
+
+Для миграции оставшихся пакетов используй уже мигрированные файлы как образцы:
+
+| Что посмотреть | Эталонный файл |
+|---------------|---------------|
+| CMakeLists.txt (библиотека) | `camera_model/CMakeLists.txt` |
+| CMakeLists.txt (узел) | `feature_tracker/CMakeLists.txt` или `vins_estimator/CMakeLists.txt` |
+| package.xml | `feature_tracker/package.xml` |
+| Node main() | `feature_tracker/src/feature_tracker_node.cpp` |
+| parameters.cpp/h | `feature_tracker/src/parameters.cpp` или `vins_estimator/src/parameters.cpp` |
+| visualization с TF | `vins_estimator/src/utility/visualization.cpp` |
+| CameraPoseVisualization | `vins_estimator/src/utility/CameraPoseVisualization.cpp` |
+| Ceres Manifold usage | `vins_estimator/src/estimator.cpp` (строки ~710-718) |
+| stampToSec helper | `vins_estimator/src/utility/utility.h` |
+
+---
+
+## 10. Инновационные дополнения этого форка (сохранить при миграции)
+
+Эти функции добавлены в `UAV_perfom` branch и должны быть сохранены:
+
+1. **AGAST corner detector** + grid-based feature selection (`use_advanced_flow`)
+2. **Bidirectional optical flow** (`use_bidirectional_flow`)
+3. **Velocity-based adaptive feature tracking** (`enable_velocity_check`, `max_velocity_threshold`, `velocity_boost_features`)
+4. **IMU accelerometer filtering** (outlier rejection + exponential smoothing)
+5. **Zero Velocity Update (ZUPT)** (`enable_zupt`, `zupt_vel_threshold`, `zupt_acc_threshold`)
+6. **Altitude-based scale correction** via MAVLink velocity
+7. **Periodic td estimation** (`estimate_td_period`)
+8. **Thermal camera support** (`config/termal_cam_config.yaml`)
+9. **TF broadcasting** of loop-closed trajectory (`world → camera_pose_graph`)
+10. **Failure detection thresholds** tuned for high-speed UAV
+
+Все эти функции реализованы в коде и не требуют отдельных изменений — они используют те же ROS API, что и остальной код, и мигрируются автоматически.
+
+---
+
+## 11. Команды для проверки
+
+```bash
+# Сборка одного пакета
+cd ~/ros2_ws && source /opt/ros/jazzy/setup.bash && source install/setup.bash 2>/dev/null
+colcon build --symlink-install --packages-select --base-paths src
+
+# Сборка всех пакетов VINS-Mono
+colcon build --symlink-install --packages-select camera_model benchmark_publisher feature_tracker vins_estimator pose_graph vins_bringup --base-paths src
+
+# Проверка ошибок компиляции (без сборки)
+colcon build --packages-select --base-paths src --cmake-args -DCMAKE_BUILD_TYPE=Release 2>&1 | grep "error:"
+
+# Запуск узла
+ros2 run feature_tracker feature_tracker --ros-args -p config_file:=/path/to/config.yaml -p vins_folder:=/path/to/
+```
diff --git a/ROS2_MIGRATION_PLAN.md b/ROS2_MIGRATION_PLAN.md
new file mode 100644
index 000000000..93e3e32f0
--- /dev/null
+++ b/ROS2_MIGRATION_PLAN.md
@@ -0,0 +1,386 @@
+# VINS-Mono → ROS2 Migration Plan
+
+## Target Environment
+- **ROS2 Distro**: Jazzy Jalisco (installed at `/opt/ros/jazzy`)
+- **Hardware**: Jetson Orin NX (aarch64, 8 cores, 9.5 GB RAM)
+- **Build System**: `colcon build --symlink-install` in `~/ros2_ws`
+- **C++ Standard**: C++17 (required by ROS2 Jazzy / rclcpp)
+
+---
+
+## 1. Key Findings from Analysis
+
+### 1.1 Environment Status
+| Component | Status |
+|-----------|--------|
+| ROS2 Jazzy | ✅ Installed |
+| rclcpp, sensor_msgs, nav_msgs, geometry_msgs, std_msgs, visualization_msgs | ✅ Available |
+| tf2_ros, tf2_geometry_msgs, tf2_eigen | ✅ Available |
+| cv_bridge, image_transport, message_filters | ✅ Available |
+| ament_cmake, ament_index_cpp | ✅ Available |
+| Ceres 2.2.0, OpenCV 4.6, Eigen 3.4, Boost 1.83, yaml-cpp 0.8 | ✅ Available |
+| `sensor_msgs::msg::PointCloud` + `ChannelFloat32` | ✅ Available (deprecated but functional) |
+
+### 1.2 Critical Decision: PointCloud Message
+`sensor_msgs::msg::PointCloud` is **deprecated but still available** in ROS2 Jazzy.
+**Decision**: Use `sensor_msgs::msg::PointCloud` as-is for initial migration to minimize code changes. This avoids rewriting all the ChannelFloat32 encoding/decoding logic. Can migrate to PointCloud2 or custom messages later if needed.
+
+### 1.3 Package Summary (6 packages)
+| Package | ROS API Density | Migration Effort |
+|---------|----------------|-----------------|
+| `camera_model` | Minimal (library) | Low — mostly CMake changes |
+| `feature_tracker` | High | Medium |
+| `vins_estimator` | Very High | High — most complex |
+| `pose_graph` | Very High | High — includes ThirdParty libs |
+| `benchmark_publisher` | Medium | Low — simplest node |
+| `vins_bringup` | Low | Low — launch-only package |
+
+---
+
+## 2. Migration Architecture
+
+### 2.1 New Package Structure
+```
+~/ros2_ws/src/VINS-Mono-inno/
+├── vins_msgs/ # NEW: shared custom messages (optional, future)
+├── camera_model/ # Library + calibration tool
+├── feature_tracker/ # Node
+├── vins_estimator/ # Node (main estimator)
+├── pose_graph/ # Node (loop closure)
+├── benchmark_publisher/ # Node (benchmark)
+├── vins_bringup/ # Launch package
+├── config/ # Shared config (installed to share dir)
+├── support_files/ # Shared support files (installed to share dir)
+└── ... # Remaining repo files
+```
+
+### 2.2 Dependency Graph (build order)
+```
+camera_model (library, no ROS deps)
+ ↓
+feature_tracker, vins_estimator, pose_graph (depend on camera_model)
+ ↓
+benchmark_publisher (independent)
+ ↓
+vins_bringup (launch files, depends on all nodes)
+```
+
+---
+
+## 3. Per-Package Migration Steps
+
+### 3.1 `camera_model` (Library Package)
+
+**Changes needed:**
+1. **package.xml**: format 3, `ament_cmake`
+2. **CMakeLists.txt**:
+ - Remove `catkin` macros
+ - Use `add_library(camera_model ...)` + `ament_export_targets(export_camera_model HAS_LIBRARY_TARGET)`
+ - Install headers to `include/camodocal/`
+ - Install library
+3. **Source code**: No changes needed (pure C++ with Ceres/OpenCV/Boost)
+4. **Dependencies**: Remove `roscpp`, `std_msgs` (unused in library code)
+
+### 3.2 `feature_tracker`
+
+**package.xml changes:**
+```xml
+ament_cmake
+rclcpp
+std_msgs
+sensor_msgs
+cv_bridge
+camera_model
+```
+
+**CMakeLists.txt changes:**
+- `find_package(ament_cmake REQUIRED)`
+- `find_package(rclcpp REQUIRED)`
+- `find_package(sensor_msgs REQUIRED)`
+- `find_package(cv_bridge REQUIRED)`
+- `find_package(camera_model REQUIRED)`
+- `add_executable(feature_tracker_node src/feature_tracker_node.cpp ...)`
+- `ament_target_dependencies(feature_tracker_node rclcpp sensor_msgs cv_bridge camera_model)`
+- `install(TARGETS feature_tracker_node DESTINATION lib/${PROJECT_NAME})`
+
+**Source code changes:**
+
+`feature_tracker_node.cpp`:
+```cpp
+// ROS1 → ROS2
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "sensor_msgs/msg/point_cloud.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "cv_bridge/cv_bridge.h"
+
+// Node class instead of global callbacks
+class FeatureTrackerNode : public rclcpp::Node {
+ // subscriptions, publishers as members
+ // img_callback as member function
+};
+
+int main(int argc, char** argv) {
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+}
+```
+
+`parameters.cpp`:
+```cpp
+// readParam(n, "key") → node->declare_parameter("key", default)
+// cv::FileStorage usage stays unchanged
+```
+
+**Key API mappings:**
+| ROS1 | ROS2 |
+|------|------|
+| `ros::init(argc, argv, "name")` | `rclcpp::init(argc, argv)` |
+| `ros::NodeHandle n("~")` | `rclcpp::Node` (use `~` via remapping or NodeOptions) |
+| `n.subscribe(topic, q, cb)` | `node->create_subscription(topic, qos, cb)` |
+| `n.advertise(topic, q)` | `node->create_publisher(topic, qos)` |
+| `pub.publish(msg)` | `pub->publish(msg)` |
+| `ros::spin()` | `rclcpp::spin(node)` |
+| `ros::Time::now().toSec()` | `node->now().seconds()` |
+| `ROS_INFO(...)` | `RCLCPP_INFO(node->get_logger(), ...)` |
+| `ROS_WARN_THROTTLE(2.0, ...)` | `RCLCPP_WARN_THROTTLE(node->get_logger(), *node->get_clock(), 2000ms, ...)` |
+| `sensor_msgs::ImageConstPtr` | `sensor_msgs::msg::Image::ConstSharedPtr` |
+| `sensor_msgs::PointCloud` | `sensor_msgs::msg::PointCloud` |
+| `cv_bridge::toCvCopy` | `cv_bridge::toCvCopy` (same API) |
+
+### 3.3 `vins_estimator` (Most Complex)
+
+**Major changes:**
+1. **estimator_node.cpp**: Convert to `rclcpp::Node` class
+ - 5 subscribers, 12+ publishers
+ - `ros::TransportHints().tcpNoDelay()` → `rclcpp::QoS` with `BestEffort` reliability
+ - `std::thread measurement_process` stays the same
+ - `std::mutex` / `std::condition_variable` stays the same
+2. **estimator.h**: `std_msgs::Header` → `std_msgs::msg::Header`
+3. **estimator.cpp**: `ros::Time::now().toSec()` → pass clock or use `rclcpp::Clock`
+4. **visualization.cpp**:
+ - `tf::TransformBroadcaster` → `tf2_ros::TransformBroadcaster`
+ - `tf::Transform` → `geometry_msgs::msg::TransformStamped`
+ - All publishers → `rclcpp::Publisher<...>::SharedPtr`
+5. **CameraPoseVisualization.cpp**: `ros::Publisher` → `rclcpp::Publisher`
+6. **parameters.cpp**: Same pattern as feature_tracker
+
+**QoS for IMU subscriber:**
+```cpp
+rclcpp::QoS imu_qos(2000);
+imu_qos.best_effort(); // replaces tcpNoDelay
+auto sub_imu = node->create_subscription(
+ IMU_TOPIC, imu_qos, imu_callback);
+```
+
+**TF broadcasting:**
+```cpp
+#include "tf2_ros/transform_broadcaster.h"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+// In visualization:
+static tf2_ros::TransformBroadcaster br(node);
+geometry_msgs::msg::TransformStamped transform;
+transform.header.stamp = node->now();
+transform.header.frame_id = "world";
+transform.child_frame_id = "body";
+// ... set translation/rotation
+br.sendTransform(transform);
+```
+
+### 3.4 `pose_graph`
+
+**Major changes:**
+1. **pose_graph_node.cpp**: Convert to `rclcpp::Node` class
+ - 7 subscribers, 5+ publishers
+ - `ros::package::getPath("pose_graph")` → `ament_index_cpp::get_package_share_directory("pose_graph")`
+ - `std::thread process()` and `std::thread command()` stay the same
+2. **pose_graph.cpp**:
+ - TF broadcaster → `tf2_ros::TransformBroadcaster`
+ - All publishers → `rclcpp::Publisher`
+ - `ros::Time(double)` → `rclcpp::Time(double * 1e9)` or manual conversion
+3. **ThirdParty/** (DBoW, DUtils, DVision, VocabularyBinary): **No changes** — pure C++
+4. **Support files**: Install `brief_k10L6.bin` and `brief_pattern.yml` to share directory
+5. **parameters.h**: Remove `ros/ros.h`, update includes
+
+**Support files installation:**
+```cmake
+install(DIRECTORY ../support_files/
+ DESTINATION share/${PROJECT_NAME}/support_files
+ FILES_MATCHING PATTERN "*.bin" PATTERN "*.yml")
+```
+
+### 3.5 `ar_demo` (removed from current scope)
+
+This package is intentionally out of scope for the current migration and has been removed from the repository tree.
+
+### 3.6 `benchmark_publisher`
+
+**Changes:** Minimal — convert to `rclcpp::Node`, remove unused `tf` dependency.
+
+### 3.7 `data_generator` (removed from current scope)
+
+This package is intentionally out of scope for the current migration and has been removed from the repository tree.
+
+---
+
+## 4. Launch File Migration
+
+All VINS launch XML files → `.launch.py` (Python) in new `vins_bringup` package.
+
+**Example ROS2 launch:**
+```python
+# vins_bringup/launch/euroc.launch.py
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+ config_path = os.path.join(
+ get_package_share_directory('vins_bringup'), 'config', 'euroc', 'euroc_config.yaml')
+
+ return LaunchDescription([
+ Node(
+ package='feature_tracker',
+ executable='feature_tracker_node',
+ name='feature_tracker',
+ output='screen',
+ parameters=[{
+ 'config_file': config_path,
+ 'vins_folder': os.path.dirname(config_path),
+ }],
+ remappings=[],
+ ),
+ Node(
+ package='vins_estimator',
+ executable='vins_estimator_node',
+ name='vins_estimator',
+ output='screen',
+ parameters=[{
+ 'config_file': config_path,
+ 'vins_folder': os.path.dirname(config_path),
+ }],
+ ),
+ Node(
+ package='pose_graph',
+ executable='pose_graph_node',
+ name='pose_graph',
+ output='screen',
+ parameters=[{
+ 'config_file': config_path,
+ 'visualization_shift_x': 0,
+ 'visualization_shift_y': 0,
+ 'skip_cnt': 0,
+ 'skip_dis': 0.0,
+ }],
+ ),
+ ])
+```
+
+---
+
+## 5. Config & Support Files Installation
+
+**New `vins_bringup` package** will install shared resources:
+```cmake
+install(DIRECTORY ../config/
+ DESTINATION share/${PROJECT_NAME}/config)
+install(DIRECTORY ../support_files/
+ DESTINATION share/${PROJECT_NAME}/support_files)
+install(DIRECTORY launch/
+ DESTINATION share/${PROJECT_NAME}/launch)
+```
+
+---
+
+## 6. Jetson Orin NX Optimization Notes
+
+### 6.1 Build Optimization
+- Use `-O2` or `-O3` optimization (already in original CMakeLists)
+- Enable NEON SIMD: `-mfpu=neon` (arm64 has NEON by default)
+- Use `-j$(nproc)` for parallel build (8 cores available)
+- Consider `-flto` for link-time optimization
+
+### 6.2 Runtime Optimization
+- **QoS profiles**: Use `BestEffort` for sensor data (IMU, images) to reduce overhead
+- **Thread affinity**: Original `taskset -c N` in black_box.launch can be preserved
+- **Memory**: 9.5 GB RAM — sufficient for VINS-Mono
+- **Feature count**: Already optimized in this fork (150-200 features)
+
+### 6.3 CMake Optimization Flags
+```cmake
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DNDEBUG -fopenmp")
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+```
+
+---
+
+## 7. Migration Order (Recommended)
+
+1. **camera_model** — library, no ROS deps, foundation for others
+2. **benchmark_publisher** — simplest node, good test of build system
+3. **feature_tracker** — medium complexity, core pipeline
+4. **vins_estimator** — most complex, depends on camera_model
+5. **pose_graph** — complex, depends on camera_model + ThirdParty
+6. **vins_bringup** — launch files, depends on all nodes
+
+---
+
+## 8. Common ROS1 → ROS2 API Mapping (Quick Reference)
+
+| ROS1 | ROS2 |
+|------|------|
+| `#include ` | `#include "rclcpp/rclcpp.hpp"` |
+| `#include ` | `#include "sensor_msgs/msg/image.hpp"` |
+| `#include ` | `#include "nav_msgs/msg/odometry.hpp"` |
+| `#include ` | `#include "geometry_msgs/msg/point.hpp"` |
+| `#include ` | `#include "std_msgs/msg/bool.hpp"` |
+| `#include ` | `#include "visualization_msgs/msg/marker.hpp"` |
+| `#include ` | `#include "tf2_ros/transform_broadcaster.h"` |
+| `#include ` | `#include "cv_bridge/cv_bridge.h"` (same) |
+| `ros::init(argc, argv, "node")` | `rclcpp::init(argc, argv)` |
+| `ros::NodeHandle n("~")` | `rclcpp::Node::make_shared("node")` |
+| `n.getParam("key", var)` | `node->declare_parameter("key", default); node->get_parameter("key", var)` |
+| `n.subscribe(topic, q, cb)` | `node->create_subscription(topic, QoS, cb)` |
+| `n.advertise(topic, q)` | `node->create_publisher(topic, QoS)` |
+| `ros::spin()` | `rclcpp::spin(node)` |
+| `ros::ok()` | `rclcpp::ok()` |
+| `ros::Rate r(hz)` | `rclcpp::Rate r(hz)` |
+| `ros::Time::now().toSec()` | `node->now().seconds()` |
+| `ros::Time(t).toSec()` | `rclcpp::Time(t).seconds()` |
+| `ros::Duration(d).sleep()` | `rclcpp::sleep_for(std::chrono::duration(d))` |
+| `ROS_INFO(...)` | `RCLCPP_INFO(rclcpp::get_logger("name"), ...)` |
+| `ROS_WARN(...)` | `RCLCPP_WARN(rclcpp::get_logger("name"), ...)` |
+| `ROS_DEBUG(...)` | `RCLCPP_DEBUG(rclcpp::get_logger("name"), ...)` |
+| `ROS_ASSERT(cond)` | `assert(cond)` |
+| `ROS_BREAK()` | `std::abort()` |
+| `sensor_msgs::ImageConstPtr` | `sensor_msgs::msg::Image::ConstSharedPtr` |
+| `sensor_msgs::PointCloudPtr` | `sensor_msgs::msg::PointCloud::SharedPtr` |
+| `msg->header.stamp.toSec()` | `rclcpp::Time(msg->header.stamp).seconds()` |
+| `msg->header.stamp = ros::Time(t)` | `msg->header.stamp = rclcpp::Time(t).to_msg()` or manual |
+| `ros::package::getPath("pkg")` | `ament_index_cpp::get_package_share_directory("pkg")` |
+| `catkin_package(...)` | `ament_export_*` macros |
+| `add_dependencies(target catkin_package)` | Not needed in ament |
+
+---
+
+## 9. Potential Issues & Mitigations
+
+| Issue | Mitigation |
+|-------|------------|
+| `sensor_msgs::msg::PointCloud` deprecated | Works in Jazzy; plan future migration to PointCloud2 |
+| `ros::Time::now()` in library code (estimator.cpp) | Pass `rclcpp::Clock` or node pointer |
+| `std_msgs::Header` in estimator.h | Change to `std_msgs::msg::Header` |
+| TF static broadcaster in visualization.cpp | Make it a class member or shared pointer |
+| `ros::TransportHints().tcpNoDelay()` | Use `rclcpp::QoS` with `BestEffort` |
+| Global callback functions with global state | Convert to class member functions |
+| `ROS_WARN_THROTTLE(rate, ...)` | `RCLCPP_WARN_THROTTLE(logger, clock, ms, ...)` |
+| Support files path (`pkg_path + "/../support_files/"`) | Install to share dir, use `get_package_share_directory` |
+| `cv::FileStorage` YAML with `!!opencv-matrix` | Works unchanged (ROS-independent) |
+| Multi-threaded spin + processing thread | Keep `std::thread` pattern, use `rclcpp::spin` in main |
diff --git a/USB_CAM_SETUP.md b/USB_CAM_SETUP.md
new file mode 100644
index 000000000..97e721619
--- /dev/null
+++ b/USB_CAM_SETUP.md
@@ -0,0 +1,248 @@
+# Адаптация VINS-Mono для USB камеры и MAVROS IMU
+
+## Обзор конфигурации
+
+Созданы два новых файла для вашей системы:
+- **`config/usb_cam_config.yaml`** - конфигурация параметров
+- **`vins_estimator/launch/usb_cam.launch`** - файл запуска
+
+## Параметры камеры
+
+### Разрешение и калибровка
+```
+Разрешение: 640x480
+Матрица камеры K:
+ fx = 473.41 пикселей
+ fy = 473.57 пикселей
+ cx = 311.40 пикселей (центр по X)
+ cy = 256.94 пикселей (центр по Y)
+
+Дистortion (модель plumb_bob):
+ k1 = 0.0516 (радиальное искажение)
+ k2 = -0.0484 (радиальное искажение)
+ p1 = 0.0047 (тангенциальное искажение)
+ p2 = -0.0033 (тангенциальное искажение)
+```
+
+## Экстринсики камеры-IMU (Extrinsics)
+
+### Геометрия
+Ваша система имеет следующее расположение:
+```
+IMU координаты: Камера повернута на 90° вправо
+X - forward относительно IMU
+Y - left
+Z - up
+```
+
+### Матрица вращения (Rotation Matrix)
+Поворот камеры на -90° вокруг оси Z (вправо по курсу):
+```
+imu^R_cam = [0 1 0]
+ [-1 0 0]
+ [0 0 1]
+
+Преобразование: imu_point = R_cam * camera_point
+```
+
+### Вектор трансляции (Translation)
+```
+imu^T_cam = [0, 0, 0]^T (камера находится в центре IMU)
+```
+
+Если камера смещена относительно IMU, отредактируйте значения в строках:
+```yaml
+extrinsicTranslation: !!opencv-matrix
+ rows: 3
+ cols: 1
+ dt: d
+ data: [dx, dy, dz] # Смещения в метрах
+```
+
+## ROS Топики
+
+### Входящие сигналы:
+- **Изображения:** `/usb_cam/image_raw`
+- **Данные IMU:** `/mavros/imu/data`
+
+### Исходящие сигналы:
+- `/vins_estimator/odometry` - визуально-инерциальная одометрия
+- `/vins_estimator/path` - траектория движения
+- `/vins_estimator/relo_relative_pose` - релокализация
+- `/feature_tracker/feature` - отслеживаемые признаки (для отладки)
+- `/pose_graph/base_path` - оптимизированная траектория
+
+## Запуск системы
+
+### 1. Сборка проекта
+```bash
+cd ~/catkin_ws
+catkin_make
+source devel/setup.bash
+```
+
+### 2. Запуск USB камеры (если еще не запущена)
+```bash
+roslaunch usb_cam usb_cam-test.launch
+# или
+rosrun usb_cam usb_cam_node
+```
+
+### 3. Запуск MAVROS
+```bash
+roslaunch mavros apm.launch fcu_url:=/dev/ttyUSB0:921600
+# или другой адрес вашего автопилота
+```
+
+### 4. Запуск VINS-Mono с USB камерой
+```bash
+roslaunch vins_estimator usb_cam.launch
+```
+
+В другом терминале для визуализации:
+```bash
+roslaunch vins_estimator vins_rviz.launch
+```
+
+## Параметры, которые можно настроить
+
+### Отслеживание признаков
+Файл: `config/usb_cam_config.yaml`
+```yaml
+max_cnt: 150 # Максимум отслеживаемых точек (↑ = точнее, но медленнее)
+min_dist: 30 # Минимальное расстояние между точками (в пиксе)
+freq: 10 # Частота обновления (Hz)
+F_threshold: 1.0 # Порог RANSAC (пиксели)
+show_track: 1 # Показывать отслеживаемые точки
+equalize: 1 # Выравнивание гистограммы (для темных/светлых изображений)
+```
+
+### Оптимизация
+```yaml
+max_solver_time: 0.04 # Макс время решения (мс) для реалтайма
+max_num_iterations: 8 # Макс итераций оптимизатора
+keyframe_parallax: 10.0 # Порог параллакса для выбора ключевого кадра
+```
+
+### IMU параметры
+```yaml
+acc_n: 0.08 # Шум акселерометра
+gyr_n: 0.004 # Шум гироскопа
+acc_w: 0.00004 # Дрейф акселерометра
+gyr_w: 2.0e-6 # Дрейф гироскопа
+g_norm: 9.81007 # Ускорение свободного падения
+```
+
+Эти значения подобраны для типичного 9-DOF IMU. Если результаты неудовлетворительны, их можно調整.
+
+### Замыкание цикла
+```yaml
+loop_closure: 1 # Включить обнаружение замыкания цикла
+fast_relocalization: 0 # Быстрая релокализация (для больших карт)
+load_previous_pose_graph: 0 # Переиспользовать старый граф
+```
+
+## Отладка и тестирование
+
+### Проверить топики
+```bash
+rostopic list # Список всех топиков
+rostopic echo /usb_cam/image_raw
+rostopic echo /mavros/imu/data
+rostopic echo /vins_estimator/odometry
+```
+
+### Проверить частоту обновления
+```bash
+rostopic hz /usb_cam/image_raw
+rostopic hz /mavros/imu/data
+rostopic hz /vins_estimator/odometry
+```
+
+### Визуализация в RVIZ
+```bash
+# Откроется RVIZ с предконфигурированными трансформациями
+roslaunch vins_estimator vins_rviz.launch
+```
+
+Добавьте подписки на топики:
+- `Image` → `/usb_cam/image_raw`
+- `Odometry` → `/vins_estimator/odometry`
+- `Path` → `/vins_estimator/path`
+- `Point Cloud` → `/feature_tracker/feature` (для отладки)
+
+## Синхронизация камеры-IMU
+
+Если камера и IMU работают с разными временными метками:
+
+1. **Включите автоматическую временную калибровку:**
+```yaml
+estimate_td: 1 # вместо 0
+td: 0.0 # начальное смещение времени
+```
+
+2. **Система будет автоматически калибровать временное смещение** между камерой и IMU
+
+## Проблемы и решения
+
+### Проблема: "Waiting for features..."
+**Решение:** Убедитесь, что:
+- Камера получает достаточно света
+- `max_cnt` не слишком мал
+- `equalize: 1` для темных изображений
+
+### Проблема: "Cannot find corresponding features"
+**Решение:**
+- Уменьшите `min_dist` (минимальное расстояние между точками)
+- Увеличьте `max_cnt` (максимум точек)
+- Проверьте калибровку камеры
+
+### Проблема: "IMU data drift"
+**Решение:**
+- Отредактируйте параметры `acc_w` и `gyr_w` в конфиге
+- Используйте более точный IMU
+
+### Проблема: Неправильная ориентация камеры
+**Решение:** Отредактируйте матрицу вращения в конфиге:
+```yaml
+extrinsicRotation: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ data: [...] # Измените эту матрицу
+```
+
+## Сохранение и загрузка карты
+
+### Сохранить текущую карту:
+В терминале `vins_estimator` нажмите **`s` + Enter**
+
+### Загрузить сохраненную карту:
+Установите в конфиге:
+```yaml
+load_previous_pose_graph: 1
+pose_graph_save_path: "/path/to/saved/pose_graph/"
+```
+
+## Контрольный список перед запуском
+
+- [ ] Откалибрована USB камера (параметры K, D обновлены)
+- [ ] MAVROS работает и публикует `/mavros/imu/data`
+- [ ] USB камера работает и публикует `/usb_cam/image_raw`
+- [ ] Проверены топики: `rostopic list | grep -E "usb_cam|mavros|imu"`
+- [ ] Созданы директории вывода: `mkdir -p /home/op/output/pose_graph/`
+- [ ] Выполнена сборка: `cd ~/catkin_ws && catkin_make`
+
+## Справка по файлам
+
+- **Основной конфиг:** `config/usb_cam_config.yaml`
+- **Launch файл:** `vins_estimator/launch/usb_cam.launch`
+- **Код оценивателя:** `vins_estimator/src/estimator_node.cpp`
+- **Отслеживание признаков:** `feature_tracker/src/feature_tracker_node.cpp`
+- **Граф позиций:** `pose_graph/src/pose_graph_node.cpp`
+
+## Дополнительные ресурсы
+
+- [VINS-Mono GitHub](https://github.com/HKUST-Aerial-Robotics/VINS-Mono)
+- [VINS-Mono Paper (IEEE)](https://ieeexplore.ieee.org/document/8421746)
+- [ROS Navigation](http://wiki.ros.org/ROS/Tutorials)
diff --git a/ar_demo/CMakeLists.txt b/ar_demo/CMakeLists.txt
deleted file mode 100644
index fc4e682b6..000000000
--- a/ar_demo/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(ar_demo)
-
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE")
-#-DEIGEN_USE_MKL_ALL")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
-
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- rospy
- std_msgs
- image_transport
- sensor_msgs
- cv_bridge
- message_filters
- camera_model
-)
-find_package(OpenCV REQUIRED)
-
-catkin_package(
-
-)
-
-
-include_directories(
- ${catkin_INCLUDE_DIRS}
-)
-
-set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
-find_package(Eigen3)
-include_directories(
- ${catkin_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIR}
-)
-
-add_executable(ar_demo_node src/ar_demo_node.cpp)
-
- target_link_libraries(ar_demo_node
- ${catkin_LIBRARIES} ${OpenCV_LIBS}
- )
-
-
diff --git a/ar_demo/cmake/FindEigen.cmake b/ar_demo/cmake/FindEigen.cmake
deleted file mode 100644
index cdeb30557..000000000
--- a/ar_demo/cmake/FindEigen.cmake
+++ /dev/null
@@ -1,172 +0,0 @@
-# Ceres Solver - A fast non-linear least squares minimizer
-# Copyright 2015 Google Inc. All rights reserved.
-# http://ceres-solver.org/
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-# * Redistributions of source code must retain the above copyright notice,
-# this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright notice,
-# this list of conditions and the following disclaimer in the documentation
-# and/or other materials provided with the distribution.
-# * Neither the name of Google Inc. nor the names of its contributors may be
-# used to endorse or promote products derived from this software without
-# specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-# Author: alexs.mac@gmail.com (Alex Stewart)
-#
-
-# FindEigen.cmake - Find Eigen library, version >= 3.
-#
-# This module defines the following variables:
-#
-# EIGEN_FOUND: TRUE iff Eigen is found.
-# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
-#
-# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
-# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
-# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
-# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
-#
-# The following variables control the behaviour of this module:
-#
-# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
-# search for eigen includes, e.g: /timbuktu/eigen3.
-#
-# The following variables are also defined by this module, but in line with
-# CMake recommended FindPackage() module style should NOT be referenced directly
-# by callers (use the plural variables detailed above instead). These variables
-# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
-# are NOT re-called (i.e. search for library is not repeated) if these variables
-# are set with valid values _in the CMake cache_. This means that if these
-# variables are set directly in the cache, either by the user in the CMake GUI,
-# or by the user passing -DVAR=VALUE directives to CMake when called (which
-# explicitly defines a cache variable), then they will be used verbatim,
-# bypassing the HINTS variables and other hard-coded search locations.
-#
-# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
-# include directory of any dependencies.
-
-# Called if we failed to find Eigen or any of it's required dependencies,
-# unsets all public (designed to be used externally) variables and reports
-# error message at priority depending upon [REQUIRED/QUIET/] argument.
-macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
- unset(EIGEN_FOUND)
- unset(EIGEN_INCLUDE_DIRS)
- # Make results of search visible in the CMake GUI if Eigen has not
- # been found so that user does not have to toggle to advanced view.
- mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
- # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
- # use the camelcase library name, not uppercase.
- if (Eigen_FIND_QUIETLY)
- message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
- elseif (Eigen_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use no priority which emits a message
- # but continues configuration and allows generation.
- message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(EIGEN_REPORT_NOT_FOUND)
-
-# Protect against any alternative find_package scripts for this library having
-# been called previously (in a client project) which set EIGEN_FOUND, but not
-# the other variables we require / set here which could cause the search logic
-# here to fail.
-unset(EIGEN_FOUND)
-
-# Search user-installed locations first, so that we prefer user installs
-# to system installs where both exist.
-list(APPEND EIGEN_CHECK_INCLUDE_DIRS
- /usr/local/include
- /usr/local/homebrew/include # Mac OS X
- /opt/local/var/macports/software # Mac OS X.
- /opt/local/include
- /usr/include)
-# Additional suffixes to try appending to each search path.
-list(APPEND EIGEN_CHECK_PATH_SUFFIXES
- eigen3 # Default root directory for Eigen.
- Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3
- Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3
-
-# Search supplied hint directories first if supplied.
-find_path(EIGEN_INCLUDE_DIR
- NAMES Eigen/Core
- PATHS ${EIGEN_INCLUDE_DIR_HINTS}
- ${EIGEN_CHECK_INCLUDE_DIRS}
- PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
-
-if (NOT EIGEN_INCLUDE_DIR OR
- NOT EXISTS ${EIGEN_INCLUDE_DIR})
- eigen_report_not_found(
- "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
- "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
-endif (NOT EIGEN_INCLUDE_DIR OR
- NOT EXISTS ${EIGEN_INCLUDE_DIR})
-
-# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
-# if called.
-set(EIGEN_FOUND TRUE)
-
-# Extract Eigen version from Eigen/src/Core/util/Macros.h
-if (EIGEN_INCLUDE_DIR)
- set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
- if (NOT EXISTS ${EIGEN_VERSION_FILE})
- eigen_report_not_found(
- "Could not find file: ${EIGEN_VERSION_FILE} "
- "containing version information in Eigen install located at: "
- "${EIGEN_INCLUDE_DIR}.")
- else (NOT EXISTS ${EIGEN_VERSION_FILE})
- file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
-
- string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
- EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
- string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
- EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
-
- string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
- EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
- string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
- EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
-
- string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
- EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
- string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
- EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
-
- # This is on a single line s/t CMake does not interpret it as a list of
- # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
- set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
- endif (NOT EXISTS ${EIGEN_VERSION_FILE})
-endif (EIGEN_INCLUDE_DIR)
-
-# Set standard CMake FindPackage variables if found.
-if (EIGEN_FOUND)
- set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
-endif (EIGEN_FOUND)
-
-# Handle REQUIRED / QUIET optional arguments and version.
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(Eigen
- REQUIRED_VARS EIGEN_INCLUDE_DIRS
- VERSION_VAR EIGEN_VERSION)
-
-# Only mark internal variables as advanced if we found Eigen, otherwise
-# leave it visible in the standard GUI for the user to set manually.
-if (EIGEN_FOUND)
- mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
-endif (EIGEN_FOUND)
diff --git a/ar_demo/launch/3dm_bag.launch b/ar_demo/launch/3dm_bag.launch
deleted file mode 100644
index 8e1b300cd..000000000
--- a/ar_demo/launch/3dm_bag.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/ar_demo/launch/ar_rviz.launch b/ar_demo/launch/ar_rviz.launch
deleted file mode 100644
index 105063b46..000000000
--- a/ar_demo/launch/ar_rviz.launch
+++ /dev/null
@@ -1,3 +0,0 @@
-
-
-
\ No newline at end of file
diff --git a/ar_demo/launch/realsense_ar.launch b/ar_demo/launch/realsense_ar.launch
deleted file mode 100755
index 68c63ee77..000000000
--- a/ar_demo/launch/realsense_ar.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
diff --git a/ar_demo/package.xml b/ar_demo/package.xml
deleted file mode 100644
index 74270241d..000000000
--- a/ar_demo/package.xml
+++ /dev/null
@@ -1,58 +0,0 @@
-
-
- ar_demo
- 0.0.0
- The ar_demo package
-
-
-
-
- tony-ws
-
-
-
-
-
- TODO
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- roscpp
- rospy
- std_msgs
- camera_model
- roscpp
- rospy
- std_msgs
- camera_model
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/ar_demo/src/ar_demo_node.cpp b/ar_demo/src/ar_demo_node.cpp
deleted file mode 100644
index 95c3a220a..000000000
--- a/ar_demo/src/ar_demo_node.cpp
+++ /dev/null
@@ -1,557 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include "camodocal/camera_models/CameraFactory.h"
-#include "camodocal/camera_models/CataCamera.h"
-#include "camodocal/camera_models/PinholeCamera.h"
-#include
-#include
-#include
-#include
-#include
-
-using namespace std;
-using namespace Eigen;
-using namespace sensor_msgs;
-using namespace message_filters;
-using namespace camodocal;
-
-int ROW;
-int COL;
-double FOCAL_LENGTH;
-const int axis_num = 0;
-const int cube_num = 1;
-const double box_length = 0.8;
-bool USE_UNDISTORED_IMG;
-bool pose_init = false;
-int img_cnt = 0;
-
-ros::Publisher object_pub;
-image_transport::Publisher pub_ARimage;
-Vector3d Axis[6];
-Vector3d Cube_center[3];
-vector Cube_corner[3];
-vector output_Axis[6];
-vector output_Cube[3];
-vector output_corner_dis[3];
-double Cube_center_depth[3];
-queue img_buf;
-camodocal::CameraPtr m_camera;
-bool look_ground = 0;
-std_msgs::ColorRGBA line_color_r;
-std_msgs::ColorRGBA line_color_g;
-std_msgs::ColorRGBA line_color_b;
-
-void axis_generate(visualization_msgs::Marker &line_list, Vector3d &origin, int id)
-{
-
- line_list.id = id;
- line_list.header.frame_id = "world";
- line_list.header.stamp = ros::Time::now();
- line_list.action = visualization_msgs::Marker::ADD;
- line_list.type = visualization_msgs::Marker::LINE_LIST;
- line_list.scale.x = 0.1;
- line_list.color.a = 1.0;
- line_list.lifetime = ros::Duration();
-
- line_list.pose.orientation.w = 1.0;
- line_list.color.b = 1.0;
- geometry_msgs::Point p;
- p.x = origin.x();
- p.y = origin.y();
- p.z = origin.z();
- line_list.points.push_back(p);
- line_list.colors.push_back(line_color_r);
- p.x += 1.0;
- line_list.points.push_back(p);
- line_list.colors.push_back(line_color_r);
- p.x -= 1.0;
- line_list.points.push_back(p);
- line_list.colors.push_back(line_color_g);
- p.y += 1.0;
- line_list.points.push_back(p);
- line_list.colors.push_back(line_color_g);
- p.y -= 1.0;
- line_list.points.push_back(p);
- line_list.colors.push_back(line_color_b);
- p.z += 1.0;
- line_list.points.push_back(p);
- line_list.colors.push_back(line_color_b);
-}
-
-void cube_generate(visualization_msgs::Marker &marker, Vector3d &origin, int id)
-{
-
- //uint32_t shape = visualization_msgs::Marker::CUBE;
- marker.header.frame_id = "world";
- marker.header.stamp = ros::Time::now();
- marker.ns = "basic_shapes";
- marker.id = 0;
- //marker.type = shape;
- marker.action = visualization_msgs::Marker::ADD;
- marker.type = visualization_msgs::Marker::CUBE_LIST;
- /*
- marker.pose.position.x = origin.x();
- marker.pose.position.y = origin.y();
- marker.pose.position.z = origin.z();
- marker.pose.orientation.x = 0.0;
- marker.pose.orientation.y = 0.0;
- marker.pose.orientation.z = 0.0;
- marker.pose.orientation.w = 1.0;
- */
- marker.scale.x = box_length;
- marker.scale.y = box_length;
- marker.scale.z = box_length;
-
- marker.color.r = 0.0f;
- marker.color.g = 1.0f;
- marker.color.b = 0.0f;
- marker.color.a = 1.0;
-
- marker.lifetime = ros::Duration();
- geometry_msgs::Point p;
- p.x = origin.x();
- p.y = origin.y();
- p.z = origin.z();
- marker.points.push_back(p);
- marker.colors.push_back(line_color_r);
- Cube_corner[id].clear();
- Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() - box_length / 2, origin.z() - box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() - box_length / 2, origin.z() - box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() + box_length / 2, origin.z() - box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() + box_length / 2, origin.z() - box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() - box_length / 2, origin.z() + box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() - box_length / 2, origin.z() + box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() - box_length / 2, origin.y() + box_length / 2, origin.z() + box_length / 2));
- Cube_corner[id].push_back(Vector3d(origin.x() + box_length / 2, origin.y() + box_length / 2, origin.z() + box_length / 2));
-}
-
-void add_object()
-{
- visualization_msgs::MarkerArray markerArray_msg;
-
- visualization_msgs::Marker line_list;
- visualization_msgs::Marker cube_list;
-
- for (int i = 0; i < axis_num; i++)
- {
- axis_generate(line_list, Axis[i], i);
- markerArray_msg.markers.push_back(line_list);
- }
-
- for (int i = 0; i spaceToPlane(local_point, local_uv);
-
- if (local_point.z() > 0)
- //&& 0 <= local_uv.x() && local_uv.x() <= COL - 1 && 0 <= local_uv.y() && local_uv.y() <= ROW -1)
- {
- output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));
-
- local_point = camera_q.inverse() * (Axis[i] + Vector3d(1, 0, 0) - camera_p);
- m_camera->spaceToPlane(local_point, local_uv);
- output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));
-
- local_point = camera_q.inverse() * (Axis[i] + Vector3d(0, 1, 0) - camera_p);
- m_camera->spaceToPlane(local_point, local_uv);
- output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));
-
- local_point = camera_q.inverse() * (Axis[i] + Vector3d(0, 0, 1) - camera_p);
- m_camera->spaceToPlane(local_point, local_uv);
- output_Axis[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));
-
- }
- }
-
- for (int i = 0; i < cube_num; i++)
- {
- output_Cube[i].clear();
- output_corner_dis[i].clear();
- Vector3d local_point;
- Vector2d local_uv;
- local_point = camera_q.inverse() * (Cube_center[i] - camera_p);
- if (USE_UNDISTORED_IMG)
- {
- local_uv.x() = local_point(0) / local_point(2) * FOCAL_LENGTH + COL / 2;
- local_uv.y() = local_point(1) / local_point(2) * FOCAL_LENGTH + ROW / 2;
- }
- else
- m_camera->spaceToPlane(local_point, local_uv);
- if (local_point.z() > box_length / 2)
- //&& 0 <= local_uv.x() && local_uv.x() <= COL - 1 && 0 <= local_uv.y() && local_uv.y() <= ROW -1)
- {
- Cube_center_depth[i] = local_point.z();
- for (int j = 0; j < 8; j++)
- {
- local_point = camera_q.inverse() * (Cube_corner[i][j] - camera_p);
- output_corner_dis[i].push_back(local_point.norm());
- if (USE_UNDISTORED_IMG)
- {
- //ROS_INFO("directly project!");
- local_uv.x() = local_point(0) / local_point(2) * FOCAL_LENGTH + COL / 2;
- local_uv.y() = local_point(1) / local_point(2) * FOCAL_LENGTH + ROW / 2;
- }
- else
- {
- //ROS_INFO("camera model project!");
- m_camera->spaceToPlane(local_point, local_uv);
- local_uv.x() = std::min(std::max(-5000.0, local_uv.x()),5000.0);
- local_uv.y() = std::min(std::max(-5000.0, local_uv.y()),5000.0);
- }
- output_Cube[i].push_back(Vector3d(local_uv.x(), local_uv.y(), 1));
- }
- }
- else
- {
- Cube_center_depth[i] = -1;
- }
-
- }
-}
-
-void draw_object(cv::Mat &AR_image)
-{
- for (int i = 0; i < axis_num; i++)
- {
- if(output_Axis[i].empty())
- continue;
- cv::Point2d origin(output_Axis[i][0].x(), output_Axis[i][0].y());
- cv::Point2d axis_x(output_Axis[i][1].x(), output_Axis[i][1].y());
- cv::Point2d axis_y(output_Axis[i][2].x(), output_Axis[i][2].y());
- cv::Point2d axis_z(output_Axis[i][3].x(), output_Axis[i][3].y());
- cv::line(AR_image, origin, axis_x, cv::Scalar(0, 0, 255), 2, 8, 0);
- cv::line(AR_image, origin, axis_y, cv::Scalar(0, 255, 0), 2, 8, 0);
- cv::line(AR_image, origin, axis_z, cv::Scalar(255, 0, 0), 2, 8, 0);
- }
-
- //depth sort big---->small
- int index[cube_num];
- for (int i = 0; i < cube_num; i++)
- {
- index[i] = i;
- //cout << "i " << i << " init depth" << Cube_center_depth[i] << endl;
- }
- for (int i = 0; i < cube_num; i++)
- for (int j = 0; j < cube_num - i - 1; j++)
- {
- if (Cube_center_depth[j] < Cube_center_depth[j + 1])
- {
- double tmp = Cube_center_depth[j];
- Cube_center_depth[j] = Cube_center_depth[j + 1];
- Cube_center_depth[j + 1] = tmp;
- int tmp_index = index[j];
- index[j] = index[j + 1];
- index[j + 1] = tmp_index;
- }
- }
-
- for (int k = 0; k < cube_num; k++)
- {
- int i = index[k];
- //cout << "draw " << i << "depth " << Cube_center_depth[i] << endl;
- if (output_Cube[i].empty())
- continue;
- //draw color
- cv::Point* p = new cv::Point[8];
- p[0] = cv::Point(output_Cube[i][0].x(), output_Cube[i][0].y());
- p[1] = cv::Point(output_Cube[i][1].x(), output_Cube[i][1].y());
- p[2] = cv::Point(output_Cube[i][2].x(), output_Cube[i][2].y());
- p[3] = cv::Point(output_Cube[i][3].x(), output_Cube[i][3].y());
- p[4] = cv::Point(output_Cube[i][4].x(), output_Cube[i][4].y());
- p[5] = cv::Point(output_Cube[i][5].x(), output_Cube[i][5].y());
- p[6] = cv::Point(output_Cube[i][6].x(), output_Cube[i][6].y());
- p[7] = cv::Point(output_Cube[i][7].x(), output_Cube[i][7].y());
-
- int npts[1] = {4};
- float min_depth = 100000;
- int min_index = 5;
- for(int j= 0; j < (int)output_corner_dis[i].size(); j++)
- {
- if(output_corner_dis[i][j] < min_depth)
- {
- min_depth = output_corner_dis[i][j];
- min_index = j;
- }
- }
-
- cv::Point plain[1][4];
- const cv::Point* ppt[1] = {plain[0]};
- //first draw large depth plane
- int point_group[8][12] = {{0,1,5,4, 0,4,6,2, 0,1,3,2},
- {0,1,5,4, 1,5,7,3, 0,1,3,2},
- {2,3,7,6, 0,4,6,2, 0,1,3,2},
- {2,3,7,6, 1,5,7,3, 0,1,3,2},
- {0,1,5,4, 0,4,6,2, 4,5,7,6},
- {0,1,5,4, 1,5,7,3, 4,5,7,6},
- {2,3,7,6, 0,4,6,2, 4,5,7,6},
- {2,3,7,6, 1,5,7,3, 4,5,7,6}};
-
- plain[0][0] = p[point_group[min_index][4]];
- plain[0][1] = p[point_group[min_index][5]];
- plain[0][2] = p[point_group[min_index][6]];
- plain[0][3] = p[point_group[min_index][7]];
- cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 200, 0));
-
- plain[0][0] = p[point_group[min_index][0]];
- plain[0][1] = p[point_group[min_index][1]];
- plain[0][2] = p[point_group[min_index][2]];
- plain[0][3] = p[point_group[min_index][3]];
- cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(200, 0, 0));
-
- if(output_corner_dis[i][point_group[min_index][2]] + output_corner_dis[i][point_group[min_index][3]] >
- output_corner_dis[i][point_group[min_index][5]] + output_corner_dis[i][point_group[min_index][6]])
- {
- plain[0][0] = p[point_group[min_index][4]];
- plain[0][1] = p[point_group[min_index][5]];
- plain[0][2] = p[point_group[min_index][6]];
- plain[0][3] = p[point_group[min_index][7]];
- cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 200, 0));
-
- }
- plain[0][0] = p[point_group[min_index][8]];
- plain[0][1] = p[point_group[min_index][9]];
- plain[0][2] = p[point_group[min_index][10]];
- plain[0][3] = p[point_group[min_index][11]];
- cv::fillPoly(AR_image, ppt, npts, 1, cv::Scalar(0, 0, 200));
- delete p;
- }
-}
-
-void callback(const ImageConstPtr& img_msg, const nav_msgs::Odometry::ConstPtr pose_msg)
-{
- //throw the first few unstable pose
- if(img_cnt < 50)
- {
- img_cnt ++;
- return;
- }
- //ROS_INFO("sync callback!");
- Vector3d camera_p(pose_msg->pose.pose.position.x,
- pose_msg->pose.pose.position.y,
- pose_msg->pose.pose.position.z);
- Quaterniond camera_q(pose_msg->pose.pose.orientation.w,
- pose_msg->pose.pose.orientation.x,
- pose_msg->pose.pose.orientation.y,
- pose_msg->pose.pose.orientation.z);
-
- //test plane
- Vector3d cam_z(0, 0, -1);
- Vector3d w_cam_z = camera_q * cam_z;
- //cout << "angle " << acos(w_cam_z.dot(Vector3d(0, 0, 1))) * 180.0 / M_PI << endl;
- if (acos(w_cam_z.dot(Vector3d(0, 0, 1))) * 180.0 / M_PI < 90)
- {
- //ROS_WARN(" look down");
- look_ground = 1;
- }
- else
- look_ground = 0;
-
- project_object(camera_p, camera_q);
-
- cv_bridge::CvImageConstPtr ptr;
- if (img_msg->encoding == "8UC1")
- {
- sensor_msgs::Image img;
- img.header = img_msg->header;
- img.height = img_msg->height;
- img.width = img_msg->width;
- img.is_bigendian = img_msg->is_bigendian;
- img.step = img_msg->step;
- img.data = img_msg->data;
- img.encoding = "mono8";
- ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
- }
- else
- ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
-
- //cv_bridge::CvImagePtr bridge_ptr = cv_bridge::toCvCopy(ptr, sensor_msgs::image_encodings::MONO8);
- cv::Mat AR_image;
- AR_image = ptr->image.clone();
- cv::cvtColor(AR_image, AR_image, cv::COLOR_GRAY2RGB);
- draw_object(AR_image);
-
- sensor_msgs::ImagePtr AR_msg = cv_bridge::CvImage(img_msg->header, "bgr8", AR_image).toImageMsg();
- pub_ARimage.publish(AR_msg);
-
-}
-void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg)
-{
- if (!look_ground)
- return;
- int height_range[30];
- double height_sum[30];
- for (int i = 0; i < 30; i++)
- {
- height_range[i] = 0;
- height_sum[i] = 0;
- }
- for (unsigned int i = 0; i < point_msg->points.size(); i++)
- {
- //double x = point_msg->points[i].x;
- //double y = point_msg->points[i].y;
- double z = point_msg->points[i].z;
- int index = (z + 2.0) / 0.1;
- if (0 <= index && index < 30)
- {
- height_range[index]++;
- height_sum[index] += z;
- }
- //cout << "point " << " z " << z << endl;
- }
- int max_num = 0;
- int max_index = -1;
- for (int i = 1; i < 29; i++)
- {
- if (max_num < height_range[i])
- {
- max_num = height_range[i];
- max_index = i;
- }
- }
- if (max_index == -1)
- return;
- int tmp_num = height_range[max_index - 1] + height_range[max_index] + height_range[max_index + 1];
- double new_height = (height_sum[max_index - 1] + height_sum[max_index] + height_sum[max_index + 1]) / tmp_num;
- //ROS_WARN("detect ground plain, height %f", new_height);
- if (tmp_num < (int)point_msg->points.size() / 2)
- {
- //ROS_INFO("points not enough");
- return;
- }
- //update height
- for (int i = 0; i < cube_num; i++)
- {
- Cube_center[i].z() = new_height + box_length / 2.0;
- }
- add_object();
-
-}
-void img_callback(const ImageConstPtr& img_msg)
-{
- if(pose_init)
- {
- img_buf.push(img_msg);
- }
- else
- return;
-}
-void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
-{
- if(!pose_init)
- {
- pose_init = true;
- return;
- }
-
- if (img_buf.empty())
- {
- //ROS_WARN("image coming late");
- return;
- }
-
- while (img_buf.front()->header.stamp < pose_msg->header.stamp && !img_buf.empty())
- {
- img_buf.pop();
- }
-
- if (!img_buf.empty())
- {
- callback(img_buf.front(), pose_msg);
- img_buf.pop();
- }
- //else
- // ROS_WARN("image coming late");
-}
-int main( int argc, char** argv )
-{
- ros::init(argc, argv, "points_and_lines");
- ros::NodeHandle n("~");
- object_pub = n.advertise("AR_object", 10);
- n.getParam("use_undistored_img", USE_UNDISTORED_IMG);
- ros::Subscriber sub_img;
- if (USE_UNDISTORED_IMG)
- {
- // the same as image crop
- ROW = 600;
- COL = 480;
- FOCAL_LENGTH = 320.0;
- sub_img = n.subscribe("image_undistored", 100, img_callback);
- }
- else
- {
- ROW = 752;
- COL = 480;
- FOCAL_LENGTH = 460.0;
- sub_img = n.subscribe("image_raw", 100, img_callback);
- }
-
- Axis[0] = Vector3d(0, 1.5, -1.2);
- Axis[1]= Vector3d(-10, 5, 0);
- Axis[2] = Vector3d(3, 3, 3);
- Axis[3] = Vector3d(-2, 2, 0);
- Axis[4]= Vector3d(5, 10, -5);
- Axis[5] = Vector3d(0, 10, -1);
-
- Cube_center[0] = Vector3d(0, 1.5, -1.2 + box_length / 2.0);
- //Cube_center[0] = Vector3d(0, 3, -1.2 + box_length / 2.0);
- Cube_center[1] = Vector3d(4, -2, -1.2 + box_length / 2.0);
- Cube_center[2] = Vector3d(0, -2, -1.2 + box_length / 2.0);
-
- ros::Subscriber pose_img = n.subscribe("camera_pose", 100, pose_callback);
- ros::Subscriber sub_point = n.subscribe("pointcloud", 2000, point_callback);
- image_transport::ImageTransport it(n);
- pub_ARimage = it.advertise("AR_image", 1000);
-
- line_color_r.r = 1.0;
- line_color_r.a = 1.0;
- line_color_g.g = 1.0;
- line_color_g.a = 1.0;
- line_color_b.b = 1.0;
- line_color_b.a = 1.0;
-
- string calib_file;
- n.getParam("calib_file", calib_file);
- ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
- m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
-
- ros::Rate r(100);
- ros::Duration(1).sleep();
- add_object();
- add_object();
- ros::spin();
-}
-
diff --git a/benchmark_publisher/CMakeLists.txt b/benchmark_publisher/CMakeLists.txt
index fabccc1cb..969931bda 100644
--- a/benchmark_publisher/CMakeLists.txt
+++ b/benchmark_publisher/CMakeLists.txt
@@ -1,28 +1,37 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
project(benchmark_publisher)
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -rdynamic")
+if(NOT CMAKE_BUILD_TYPE)
+ set(CMAKE_BUILD_TYPE "Release")
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- tf
- )
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -DEIGEN_DONT_PARALLELIZE")
-catkin_package()
-include_directories(${catkin_INCLUDE_DIRS})
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
-set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
-find_package(Eigen3)
-include_directories(
- ${catkin_INCLUDE_DIRS}
- ${EIGEN3_INCLUDE_DIR}
-)
+find_package(Eigen3 REQUIRED)
add_executable(benchmark_publisher
src/benchmark_publisher_node.cpp
)
-target_link_libraries(benchmark_publisher ${catkin_LIBRARIES})
+ament_target_dependencies(benchmark_publisher
+ rclcpp
+ nav_msgs
+ geometry_msgs
+)
+
+target_include_directories(benchmark_publisher PRIVATE ${EIGEN3_INCLUDE_DIR})
+
+install(TARGETS benchmark_publisher
+ DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
diff --git a/benchmark_publisher/launch/publish.launch b/benchmark_publisher/launch/publish.launch
deleted file mode 100644
index 5e066fa61..000000000
--- a/benchmark_publisher/launch/publish.launch
+++ /dev/null
@@ -1,13 +0,0 @@
-
-
-
-
-
-
-
-
-
-
diff --git a/benchmark_publisher/package.xml b/benchmark_publisher/package.xml
index 6a33c48f0..2a3866ae0 100644
--- a/benchmark_publisher/package.xml
+++ b/benchmark_publisher/package.xml
@@ -1,52 +1,19 @@
-
+
benchmark_publisher
0.0.0
The benchmark_publisher package
-
-
-
dvorak
-
-
-
-
-
TODO
+ ament_cmake
-
-
-
-
-
-
-
-
-
-
+ rclcpp
+ nav_msgs
+ geometry_msgs
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- roscpp
- roscpp
-
-
-
-
-
+ ament_cmake
\ No newline at end of file
diff --git a/benchmark_publisher/src/benchmark_publisher_node.cpp b/benchmark_publisher/src/benchmark_publisher_node.cpp
index c405c87fd..566a7d7bb 100644
--- a/benchmark_publisher/src/benchmark_publisher_node.cpp
+++ b/benchmark_publisher/src/benchmark_publisher_node.cpp
@@ -1,10 +1,11 @@
#include
#include
-#include
-#include
-#include
-#include
-#include
+#include
+#include
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
#include
#include
@@ -14,21 +15,6 @@ using namespace Eigen;
const int SKIP = 50;
string benchmark_output_path;
string estimate_output_path;
-template
-T readParam(ros::NodeHandle &n, std::string name)
-{
- T ans;
- if (n.getParam(name, ans))
- {
- ROS_INFO_STREAM("Loaded " << name << ": " << ans);
- }
- else
- {
- ROS_ERROR_STREAM("Failed to load " << name);
- n.shutdown();
- }
- return ans;
-}
struct Data
{
@@ -54,22 +40,21 @@ struct Data
int idx = 1;
vector benchmark;
-ros::Publisher pub_odom;
-ros::Publisher pub_path;
-nav_msgs::Path path;
+rclcpp::Publisher::SharedPtr pub_odom;
+rclcpp::Publisher::SharedPtr pub_path;
+nav_msgs::msg::Path path;
int init = 0;
Quaterniond baseRgt;
Vector3d baseTgt;
-tf::Transform trans;
-void odom_callback(const nav_msgs::OdometryConstPtr &odom_msg)
+void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg)
{
- //ROS_INFO("odom callback!");
- if (odom_msg->header.stamp.toSec() > benchmark.back().t)
+ double stamp = rclcpp::Time(odom_msg->header.stamp).seconds();
+ if (stamp > benchmark.back().t)
return;
-
- for (; idx < static_cast(benchmark.size()) && benchmark[idx].t <= odom_msg->header.stamp.toSec(); idx++)
+
+ for (; idx < static_cast(benchmark.size()) && benchmark[idx].t <= stamp; idx++)
;
@@ -90,8 +75,12 @@ void odom_callback(const nav_msgs::OdometryConstPtr &odom_msg)
return;
}
- nav_msgs::Odometry odometry;
- odometry.header.stamp = ros::Time(benchmark[idx - 1].t);
+ nav_msgs::msg::Odometry odometry;
+ builtin_interfaces::msg::Time stamp_msg;
+ int64_t ns = static_cast(benchmark[idx - 1].t * 1e9);
+ stamp_msg.sec = static_cast(ns / 1000000000LL);
+ stamp_msg.nanosec = static_cast(ns % 1000000000LL);
+ odometry.header.stamp = stamp_msg;
odometry.header.frame_id = "world";
odometry.child_frame_id = "world";
@@ -115,46 +104,50 @@ void odom_callback(const nav_msgs::OdometryConstPtr &odom_msg)
odometry.twist.twist.linear.x = tmp_V.x();
odometry.twist.twist.linear.y = tmp_V.y();
odometry.twist.twist.linear.z = tmp_V.z();
- pub_odom.publish(odometry);
+ pub_odom->publish(odometry);
- geometry_msgs::PoseStamped pose_stamped;
+ geometry_msgs::msg::PoseStamped pose_stamped;
pose_stamped.header = odometry.header;
pose_stamped.pose = odometry.pose.pose;
path.header = odometry.header;
path.poses.push_back(pose_stamped);
- pub_path.publish(path);
+ pub_path->publish(path);
}
int main(int argc, char **argv)
{
- ros::init(argc, argv, "benchmark_publisher");
- ros::NodeHandle n("~");
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared("benchmark_publisher");
+
+ std::string csv_file;
+ node->declare_parameter("data_name", "");
+ node->get_parameter("data_name", csv_file);
- string csv_file = readParam(n, "data_name");
std::cout << "load ground truth " << csv_file << std::endl;
FILE *f = fopen(csv_file.c_str(), "r");
- if (f==NULL)
+ if (f == NULL)
{
- ROS_WARN("can't load ground truth; wrong path");
- //std::cerr << "can't load ground truth; wrong path " << csv_file << std::endl;
- return 0;
+ RCLCPP_WARN(node->get_logger(), "can't load ground truth; wrong path");
+ rclcpp::shutdown();
+ return 0;
}
char tmp[10000];
if (fgets(tmp, 10000, f) == NULL)
{
- ROS_WARN("can't load ground truth; no data available");
+ RCLCPP_WARN(node->get_logger(), "can't load ground truth; no data available");
}
while (!feof(f))
benchmark.emplace_back(f);
fclose(f);
benchmark.pop_back();
- ROS_INFO("Data loaded: %d", (int)benchmark.size());
+ RCLCPP_INFO(node->get_logger(), "Data loaded: %d", (int)benchmark.size());
+
+ pub_odom = node->create_publisher("odometry", 1000);
+ pub_path = node->create_publisher("path", 1000);
- pub_odom = n.advertise("odometry", 1000);
- pub_path = n.advertise("path", 1000);
+ auto sub_odom = node->create_subscription(
+ "estimated_odometry", 1000, odom_callback);
- ros::Subscriber sub_odom = n.subscribe("estimated_odometry", 1000, odom_callback);
-
- ros::Rate r(20);
- ros::spin();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
}
diff --git a/camera_imu_calibration.py b/camera_imu_calibration.py
new file mode 100644
index 000000000..a552df3c0
--- /dev/null
+++ b/camera_imu_calibration.py
@@ -0,0 +1,241 @@
+#!/usr/bin/env python3
+"""
+Интерактивный инструмент для определения матрицы вращения камеры-IMU.
+
+Использование:
+1. Запустите этот скрипт
+2. Следуйте инструкциям и отвечайте на вопросы о расположении осей
+3. Получите матрицу вращения для конфига
+"""
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+import sys
+
+def print_section(title):
+ print("\n" + "="*60)
+ print(f" {title}")
+ print("="*60)
+
+def get_axis_direction(sensor_name, axis_name):
+ """Получить направление оси от пользователя"""
+ directions = {
+ 'x': 'X',
+ 'y': 'Y',
+ 'z': 'Z',
+ '+x': '+X (forward)',
+ '-x': '-X (backward)',
+ '+y': '+Y (right)',
+ '-y': '-Y (left)',
+ '+z': '+Z (up)',
+ '-z': '-Z (down)'
+ }
+
+ print(f"\n{sensor_name} - Ось {axis_name}:")
+ print(" Введите направление этой оси:")
+ print(" +x = forward, -x = backward")
+ print(" +y = right, -y = left")
+ print(" +z = up, -z = down")
+ print(" (пример: +z, -y, +x)")
+
+ while True:
+ response = input(f" {sensor_name} {axis_name} направлена в: ").strip().lower()
+ if response in directions:
+ return response
+ print(" ❌ Некорректный ввод. Попробуйте снова.")
+
+def direction_to_vector(direction):
+ """Преобразовать направление в вектор"""
+ mapping = {
+ '+x': np.array([1, 0, 0]),
+ '-x': np.array([-1, 0, 0]),
+ '+y': np.array([0, 1, 0]),
+ '-y': np.array([0, -1, 0]),
+ '+z': np.array([0, 0, 1]),
+ '-z': np.array([0, 0, -1]),
+ }
+ return mapping[direction]
+
+def get_imu_axes():
+ """Получить оси IMU"""
+ print_section("КОНФИГУРАЦИЯ IMU")
+ print("\nОпределите направление осей IMU в пространстве.")
+ print("(Как правило X-forward, Y-left/right, Z-up)")
+
+ imu_x = get_axis_direction("IMU", "X")
+ imu_y = get_axis_direction("IMU", "Y")
+ imu_z = get_axis_direction("IMU", "Z")
+
+ return imu_x, imu_y, imu_z
+
+def get_camera_axes():
+ """Получить оси камеры"""
+ print_section("КОНФИГУРАЦИЯ КАМЕРЫ")
+ print("\nОпределите направление осей камеры в пространстве.")
+ print("(Стандартная ориентация камеры: Z-forward, X-right, Y-down)")
+
+ cam_x = get_axis_direction("КАМЕРА", "X")
+ cam_y = get_axis_direction("КАМЕРА", "Y")
+ cam_z = get_axis_direction("КАМЕРА", "Z")
+
+ return cam_x, cam_y, cam_z
+
+def verify_orthogonal(v1, v2, v3, name):
+ """Проверить ортогональность векторов"""
+ # Проверить что все векторы разные
+ if np.allclose(np.abs(v1), np.abs(v2)) or np.allclose(np.abs(v1), np.abs(v3)) or np.allclose(np.abs(v2), np.abs(v3)):
+ print(f"\n❌ ОШИБКА в {name}: Две оси указывают на одно направление!")
+ return False
+
+ # Проверить ортогональность (скалярное произведение должно быть 0)
+ dot12 = np.dot(v1, v2)
+ dot13 = np.dot(v1, v3)
+ dot23 = np.dot(v2, v3)
+
+ tol = 1e-6
+ if abs(dot12) > tol or abs(dot13) > tol or abs(dot23) > tol:
+ print(f"\n❌ ОШИБКА в {name}: Оси не ортогональны!")
+ return False
+
+ return True
+
+def vectors_to_rotation_matrix(cam_x_dir, cam_y_dir, cam_z_dir, imu_x_dir, imu_y_dir, imu_z_dir):
+ """
+ Вычислить матрицу вращения от камеры к IMU.
+
+ Входные данные:
+ - cam_x_dir, cam_y_dir, cam_z_dir: направления осей камеры в мировых координатах
+ - imu_x_dir, imu_y_dir, imu_z_dir: направления осей IMU в мировых координатах
+
+ Выход:
+ - R: матрица вращения такая что imu_point = R * camera_point
+ """
+
+ # Матрица, где столбцы - это базис камеры в мировых координатах
+ camera_basis = np.column_stack([cam_x_dir, cam_y_dir, cam_z_dir])
+
+ # Матрица, где столбцы - это базис IMU в мировых координатах
+ imu_basis = np.column_stack([imu_x_dir, imu_y_dir, imu_z_dir])
+
+ # R преобразует вектор из камеры в IMU
+ # мир = camera_basis @ cam_coords
+ # мир = imu_basis @ imu_coords
+ # Поэтому: imu_basis @ imu_coords = camera_basis @ cam_coords
+ # imu_coords = (imu_basis^-1 @ camera_basis) @ cam_coords
+ # R = imu_basis^-1 @ camera_basis
+
+ R = np.linalg.inv(imu_basis) @ camera_basis
+
+ return R
+
+def print_matrix_in_yaml(R):
+ """Вывести матрицу в формате YAML для конфига"""
+ print_section("РЕЗУЛЬТАТ: МАТРИЦА ВРАЩЕНИЯ")
+ print("\nКопируйте эту матрицу в config/usb_cam_config.yaml:")
+ print("\nextrinsicRotation: !!opencv-matrix")
+ print(" rows: 3")
+ print(" cols: 3")
+ print(" dt: d")
+ print(" data: [", end="")
+
+ # Вывести в одну строку для копирования
+ flat = R.flatten()
+ for i, val in enumerate(flat):
+ # Округлить до 6 знаков после запятой
+ print(f"{val:.15f}", end="")
+ if i < len(flat) - 1:
+ print(", ", end="")
+ print("]")
+
+ print("\nИли в развёрнутом виде:")
+ print("extrinsicRotation: !!opencv-matrix")
+ print(" rows: 3")
+ print(" cols: 3")
+ print(" dt: d")
+ print(" data: [" + f"{R[0,0]:.6f}, {R[0,1]:.6f}, {R[0,2]:.6f},")
+ print(f" {R[1,0]:.6f}, {R[1,1]:.6f}, {R[1,2]:.6f},")
+ print(f" {R[2,0]:.6f}, {R[2,1]:.6f}, {R[2,2]:.6f}]")
+
+def print_verification(cam_x, cam_y, cam_z, imu_x, imu_y, imu_z, R):
+ """Напечатать проверку преобразований"""
+ print_section("ПРОВЕРКА ПРЕОБРАЗОВАНИЙ")
+
+ print("\nОси КАМЕРЫ в МИРОВЫХ координатах:")
+ print(f" X: {cam_x} → {direction_to_vector(cam_x)}")
+ print(f" Y: {cam_y} → {direction_to_vector(cam_y)}")
+ print(f" Z: {cam_z} → {direction_to_vector(cam_z)}")
+
+ print("\nОси IMU в МИРОВЫХ координатах:")
+ print(f" X: {imu_x} → {direction_to_vector(imu_x)}")
+ print(f" Y: {imu_y} → {direction_to_vector(imu_y)}")
+ print(f" Z: {imu_z} → {direction_to_vector(imu_z)}")
+
+ print("\nМатрица вращения imu^R_cam:")
+ print(R)
+
+ print("\nПроверка: преобразование осей камеры через R:")
+ cam_x_vec = direction_to_vector(cam_x)
+ cam_y_vec = direction_to_vector(cam_y)
+ cam_z_vec = direction_to_vector(cam_z)
+
+ result_x = R @ cam_x_vec
+ result_y = R @ cam_y_vec
+ result_z = R @ cam_z_vec
+
+ imu_x_vec = direction_to_vector(imu_x)
+ imu_y_vec = direction_to_vector(imu_y)
+ imu_z_vec = direction_to_vector(imu_z)
+
+ print(f"\n R @ cam_X = {result_x} (должно быть {imu_x_vec} - IMU X)")
+ print(f" R @ cam_Y = {result_y} (должно быть {imu_y_vec} - IMU Y)")
+ print(f" R @ cam_Z = {result_z} (должно быть {imu_z_vec} - IMU Z)")
+
+ # Проверить корректность
+ if (np.allclose(result_x, imu_x_vec) and
+ np.allclose(result_y, imu_y_vec) and
+ np.allclose(result_z, imu_z_vec)):
+ print("\n✅ Матрица ВЕРНА! Оси правильно преобразуются.")
+ else:
+ print("\n❌ Что-то не так. Проверьте введённые данные.")
+
+def main():
+ print_section("КАЛИБРОВКА МАТРИЦЫ ВРАЩЕНИЯ КАМЕРЫ-IMU")
+ print("\nЭтот инструмент поможет вам определить правильную матрицу вращения")
+ print("между камерой и IMU на основе фактического расположения осей.")
+
+ # Получить оси IMU
+ imu_x, imu_y, imu_z = get_imu_axes()
+
+ # Получить оси камеры
+ cam_x, cam_y, cam_z = get_camera_axes()
+
+ # Преобразовать в векторы
+ cam_x_vec = direction_to_vector(cam_x)
+ cam_y_vec = direction_to_vector(cam_y)
+ cam_z_vec = direction_to_vector(cam_z)
+
+ imu_x_vec = direction_to_vector(imu_x)
+ imu_y_vec = direction_to_vector(imu_y)
+ imu_z_vec = direction_to_vector(imu_z)
+
+ # Проверить ортогональность
+ if not verify_orthogonal(cam_x_vec, cam_y_vec, cam_z_vec, "КАМЕРА"):
+ sys.exit(1)
+ if not verify_orthogonal(imu_x_vec, imu_y_vec, imu_z_vec, "IMU"):
+ sys.exit(1)
+
+ # Вычислить матрицу вращения
+ R = vectors_to_rotation_matrix(cam_x_vec, cam_y_vec, cam_z_vec,
+ imu_x_vec, imu_y_vec, imu_z_vec)
+
+ # Вывести результаты
+ print_verification(cam_x, cam_y, cam_z, imu_x, imu_y, imu_z, R)
+ print_matrix_in_yaml(R)
+
+ print_section("ДАЛЕЕ")
+ print("\n1. Скопируйте матрицу выше в файл config/usb_cam_config.yaml")
+ print("2. Пересоберите: cd ~/catkin_ws && catkin_make")
+ print("3. Перезапустите VINS: roslaunch vins_estimator usb_cam.launch")
+
+if __name__ == "__main__":
+ main()
diff --git a/camera_model/CMakeLists.txt b/camera_model/CMakeLists.txt
index 95a0c5b3d..c1d633790 100644
--- a/camera_model/CMakeLists.txt
+++ b/camera_model/CMakeLists.txt
@@ -1,41 +1,27 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
project(camera_model)
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC")
+if(NOT CMAKE_BUILD_TYPE)
+ set(CMAKE_BUILD_TYPE "Release")
+endif()
-find_package(catkin REQUIRED COMPONENTS
- roscpp
- std_msgs
- )
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
-find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
-include_directories(${Boost_INCLUDE_DIRS})
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC -DNDEBUG")
+find_package(ament_cmake REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
find_package(OpenCV REQUIRED)
-
-# set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3")
find_package(Ceres REQUIRED)
-include_directories(${CERES_INCLUDE_DIRS})
-
-
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES camera_model
- CATKIN_DEPENDS roscpp std_msgs
-# DEPENDS system_lib
- )
-
-include_directories(
- ${catkin_INCLUDE_DIRS}
- )
+include_directories(${Boost_INCLUDE_DIRS})
+include_directories(${CERES_INCLUDE_DIRS})
include_directories("include")
-
-add_executable(Calibration
- src/intrinsic_calib.cc
+add_library(camera_model SHARED
src/chessboard/Chessboard.cc
src/calib/CameraCalibration.cc
src/camera_models/Camera.cc
@@ -49,7 +35,10 @@ add_executable(Calibration
src/gpl/gpl.cc
src/gpl/EigenQuaternionParameterization.cc)
-add_library(camera_model
+target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
+
+add_executable(Calibration
+ src/intrinsic_calib.cc
src/chessboard/Chessboard.cc
src/calib/CameraCalibration.cc
src/camera_models/Camera.cc
@@ -63,5 +52,27 @@ add_library(camera_model
src/gpl/gpl.cc
src/gpl/EigenQuaternionParameterization.cc)
-target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
-target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
+target_link_libraries(Calibration ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES} camera_model)
+
+# Export library for downstream packages
+ament_export_targets(export_camera_model HAS_LIBRARY_TARGET)
+ament_export_dependencies(OpenCV Ceres)
+ament_export_include_directories(include)
+
+# Ensure Boost targets are available for downstream packages
+find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
+ament_export_dependencies(Boost)
+
+install(TARGETS camera_model
+ EXPORT export_camera_model
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin)
+
+install(DIRECTORY include/
+ DESTINATION include)
+
+install(TARGETS Calibration
+ DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
diff --git a/camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h b/camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h
index 3d08f0ea0..22e75abc8 100644
--- a/camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h
+++ b/camera_model/include/camodocal/gpl/EigenQuaternionParameterization.h
@@ -1,7 +1,7 @@
#ifndef EIGENQUATERNIONPARAMETERIZATION_H
#define EIGENQUATERNIONPARAMETERIZATION_H
-#include "ceres/local_parameterization.h"
+#include "camodocal/gpl/ceres_local_parameterization_compat.h"
namespace camodocal
{
diff --git a/camera_model/include/camodocal/gpl/ceres_local_parameterization_compat.h b/camera_model/include/camodocal/gpl/ceres_local_parameterization_compat.h
new file mode 100644
index 000000000..45f195fb1
--- /dev/null
+++ b/camera_model/include/camodocal/gpl/ceres_local_parameterization_compat.h
@@ -0,0 +1,70 @@
+#ifndef CERES_LOCAL_PARAMETERIZATION_COMPAT_H
+#define CERES_LOCAL_PARAMETERIZATION_COMPAT_H
+
+#include
+#include
+
+namespace ceres {
+
+// Compatibility wrapper: Ceres 2.2 removed LocalParameterization in favor of Manifold.
+// This shim provides the old LocalParameterization API on top of Manifold,
+// so existing code can be compiled with minimal changes.
+//
+// Usage: inherit from LocalParameterization and implement:
+// bool Plus(const double* x, const double* delta, double* x_plus_delta) const
+// bool ComputeJacobian(const double* x, double* jacobian) const
+// int GlobalSize() const
+// int LocalSize() const
+class LocalParameterization : public Manifold {
+public:
+ virtual ~LocalParameterization() {}
+
+ // Old API — user implements these
+ virtual bool Plus(const double* x, const double* delta,
+ double* x_plus_delta) const = 0;
+ virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
+ virtual int GlobalSize() const = 0;
+ virtual int LocalSize() const = 0;
+
+ // Manifold interface — maps old names to new
+ int AmbientSize() const final { return GlobalSize(); }
+ int TangentSize() const final { return LocalSize(); }
+
+ // Manifold::Plus is pure virtual; we provide it via the old Plus
+ // (same signature, so the derived class's Plus satisfies both)
+
+ bool PlusJacobian(const double* x, double* jacobian) const final {
+ return ComputeJacobian(x, jacobian);
+ }
+
+ bool Minus(const double* y, const double* x, double* y_minus_x) const final {
+ int n = LocalSize();
+ for (int i = 0; i < n; ++i) {
+ y_minus_x[i] = y[i] - x[i];
+ }
+ return true;
+ }
+
+ bool MinusJacobian(const double* x, double* jacobian) const final {
+ int n = LocalSize();
+ int m = GlobalSize();
+ for (int i = 0; i < n * m; ++i) jacobian[i] = 0.0;
+ for (int i = 0; i < n && i < m; ++i) {
+ jacobian[i * m + i] = 1.0;
+ }
+ return true;
+ }
+};
+
+// Compatibility for AutoDiffLocalParameterization
+template
+class AutoDiffLocalParameterization {
+public:
+ static LocalParameterization* Create() {
+ return new AutoDiffManifold();
+ }
+};
+
+} // namespace ceres
+
+#endif // CERES_LOCAL_PARAMETERIZATION_COMPAT_H
\ No newline at end of file
diff --git a/camera_model/package.xml b/camera_model/package.xml
index 511c997f6..72cb40f88 100644
--- a/camera_model/package.xml
+++ b/camera_model/package.xml
@@ -1,54 +1,23 @@
-
+
camera_model
0.0.0
The camera_model package
-
-
-
dvorak
-
-
-
-
-
TODO
+ ament_cmake
-
-
-
-
-
-
-
-
-
-
+ boost
+ libopencv-dev
+ libceres-dev
+ boost
+ libopencv-dev
+ libceres-dev
-
-
-
-
-
-
-
-
-
-
-
- catkin
- roscpp
- std_msgs
- roscpp
- std_msgs
-
-
-
-
-
+ ament_cmake
\ No newline at end of file
diff --git a/camera_model/src/calib/CameraCalibration.cc b/camera_model/src/calib/CameraCalibration.cc
index 682fe07a1..aa717834c 100644
--- a/camera_model/src/calib/CameraCalibration.cc
+++ b/camera_model/src/calib/CameraCalibration.cc
@@ -507,8 +507,8 @@ CameraCalibration::optimize(CameraPtr& camera,
ceres::LocalParameterization* quaternionParameterization =
new EigenQuaternionParameterization;
- problem.SetParameterization(transformVec.at(i).rotationData(),
- quaternionParameterization);
+ problem.SetManifold(transformVec.at(i).rotationData(),
+ quaternionParameterization);
}
std::cout << "begin ceres" << std::endl;
diff --git a/config/3dm/3dm_config.yaml b/config/3dm/3dm_config.yaml
deleted file mode 100644
index 8677bb261..000000000
--- a/config/3dm/3dm_config.yaml
+++ /dev/null
@@ -1,86 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/imu_3dm_gx4/imu"
-image_topic: "/mv_25001498/image_raw"
-output_path: "/home/tony-ws1/output/"
-
-# MEI is better than PINHOLE for large FOV camera
-model_type: MEI
-camera_name: camera
-image_width: 752
-image_height: 480
-mirror_parameters:
- xi: 2.057e+00
-distortion_parameters:
- k1: 7.145e-02
- k2: 5.059e-01
- p1: 4.727e-05
- p2: -5.492e-04
-projection_parameters:
- gamma1: 1.115e+03
- gamma2: 1.114e+03
- u0: 3.672e+02
- v0: 2.385e+02
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [ 0, 0, -1,
- -1, 0, 0,
- 0, 1, 0]
-
-#Translation from camera frame to imu frame, imu^T_cam
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [ 0, 0, 0.02]
-
-#feature traker paprameters
-
-max_cnt: 150 # max feature number in feature tracking
-min_dist: 20 # min distance between two features
-freq: 20 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 0 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-#optimization parameters
-
-max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 10 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.2 # accelerometer measurement noise standard deviation.
-gyr_n: 0.05 # gyroscope measurement noise standard deviation.
-acc_w: 0.002 # accelerometer bias random work noise standard deviation.
-gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation.
-g_norm: 9.805 #
-
-#loop closure parameters
-loop_closure: 0 # start loop closure
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-fast_relocalization: 0 # useful in real-time and large project
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 0 # online estimate time offset between camera and imu
-td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
\ No newline at end of file
diff --git a/config/AR_demo.rviz b/config/AR_demo.rviz
deleted file mode 100644
index 12c630e12..000000000
--- a/config/AR_demo.rviz
+++ /dev/null
@@ -1,235 +0,0 @@
-Panels:
- - Class: rviz/Displays
- Help Height: 0
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- - /AR_image1/Status1
- - /raw image1/Status1
- - /Path2/Status1
- Splitter Ratio: 0.465116
- Tree Height: 729
- - Class: rviz/Selection
- Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.588679
- - Class: rviz/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: PointCloud
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz/Grid
- Color: 130; 130; 130
- Enabled: true
- Line Style:
- Line Width: 0.03
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: -1.2
- Plane: XY
- Plane Cell Count: 10
- Reference Frame:
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 0; 170; 0
- Enabled: true
- Head Diameter: 0.3
- Head Length: 0.2
- Length: 0.3
- Line Style: Lines
- Line Width: 0.01
- Name: Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Style: None
- Radius: 0.03
- Shaft Diameter: 0.1
- Shaft Length: 0.1
- Topic: /vins_estimator/path
- Unreliable: false
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: false
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/PointCloud
- Color: 255; 255; 255
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: true
- Max Color: 0; 0; 0
- Max Intensity: 4096
- Min Color: 0; 0; 0
- Min Intensity: 0
- Name: PointCloud
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 1
- Size (m): 0.5
- Style: Points
- Topic: /vins_estimator/point_cloud
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: false
- Value: true
- - Class: rviz/Image
- Enabled: true
- Image Topic: /ar_demo_node/AR_image
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: AR_image
- Normalize Range: true
- Queue Size: 2
- Transport Hint: raw
- Unreliable: false
- Value: true
- - Class: rviz/Image
- Enabled: true
- Image Topic: /mv_25001498/image_raw
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: raw image
- Normalize Range: true
- Queue Size: 2
- Transport Hint: raw
- Unreliable: false
- Value: true
- - Class: rviz/MarkerArray
- Enabled: true
- Marker Topic: /vins_estimator/camera_pose_visual
- Name: MarkerArray
- Namespaces:
- CameraPoseVisualization: true
- Queue Size: 100
- Value: true
- - Class: rviz/MarkerArray
- Enabled: true
- Marker Topic: /ar_demo_node/AR_object
- Name: MarkerArray
- Namespaces:
- basic_shapes: true
- Queue Size: 100
- Value: true
- - Class: rviz/Axes
- Enabled: true
- Length: 1
- Name: Axes
- Radius: 0.1
- Reference Frame:
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 255; 85; 255
- Enabled: true
- Head Diameter: 0.3
- Head Length: 0.2
- Length: 0.3
- Line Style: Billboards
- Line Width: 0.3
- Name: Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Style: None
- Radius: 0.03
- Shaft Diameter: 0.1
- Shaft Length: 0.1
- Topic: /odom_translation/vins
- Unreliable: false
- Value: true
- Enabled: true
- Global Options:
- Background Color: 0; 0; 0
- Fixed Frame: world
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/SetInitialPose
- Topic: /initialpose
- - Class: rviz/SetGoal
- Topic: /move_base_simple/goal
- - Class: rviz/PublishPoint
- Single click: true
- Topic: /clicked_point
- Value: true
- Views:
- Current:
- Class: rviz/Orbit
- Distance: 33.293
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.06
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: -3.42082
- Y: -4.08465
- Z: -8.071
- Name: Current View
- Near Clip Distance: 0.01
- Pitch: 1.5698
- Target Frame:
- Value: Orbit (rviz)
- Yaw: 0.0571614
- Saved: ~
-Window Geometry:
- AR_image:
- collapsed: false
- Displays:
- collapsed: true
- Height: 959
- Hide Left Dock: false
- Hide Right Dock: true
- QMainWindow State: 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
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: true
- Width: 1572
- X: 286
- Y: 43
- raw image:
- collapsed: false
diff --git a/config/black_box/black_box_config.yaml b/config/black_box/black_box_config.yaml
deleted file mode 100644
index f834011b5..000000000
--- a/config/black_box/black_box_config.yaml
+++ /dev/null
@@ -1,89 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/djiros/imu"
-image_topic: "/djiros/image"
-output_path: "/home/urop/output/" # vins outputs will be wrttento vins_folder_path + output_path
-
-#camera calibration
-model_type: MEI
-camera_name: camera
-image_width: 752
-image_height: 480
-mirror_parameters:
- xi: 2.2134257311108083e+00
-distortion_parameters:
- k1: 1.4213768437132895e-01
- k2: 9.1226950620748259e-01
- p1: 1.2056297779277966e-03
- p2: 2.0300076091651340e-03
-projection_parameters:
- gamma1: 1.1659242643040975e+03
- gamma2: 1.1656143723709608e+03
- u0: 3.9238492754088008e+02
- v0: 2.4392485271819217e+02
-
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [ 3.5547377966504534e-02, -9.9819308302994181e-01,
- -4.8445360055281682e-02, 9.9855985185796758e-01,
- 3.3527772724371241e-02, 4.1882104932016398e-02,
- -4.0182162424389177e-02, -4.9864390574059170e-02,
- 9.9794736152543517e-01 ]
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [ 6.5272135116678912e-02, -6.8544177447541876e-02,
- 3.7304589197375740e-02 ]
-
-
-#feature traker paprameters
-
-max_cnt: 120 # max feature number in feature tracking
-min_dist: 30 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 1 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.2 # accelerometer measurement noise standard deviation.
-gyr_n: 0.05 # gyroscope measurement noise standard deviation.
-acc_w: 0.002 # accelerometer bias random work noise standard deviation.
-gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation.
-g_norm: 9.805 #
-
-#loop closure parameters
-loop_closure: 1 # start loop closure
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-fast_relocalization: 1 # useful in real-time and large project
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 1 # online estimate time offset between camera and imu
-td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 1 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
diff --git a/config/cla/cla_config.yaml b/config/cla/cla_config.yaml
deleted file mode 100644
index 7c0818da6..000000000
--- a/config/cla/cla_config.yaml
+++ /dev/null
@@ -1,81 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/imu0"
-image_topic: "/cam0/image_raw"
-output_path: "/home/tony-ws1/output/"
-
-#camera calibration
-model_type: KANNALA_BRANDT
-camera_name: camera
-image_width: 752
-image_height: 480
-projection_parameters:
- k2: -0.005740195474458931
- k3: 0.02878252863739417
- k4: -0.04010621197185408
- k5: 0.02008469575876223
- mu: 472.2863830700696
- mv: 470.83759684346785
- u0: 368.8316828103749
- v0: 232.23688706965652
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [ 0.99992185, -0.01241689, -0.00145542,
- 0.01242827, 0.99989004, 0.00809021,
- 0.0013548, -0.00810766, 0.99996621 ]
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [ 0.03661314, -0.01027102, -0.00654466 ]
-
-#feature traker paprameters
-max_cnt: 150 # max feature number in feature tracking
-min_dist: 30 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 1 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 4e-2 # accelerometer measurement noise standard deviation. #0.2 4e-2
-gyr_n: 1e-3 # gyroscope measurement noise standard deviation. #0.05 1e-3
-acc_w: 1e-3 # accelerometer bias random work noise standard deviation. #0.02 4e-2 4e-3 1e-2
-gyr_w: 1e-4 # gyroscope bias random work noise standard deviation. #4.0e-5 1e-3 1e-4 1e-4
-g_norm: 9.81 # gravity magnitude
-
-
-#loop closure parameters
-loop_closure: 1 # start loop closure
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-fast_relocalization: 0 # useful in real-time and large project
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 0 # online estimate time offset between camera and imu
-td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
\ No newline at end of file
diff --git a/config/euroc/euroc_config.yaml b/config/euroc/euroc_config.yaml
deleted file mode 100644
index 97cd8d160..000000000
--- a/config/euroc/euroc_config.yaml
+++ /dev/null
@@ -1,82 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/imu0"
-image_topic: "/cam0/image_raw"
-output_path: "/home/shaozu/output/"
-
-#camera calibration
-model_type: PINHOLE
-camera_name: camera
-image_width: 752
-image_height: 480
-distortion_parameters:
- k1: -2.917e-01
- k2: 8.228e-02
- p1: 5.333e-05
- p2: -1.578e-04
-projection_parameters:
- fx: 4.616e+02
- fy: 4.603e+02
- cx: 3.630e+02
- cy: 2.481e+02
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [0.0148655429818, -0.999880929698, 0.00414029679422,
- 0.999557249008, 0.0149672133247, 0.025715529948,
- -0.0257744366974, 0.00375618835797, 0.999660727178]
-#Translation from camera frame to imu frame, imu^T_cam
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [-0.0216401454975,-0.064676986768, 0.00981073058949]
-
-#feature traker paprameters
-max_cnt: 150 # max feature number in feature tracking
-min_dist: 30 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 1 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04
-gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
-acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02
-gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5
-g_norm: 9.81007 # gravity magnitude
-
-#loop closure parameters
-loop_closure: 1 # start loop closure
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-fast_relocalization: 0 # useful in real-time and large project
-pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 0 # online estimate time offset between camera and imu
-td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
diff --git a/config/euroc/euroc_config_no_extrinsic.yaml b/config/euroc/euroc_config_no_extrinsic.yaml
deleted file mode 100644
index 14887eee0..000000000
--- a/config/euroc/euroc_config_no_extrinsic.yaml
+++ /dev/null
@@ -1,68 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/imu0"
-image_topic: "/cam0/image_raw"
-output_path: "/home/tony-ws1/output/"
-
-#camera calibration
-model_type: PINHOLE
-camera_name: camera
-image_width: 752
-image_height: 480
-distortion_parameters:
- k1: -2.917e-01
- k2: 8.228e-02
- p1: 5.333e-05
- p2: -1.578e-04
-projection_parameters:
- fx: 4.616e+02
- fy: 4.603e+02
- cx: 3.630e+02
- cy: 2.481e+02
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-
-#feature traker paprameters
-max_cnt: 150 # max feature number in feature tracking
-min_dist: 30 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 1 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2
-gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05
-acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02
-gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
-g_norm: 9.81007 # gravity magnitude
-
-#loop closure parameters
-loop_closure: 1 # start loop closure
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-fast_relocalization: 0 # useful in real-time and large project
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 0 # online estimate time offset between camera and imu
-td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
\ No newline at end of file
diff --git a/config/fisheye_bag/fisheye_bag_config.yaml b/config/fisheye_bag/fisheye_bag_config.yaml
new file mode 100644
index 000000000..396a9a3f3
--- /dev/null
+++ b/config/fisheye_bag/fisheye_bag_config.yaml
@@ -0,0 +1,136 @@
+%YAML:1.0
+
+#common parameters
+imu_topic: "/mavros/imu/data"
+image_topic: "/image_raw"
+image_input_format: "compressed"
+output_path: "/tmp/vins_fisheye_output/"
+world_frame_id: "map" # frame_id for published odometry, path, TF, and visualization
+
+#camera calibration
+# Recalibrated with OpenCV fisheye model (Kannala-Brandt)
+# RMS reprojection error: 0.676, mean view error: 0.389, frames: 130
+model_type: KANNALA_BRANDT
+camera_name: camera
+image_width: 1280
+image_height: 1024
+projection_parameters:
+ k2: -0.024036232663034018 # OpenCV dist_coeffs[0] (k1)
+ k3: -0.002794445348728834 # OpenCV dist_coeffs[1] (k2)
+ k4: 0.0011798140355741464 # OpenCV dist_coeffs[2] (k3)
+ k5: -0.00057002774959145452 # OpenCV dist_coeffs[3] (k4)
+ mu: 472.88439456353871 # fx
+ mv: 472.20758450689351 # fy
+ u0: 615.40978596261721 # cx
+ v0: 518.71597814251959 # cy
+
+# Extrinsic parameter between IMU and Camera.
+estimate_extrinsic: 1
+extrinsicRotation: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ # cam_z -> imu_z, cam_x -> -imu_y, cam_y -> imu_x
+ data: [0., 1., 0.,
+ -1., 0., 0.,
+ 0., 0., 1.]
+extrinsicTranslation: !!opencv-matrix
+ rows: 3
+ cols: 1
+ dt: d
+ data: [0.0, 0.0, 0.0]
+
+
+#feature traker paprameters
+max_cnt: 250 # target number of tracked features for a 1280x1024 frame
+min_dist: 20 # preserve spatial distribution; lower values cluster features
+freq: 25 # leaves CPU headroom while using most of the 30 Hz camera stream
+F_threshold: 2.5 # stricter geometric outlier rejection for a sharp image
+
+use_advanced_flow: 1 # stable LK + Shi-Tomasi path; AGAST path is experimental
+#andvanced flow parameters
+fast_threshold: 20 # used only when use_advanced_flow=1 (AGAST)
+use_bidirectional_flow: 0 # used only by advanced flow; disabled in this stable profile
+
+#basic tracking parameters
+gftt_quality_level: 0.01 # Shi-Tomasi sensitivity; lower detects weaker usable corners
+
+
+show_track: 1 # publish tracking image as topic
+equalize: 1 # if image is too dark or light, trun on equalize to find enough features
+
+fisheye: 1 # apply a custom valid-pixel mask during feature selection
+fisheye_mask: "config/mask.jpg" # relative to the vins_bringup share directory
+
+#autotune filter
+enable_feature_auto_tuning: 0 # adapt active detector to texture and motion blur
+feature_auto_tune_interval: 5.0 # seconds between adaptation/diagnostic reports
+feature_auto_tune_log: 0 # print [AUTO_TUNE] values to ROS console and log
+fast_threshold_min: 7 # safe AGAST tuning range
+fast_threshold_max: 45
+gftt_quality_min: 0.001 # used only when use_advanced_flow=0
+gftt_quality_max: 0.05
+
+
+#adaptive tracking for high-speed navigation
+enable_velocity_check: 0 # enable velocity-based adaptive feature tracking
+max_velocity_threshold: 5.0 # m/s - velocity threshold to trigger feature boost
+velocity_boost_features: 75 # extra features to add when high velocity detected
+min_parallax_threshold: 5.0 # pixels - minimum parallax for quality tracking
+
+#optimization parameters
+max_solver_time: 0.05 # max solver time (s) - reduced for real-time performance
+max_num_iterations: 15 # max solver itrations, to guarantee real time
+keyframe_parallax: 8.0 # keyframe selection threshold (pixel)
+
+
+#imu parameters The more accurate parameters you provide, the better performance
+acc_n: 0.15 # accelerometer measurement noise standard deviation.
+gyr_n: 0.015 # gyroscope measurement noise standard deviation.
+acc_w: 0.003 # accelerometer bias random work noise standard deviation.
+gyr_w: 0.001 # gyroscope bias random work noise standard deviation.
+g_norm: 9.805 #
+
+#loop closure parameters
+loop_closure: 0 # start loop closure (ОТКЛЮЧЕНО - может вызывать скачки при движении)
+load_previous_pose_graph: 0 # load and reuse previous pose graph
+fast_relocalization: 0 # useful in real-time and large project
+pose_graph_save_path: "/tmp/vins_fisheye_output/pose_graph/" # save and load path
+
+#unsynchronization parameters
+estimate_td: 1 # online estimate time offset between camera and imu (ОТКЛЮЧЕНО для стабильности)
+estimate_td_period: 10.0 # seconds between td optimization bursts
+td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
+
+#rolling shutter parameters
+rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera (отключено - улучшает результаты)
+rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet)
+
+#visualization parameters
+save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
+visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
+visualize_camera_size: 0.4 # size of camera marker in RVIZ
+
+#zero velocity update (ZUPT) parameters
+enable_zupt: 0 # enable zero velocity update to prevent drift when stationary
+zupt_vel_threshold: 0.15 # velocity threshold (m/s) for detecting stationary state
+zupt_acc_threshold: 0.25 # acceleration deviation threshold (m/s^2) from gravity
+
+#imu filtering parameters
+enable_imu_acc_filter: 0 # enable accelerometer outlier rejection and smoothing
+imu_acc_max_change: 3.0 # maximum allowed acceleration change (m/s^2) for outlier detection
+imu_acc_filter_alpha: 0.75 # exponential filter coefficient (0.0-1.0, higher = more smoothing)
+
+#altitude scale correction parameters
+enable_altitude_scale_correction: 0 # enable scale correction using MAVLink altitude
+altitude_correction_factor: 0.1 # correction strength (0.0-1.0, lower = gentler correction)
+
+#lidar odometry initialization & scale correction parameters
+# Uses an external lidar odometry (e.g. LIO) to initialize the VINS scale and
+# starting coordinate so that both estimators share the same reference frame.
+enable_lidar_init: 1 # enable lidar-assisted deep init + ongoing scale correction
+lidar_odom_topic: "/lio/odom" # nav_msgs/msg/Odometry source for init/scale reference
+lidar_init_timeout: 10.0 # seconds to wait for lidar odom before giving up
+lidar_init_pos_threshold: 1.0 # min displacement (m) before using lidar for scale init
+lidar_scale_correction_factor: 0.05 # ongoing gentle scale correction strength (0.0-1.0)
+lidar_sync_max_dt: 0.1 # max time diff (s) for lidar-vins sample synchronization
diff --git a/config/fisheye_mask_752x480.jpg b/config/fisheye_mask_752x480.jpg
deleted file mode 100644
index 5caf51f55..000000000
Binary files a/config/fisheye_mask_752x480.jpg and /dev/null differ
diff --git a/config/mask.jpg b/config/mask.jpg
new file mode 100644
index 000000000..b1aafa7a1
Binary files /dev/null and b/config/mask.jpg differ
diff --git a/config/realsense/realsense_color_config.yaml b/config/realsense/realsense_color_config.yaml
deleted file mode 100644
index c1eacb1e2..000000000
--- a/config/realsense/realsense_color_config.yaml
+++ /dev/null
@@ -1,82 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/camera/imu/data_raw"
-image_topic: "/camera/color/image_raw"
-output_path: "/home/tony-ws1/output/"
-
-#camera calibration
-model_type: PINHOLE
-camera_name: camera
-image_width: 640
-image_height: 480
-distortion_parameters:
- k1: 9.2615504465028850e-02
- k2: -1.8082438825995681e-01
- p1: -6.5484100374765971e-04
- p2: -3.5829351558557421e-04
-projection_parameters:
- fx: 6.0970550296798035e+02
- fy: 6.0909579671294716e+02
- cx: 3.1916667152289227e+02
- cy: 2.3558360480225772e+02
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [ 0.99964621, 0.01105994, 0.02418954,
- -0.01088975, 0.9999151, -0.00715601,
- -0.02426663, 0.00689006, 0.99968178]
-#Translation from camera frame to imu frame, imu^T_cam
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [0.07494282, -0.01077138, -0.00641822]
-
-#feature traker paprameters
-max_cnt: 150 # max feature number in feature tracking
-min_dist: 25 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 0 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2
-gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05
-acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02
-gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
-g_norm: 9.805 # gravity magnitude
-
-#loop closure parameters
-loop_closure: 1 # start loop closure
-fast_relocalization: 1 # useful in real-time and large project
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 1 # online estimate time offset between camera and imu
-td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 1 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0.033 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
\ No newline at end of file
diff --git a/config/realsense/realsense_fisheye_config.yaml b/config/realsense/realsense_fisheye_config.yaml
deleted file mode 100644
index f3e1fb584..000000000
--- a/config/realsense/realsense_fisheye_config.yaml
+++ /dev/null
@@ -1,81 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/camera/imu/data_raw"
-image_topic: "/camera/fisheye/image_raw"
-output_path: "/home/tony-ws1/output/"
-
-#camera calibration
-model_type: KANNALA_BRANDT
-camera_name: camera
-image_width: 640
-image_height: 480
-projection_parameters:
- k2: 1.7280355035195181e-02
- k3: -2.5505200860040985e-02
- k4: 2.2621441637715487e-02
- k5: -7.3355871719731113e-03
- mu: 2.7723712054408202e+02
- mv: 2.7699784668734617e+02
- u0: 3.3625356873985868e+02
- v0: 2.3603924727453901e+02
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [0.99992917, 0.00878151, 0.00803387,
- -0.00870674, 0.9999189, -0.0092943,
- -0.00811483, 0.00922369, 0.99992453]
-#Translation from camera frame to imu frame, imu^T_cam
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [0.00188568, 0.00123801, 0.01044055]
-
-#feature traker paprameters
-max_cnt: 120 # max feature number in feature tracking
-min_dist: 30 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 0 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.08 # accelerometer measurement noise standard deviation. #0.2 0.04
-gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
-acc_w: 0.00004 # accelerometer bias random work noise standard deviation. #0.02
-gyr_w: 2.0e-6 # gyroscope bias random work noise standard deviation. #4.0e-5
-g_norm: 9.805 # gravity magnitude
-
-#loop closure parameters
-loop_closure: 1 # start loop closure
-fast_relocalization: 1 # useful in real-time and large project
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 1 # online estimate time offset between camera and imu
-td: 0.010 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
diff --git a/config/simulation/simulation_config.yaml b/config/simulation/simulation_config.yaml
deleted file mode 100644
index 2fce55899..000000000
--- a/config/simulation/simulation_config.yaml
+++ /dev/null
@@ -1,52 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/data_generator/imu"
-image_topic: "/data_generator/image"
-output_path: "/home/shaozu/output/"
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
-# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [0, 0, -1,
- -1, 0, 0,
- 0, 1, 0]
-#Translation from camera frame to imu frame, imu^T_cam
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [0,0.2, 0.6]
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
-gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05 0.004
-acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.02
-gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
-g_norm: 9.81007 # gravity magnitude
-
-#unsynchronization parameters
-estimate_td: 0 # online estimate time offset between camera and imu
-td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
-
diff --git a/config/termal_cam_config.yaml b/config/termal_cam_config.yaml
new file mode 100644
index 000000000..1ae6bc943
--- /dev/null
+++ b/config/termal_cam_config.yaml
@@ -0,0 +1,122 @@
+%YAML:1.0
+
+#common parameters
+imu_topic: "/mavros/imu/data"
+image_topic: "/thermal_cam/image_raw"
+image_input_format: "raw"
+output_path: "/tmp/vins_thermal_output/"
+
+#camera calibration
+model_type: PINHOLE
+camera_name: termal_camera
+image_width: 384
+image_height: 288
+distortion_parameters:
+ k1: -0.41373247158445464
+ k2: 0.16183802909949693
+ p1: -0.0004056243478585802
+ p2: -0.0022150821156080806
+projection_parameters:
+ fx: 382.1655349116403
+ fy: 381.916045874893
+ cx: 177.59980485848902
+ cy: 137.17607935580716
+
+# Extrinsic parameter between IMU and Camera.
+# Полученно из калибровки Kalibr
+#
+# Kalibr выдал T_ci (imu to cam):
+# [[-0.99853052 -0.04402128 -0.03160572]
+# [ 0.03392303 -0.05292855 -0.99802194]
+# [ 0.04226135 -0.99762753 0.0543441 ]]
+#
+# Нам нужна T_ic (cam to imu) = T_ci^T
+estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
+ # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
+ # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
+#If you choose 0 or 1, you should write down the following matrix.
+#Rotation from camera frame to imu frame, imu^R_cam (T_ic)
+extrinsicRotation: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ # data: [-0.99853052, 0.03392303, 0.04226135,
+ # -0.04402128, -0.05292855, -0.99762753,
+ # -0.03160572, -0.99802194, 0.0543441]
+
+ data: [0., 0., 1.,
+ -1., 0., 0.,
+ 0., -1., 0.]
+#Translation from camera frame to imu frame, imu^T_cam
+# Из калибровки Kalibr
+extrinsicTranslation: !!opencv-matrix
+ rows: 3
+ cols: 1
+ dt: d
+ data: [0.00, 0.000, 0.00]
+
+#feature traker paprameters
+max_cnt: 150 # max feature number (reduced for faster processing)
+min_dist: 15 # min distance between features (larger spacing = fewer features)
+freq: 20 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
+F_threshold: 2.0 # ransac threshold (thermal: more tolerance for noise)
+fast_threshold: 20 # threshold for AGAST corner detector (higher = fewer but stronger features)
+gftt_quality_level: 0.01 # Shi-Tomasi sensitivity used by the stable tracker
+use_bidirectional_flow: 0 # used only by advanced flow
+use_advanced_flow: 1 # stable LK + Shi-Tomasi; AGAST mode is experimental
+show_track: 1 # publish tracking image as topic
+equalize: 1 # if image is too dark or light, trun on equalize to find enough features
+fisheye: 0
+
+#adaptive tracking for high-speed navigation
+enable_velocity_check: 0 # enable velocity-based adaptive feature tracking
+max_velocity_threshold: 2.5 # m/s - higher threshold to avoid unnecessary boosting
+velocity_boost_features: 50 # reduced boost for speed
+min_parallax_threshold: 3.0 # pixels - minimum parallax (thermal: higher due to lower resolution)
+
+#optimization parameters
+max_solver_time: 0.03 # max solver time (s) - reduced for real-time performance
+max_num_iterations: 15 # reduced iterations for speed
+keyframe_parallax: 10.0 # higher threshold = fewer keyframes = less computation
+
+
+#imu parameters The more accurate parameters you provide, the better performance
+acc_n: 0.15 # accelerometer measurement noise standard deviation.
+gyr_n: 0.015 # gyroscope measurement noise standard deviation.
+acc_w: 0.003 # accelerometer bias random work noise standard deviation.
+gyr_w: 0.001 # gyroscope bias random work noise standard deviation.
+g_norm: 9.805 #
+
+#loop closure parameters
+loop_closure: 1 # enable loop closure and pose graph
+load_previous_pose_graph: 0 # load and reuse previous pose graph
+fast_relocalization: 0 # useful in real-time and large project
+pose_graph_save_path: "/tmp/vins_thermal_output/pose_graph/" # save and load path
+
+#unsynchronization parameters
+estimate_td: 1 # online estimate time offset between camera and IMU
+estimate_td_period: 7.0 # seconds between td optimization bursts
+td: -0.1 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
+
+#rolling shutter parameters
+rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera (отключено - улучшает результаты)
+rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet)
+
+#visualization parameters
+save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
+visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
+visualize_camera_size: 0.4 # size of camera marker in RVIZ
+
+#zero velocity update (ZUPT) parameters
+enable_zupt: 0 # enable zero velocity update to prevent drift when stationary
+zupt_vel_threshold: 0.15 # velocity threshold (thermal: more sensitive to catch stationary state)
+zupt_acc_threshold: 0.25 # acceleration deviation threshold (thermal: tighter threshold)
+
+#imu filtering parameters
+enable_imu_acc_filter: 0 # enable accelerometer outlier rejection and smoothing
+imu_acc_max_change: 3.0 # maximum allowed acceleration change (thermal: slightly more tolerance)
+imu_acc_filter_alpha: 0.75 # exponential filter coefficient (thermal: slightly less smoothing for responsiveness)
+
+#altitude scale correction parameters
+enable_altitude_scale_correction: 0 # bag has no /mavros/local_position/velocity_local topic
+altitude_correction_factor: 0.1 # correction strength (0.0-1.0, lower = gentler correction)
diff --git a/config/tum/tum_config.yaml b/config/tum/tum_config.yaml
deleted file mode 100644
index 3916c5a37..000000000
--- a/config/tum/tum_config.yaml
+++ /dev/null
@@ -1,83 +0,0 @@
-%YAML:1.0
-
-#common parameters
-imu_topic: "/imu0"
-image_topic: "/cam0/image_raw"
-output_path: "/home/tony-ws1/output/"
-
-#camera calibration
-model_type: KANNALA_BRANDT
-camera_name: camera
-image_width: 512
-image_height: 512
-projection_parameters:
- k2: 0.0034823894022493434
- k3: 0.0007150348452162257
- k4: -0.0020532361418706202
- k5: 0.00020293673591811182
- mu: 190.97847715128717
- mv: 190.9733070521226
- u0: 254.93170605935475
- v0: 256.8974428996504
-
-# Extrinsic parameter between IMU and Camera.
-estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
- # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
- # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
-#If you choose 0 or 1, you should write down the following matrix.
-#Rotation from camera frame to imu frame, imu^R_cam
-extrinsicRotation: !!opencv-matrix
- rows: 3
- cols: 3
- dt: d
- data: [ -9.9951465899298464e-01, 7.5842033363785165e-03,
- -3.0214670573904204e-02, 2.9940114644659861e-02,
- -3.4023430206013172e-02, -9.9897246995704592e-01,
- -8.6044170750674241e-03, -9.9939225835343004e-01,
- 3.3779845322755464e-02 ]
-extrinsicTranslation: !!opencv-matrix
- rows: 3
- cols: 1
- dt: d
- data: [ 4.4511917113940799e-02, -7.3197096234105752e-02,
- -4.7972907300764499e-02 ]
-
-#feature traker paprameters
-max_cnt: 150 # max feature number in feature tracking
-min_dist: 25 # min distance between two features
-freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
-F_threshold: 1.0 # ransac threshold (pixel)
-show_track: 1 # publish tracking image as topic
-equalize: 1 # if image is too dark or light, trun on equalize to find enough features
-fisheye: 1 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
-
-#optimization parameters
-max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8 # max solver itrations, to guarantee real time
-keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
-
-#imu parameters The more accurate parameters you provide, the better performance
-acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04
-gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
-acc_w: 0.0004 # accelerometer bias random work noise standard deviation. #0.02
-gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
-g_norm: 9.80766 # gravity magnitude
-
-#loop closure parameters
-loop_closure: 0 # start loop closure
-load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
-fast_relocalization: 0 # useful in real-time and large project
-pose_graph_save_path: "/home/tony-ws1/output/pose_graph/" # save and load path
-
-#unsynchronization parameters
-estimate_td: 0 # online estimate time offset between camera and imu
-td: 0.0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
-
-#rolling shutter parameters
-rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
-rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
-
-#visualization parameters
-save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
-visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
-visualize_camera_size: 0.4 # size of camera marker in RVIZ
diff --git a/config/usb_cam_config.yaml b/config/usb_cam_config.yaml
new file mode 100644
index 000000000..0c2bf9bed
--- /dev/null
+++ b/config/usb_cam_config.yaml
@@ -0,0 +1,121 @@
+%YAML:1.0
+
+#common parameters
+imu_topic: "/mavros/imu/data"
+image_topic: "/usb_cam/image_raw"
+output_path: "/home/op/output/"
+
+#camera calibration
+model_type: PINHOLE
+camera_name: usb_camera
+image_width: 640
+image_height: 480
+distortion_parameters:
+ k1: 0.05164855018573922
+ k2: -0.04835348092937038
+ p1: 0.00473987181761685
+ p2: -0.00328630818706201
+projection_parameters:
+ fx: 473.4113628724277
+ fy: 473.5713828088313
+ cx: 311.4014706693625
+ cy: 256.9394802970527
+
+# Extrinsic parameter between IMU and Camera.
+# Полученно из калибровки Kalibr
+#
+# Kalibr выдал T_ci (imu to cam):
+# [[-0.99853052 -0.04402128 -0.03160572]
+# [ 0.03392303 -0.05292855 -0.99802194]
+# [ 0.04226135 -0.99762753 0.0543441 ]]
+#
+# Нам нужна T_ic (cam to imu) = T_ci^T
+estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
+ # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
+ # 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
+#If you choose 0 or 1, you should write down the following matrix.
+#Rotation from camera frame to imu frame, imu^R_cam (T_ic)
+# Матрица вращения из калибровки Kalibr (транспонирована)
+extrinsicRotation: !!opencv-matrix
+ rows: 3
+ cols: 3
+ dt: d
+ # data: [-0.99853052, 0.03392303, 0.04226135,
+ # -0.04402128, -0.05292855, -0.99762753,
+ # -0.03160572, -0.99802194, 0.0543441]
+
+ data: [-1., 0., 0.,
+ 0., 0., -1.,
+ 0., -1., 0.0]
+#Translation from camera frame to imu frame, imu^T_cam
+# Из калибровки Kalibr
+extrinsicTranslation: !!opencv-matrix
+ rows: 3
+ cols: 1
+ dt: d
+ data: [0.00, 0.000, 0.00]
+
+#feature traker paprameters
+max_cnt: 150 # max feature number in feature tracking
+min_dist: 10 # min distance between two features
+freq: 20 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
+F_threshold: 1.0 # ransac threshold (pixel)
+fast_threshold: 20 # threshold for AGAST corner detector (default: 20)
+use_bidirectional_flow: 0 # DISABLED for speed (forward-only tracking)
+use_advanced_flow: 1 # 1 = advanced LK (term crit + optional backward check), 0 = basic LK
+show_track: 1 # publish tracking image as topic
+equalize: 0 # if image is too dark or light, trun on equalize to find enough features
+fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
+
+#adaptive tracking for high-speed navigation
+enable_velocity_check: 1 # enable velocity-based adaptive feature tracking
+max_velocity_threshold: 3.0 # m/s - velocity threshold to trigger feature boost
+velocity_boost_features: 50 # extra features to add when high velocity detected
+min_parallax_threshold: 5.0 # pixels - minimum parallax for quality tracking
+
+#optimization parameters
+max_solver_time: 0.03 # max solver time (s) - reduced for real-time performance
+max_num_iterations: 15 # max solver itrations, to guarantee real time
+keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
+
+
+#imu parameters The more accurate parameters you provide, the better performance
+acc_n: 0.2 # accelerometer measurement noise standard deviation.
+gyr_n: 0.05 # gyroscope measurement noise standard deviation.
+acc_w: 0.002 # accelerometer bias random work noise standard deviation.
+gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation.
+g_norm: 9.805 #
+
+#loop closure parameters
+loop_closure: 1 # start loop closure (ОТКЛЮЧЕНО - может вызывать скачки при движении)
+load_previous_pose_graph: 0 # load and reuse previous pose graph
+fast_relocalization: 0 # useful in real-time and large project
+pose_graph_save_path: "/home/op/output/pose_graph/" # save and load path
+
+#unsynchronization parameters
+estimate_td: 1 # online estimate time offset between camera and imu (ОТКЛЮЧЕНО для стабильности)
+estimate_td_period: 7.0 # seconds between td optimization bursts
+td: -0.1 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
+
+#rolling shutter parameters
+rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera (отключено - улучшает результаты)
+rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet)
+
+#visualization parameters
+save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
+visualize_imu_forward: 0 # output imu forward propogation to achieve low latency and high frequence results
+visualize_camera_size: 0.4 # size of camera marker in RVIZ
+
+#zero velocity update (ZUPT) parameters
+enable_zupt: 1 # enable zero velocity update to prevent drift when stationary
+zupt_vel_threshold: 0.15 # velocity threshold (m/s) for detecting stationary state
+zupt_acc_threshold: 0.25 # acceleration deviation threshold (m/s^2) from gravity
+
+#imu filtering parameters
+enable_imu_acc_filter: 1 # enable accelerometer outlier rejection and smoothing
+imu_acc_max_change: 3.0 # maximum allowed acceleration change (m/s^2) for outlier detection
+imu_acc_filter_alpha: 0.75 # exponential filter coefficient (0.0-1.0, higher = more smoothing)
+
+#altitude scale correction parameters
+enable_altitude_scale_correction: 1 # enable scale correction using MAVLink altitude
+altitude_correction_factor: 0.1 # correction strength (0.0-1.0, lower = gentler correction)
diff --git a/config/vins_rviz_config.rviz b/config/vins_rviz_config.rviz
index 6ed63a93d..c0c7fde84 100644
--- a/config/vins_rviz_config.rviz
+++ b/config/vins_rviz_config.rviz
@@ -1,47 +1,41 @@
Panels:
- - Class: rviz/Displays
+ - Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
- Expanded: ~
- Splitter Ratio: 0.465115994
- Tree Height: 221
- - Class: rviz/Selection
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /feature points1
+ - /pose graph points1
+ Splitter Ratio: 0.5
+ Tree Height: 576
+ - Class: rviz_common/Selection
Name: Selection
- - Class: rviz/Tool Properties
- Expanded:
- - /2D Pose Estimate1
- - /2D Nav Goal1
- - /Publish Point1
+ - Class: rviz_common/Tool Properties
+ Expanded: ~
Name: Tool Properties
- Splitter Ratio: 0.588679016
- - Class: rviz/Views
+ Splitter Ratio: 0.5
+ - Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- - Class: rviz/Time
+ - Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: tracked image
- - Class: rviz/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded: ~
- Splitter Ratio: 0.5
- Tree Height: 359
+ SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
- Class: rviz/Grid
- Color: 130; 130; 130
+ Class: rviz_default_plugins/Grid
+ Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.0299999993
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -53,442 +47,325 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame:
Value: true
- - Class: rviz/Axes
+ - Class: rviz_default_plugins/TF
Enabled: true
- Length: 1
- Name: Axes
- Radius: 0.100000001
- Reference Frame:
+ Filter (blacklist): ""
+ Filter (whitelist): ""
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ {}
+ Update Interval: 0
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz_default_plugins/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: false
+ Keep: 100
+ Name: vins odometry
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /vins_estimator/odometry
+ Value: false
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz_default_plugins/Path
+ Color: 0; 255; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: vins path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /vins_estimator/path
Value: true
- Alpha: 1
Buffer Length: 1
- Class: rviz/Path
+ Class: rviz_default_plugins/Path
Color: 255; 0; 0
Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
Line Style: Lines
- Line Width: 0.5
- Name: ground_truth_path
+ Line Width: 0.029999999329447746
+ Name: pose graph path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /benchmark_publisher/path
- Unreliable: false
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /pose_graph/path_1
Value: true
- - Class: rviz/Image
+ - Class: rviz_default_plugins/MarkerArray
Enabled: true
- Image Topic: /feature_tracker/feature_img
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: tracked image
- Normalize Range: true
- Queue Size: 2
- Transport Hint: raw
- Unreliable: false
+ Name: camera pose visual
+ Namespaces:
+ {}
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /vins_estimator/camera_pose_visual
Value: true
- - Class: rviz/Image
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: feature points
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /vins_estimator/keyframe_point
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz_default_plugins/PointCloud
+ Color: 246; 211; 45
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: pose graph points
+ Position Transformer: XYZ
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /vins_estimator/point_cloud
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz_default_plugins/Image
Enabled: false
- Image Topic: /cam0/image_raw
Max Value: 1
Median window: 5
Min Value: 0
- Name: raw_image
+ Name: Image
Normalize Range: true
- Queue Size: 2
- Transport Hint: raw
- Unreliable: false
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /feature_tracker/feature_img
Value: false
- - Class: rviz/Group
- Displays:
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 0; 255; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.0299999993
- Name: Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /vins_estimator/path
- Unreliable: false
- Value: true
- - Class: rviz/MarkerArray
- Enabled: true
- Marker Topic: /vins_estimator/camera_pose_visual
- Name: camera_visual
- Namespaces:
- {}
- Queue Size: 100
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: true
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/PointCloud
- Color: 0; 255; 0
- Color Transformer: FlatColor
- Decay Time: 0
- Enabled: true
- Invert Rainbow: false
- Max Color: 255; 255; 255
- Max Intensity: 4096
- Min Color: 0; 0; 0
- Min Intensity: 0
- Name: current_point
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 1
- Size (m): 0.00999999978
- Style: Points
- Topic: /vins_estimator/point_cloud
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: true
- Value: true
- - Alpha: 1
- Autocompute Intensity Bounds: false
- Autocompute Value Bounds:
- Max Value: 10
- Min Value: -10
- Value: true
- Axis: Z
- Channel Name: intensity
- Class: rviz/PointCloud
- Color: 255; 255; 255
- Color Transformer: FlatColor
- Decay Time: 3000
- Enabled: false
- Invert Rainbow: true
- Max Color: 0; 0; 0
- Max Intensity: 4096
- Min Color: 0; 0; 0
- Min Intensity: 0
- Name: history_point
- Position Transformer: XYZ
- Queue Size: 10
- Selectable: true
- Size (Pixels): 1
- Size (m): 0.5
- Style: Points
- Topic: /vins_estimator/history_cloud
- Unreliable: false
- Use Fixed Frame: true
- Use rainbow: false
- Value: false
- - Class: rviz/TF
- Enabled: true
- Frame Timeout: 15
- Frames:
- All Enabled: true
- Marker Scale: 1
- Name: TF
- Show Arrows: true
- Show Axes: true
- Show Names: true
- Tree:
- {}
- Update Interval: 0
- Value: true
- Enabled: false
- Name: VIO
- - Class: rviz/Group
- Displays:
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 0; 255; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.0299999993
- Name: pose_graph_path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/pose_graph_path
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 255; 170; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.0299999993
- Name: base_path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/base_path
- Unreliable: false
- Value: true
- - Class: rviz/MarkerArray
- Enabled: true
- Marker Topic: /pose_graph/pose_graph
- Name: loop_visual
- Namespaces:
- {}
- Queue Size: 100
- Value: true
- - Class: rviz/MarkerArray
- Enabled: true
- Marker Topic: /pose_graph/camera_pose_visual
- Name: camera_visual
- Namespaces:
- {}
- Queue Size: 100
- Value: true
- - Class: rviz/Image
- Enabled: true
- Image Topic: /pose_graph/match_image
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: loop_match_image
- Normalize Range: true
- Queue Size: 2
- Transport Hint: raw
- Unreliable: false
- Value: true
- - Class: rviz/Marker
- Enabled: true
- Marker Topic: /pose_graph/key_odometrys
- Name: Marker
- Namespaces:
- {}
- Queue Size: 100
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz_default_plugins/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 25; 255; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.300000012
- Name: Sequence1
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/path_1
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 255; 0; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.300000012
- Name: Sequence2
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/path_2
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 0; 85; 255
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.300000012
- Name: Sequence3
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/path_3
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 255; 170; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.300000012
- Name: Sequence4
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/path_4
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 255; 170; 255
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.0299999993
- Name: Sequence5
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/path_5
- Unreliable: false
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz/Path
- Color: 25; 255; 0
- Enabled: true
- Head Diameter: 0.300000012
- Head Length: 0.200000003
- Length: 0.300000012
- Line Style: Lines
- Line Width: 0.0299999993
- Name: no_loop_path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.0299999993
- Shaft Diameter: 0.100000001
- Shaft Length: 0.100000001
- Topic: /pose_graph/no_loop_path
- Unreliable: false
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
Value: true
+ Value: true
Enabled: true
- Name: pose_graph
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /lio/odom
+ Value: true
Enabled: true
Global Options:
- Background Color: 0; 0; 0
- Fixed Frame: world
+ Background Color: 48; 48; 48
+ Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- - Class: rviz/MoveCamera
- - Class: rviz/Select
- - Class: rviz/FocusCamera
- - Class: rviz/Measure
- - Class: rviz/SetInitialPose
- Topic: /initialpose
- - Class: rviz/SetGoal
- Topic: /move_base_simple/goal
- - Class: rviz/PublishPoint
+ - Class: rviz_default_plugins/Interact
+ Hide Inactive Objects: true
+ - Class: rviz_default_plugins/MoveCamera
+ - Class: rviz_default_plugins/Select
+ - Class: rviz_default_plugins/FocusCamera
+ - Class: rviz_default_plugins/Measure
+ Line color: 128; 128; 0
+ - Class: rviz_default_plugins/SetInitialPose
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /initialpose
+ - Class: rviz_default_plugins/SetGoal
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /goal_pose
+ - Class: rviz_default_plugins/PublishPoint
Single click: true
- Topic: /clicked_point
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /clicked_point
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
Value: true
Views:
Current:
- Class: rviz/XYOrbit
- Distance: 20.9684067
+ Class: rviz_default_plugins/Orbit
+ Distance: 5.277319431304932
Enable Stereo Rendering:
- Stereo Eye Separation: 0.0599999987
+ Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: -2.7069304
- Y: -1.26974416
- Z: 2.1410624e-05
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.0500000007
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.00999999978
- Pitch: 1.0797962
- Target Frame:
- Value: XYOrbit (rviz)
- Yaw: 3.08722663
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.3303983509540558
+ Target Frame: camera
+ Value: Orbit (rviz_default_plugins)
+ Yaw: 5.818586826324463
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 912
+ Height: 831
Hide Left Dock: false
- Hide Right Dock: true
- QMainWindow State: 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
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000300000000000001560000029ffc0200000004fc0000003f0000029f000000eb0100001efa000000030100000004fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb0000000a0049006d0061006700650100000000ffffffff0000000000000000fb00000016006d006100740063006800200069006d0061006700650000000000ffffffff0000000000000000fb000000100044006900730070006c0061007900730100000000000001690000015600fffffffb00000016006d0061007400630068005f0069006d00610067006500000001e00000007a0000000000000000fb0000000a0049006d0061006700650100000111000001490000000000000000fb0000000a0049006d00610067006501000001b5000000a5000000000000000000000001000001000000029ffc0200000003fb0000000a0049006d006100670065010000003f000000b40000000000000000fb0000000a0049006d006100670065010000003f000000a50000000000000000fc0000003f0000029f000000c80100001efa000000040100000005fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000001a006600650061007400750072006500200069006d0061006700650000000000ffffffff0000000000000000fb0000001200530065006c0065006300740069006f006e0000000000ffffffff0000007300fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000009b00fffffffb0000000a00560069006500770073010000045e000001000000010000ffffff000000030000055e0000003cfc0100000001fb0000000800540069006d006501000000000000055e0000026f00ffffff000002fc0000029f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -496,13 +373,7 @@ Window Geometry:
Tool Properties:
collapsed: false
Views:
- collapsed: true
- Width: 1390
- X: 2127
- Y: 109
- loop_match_image:
- collapsed: false
- raw_image:
- collapsed: false
- tracked image:
collapsed: false
+ Width: 1374
+ X: 66
+ Y: 32
diff --git a/data_generator/CMakeLists.txt b/data_generator/CMakeLists.txt
deleted file mode 100644
index dfd0a44e5..000000000
--- a/data_generator/CMakeLists.txt
+++ /dev/null
@@ -1,23 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(data_generator)
-
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11 -march=native")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
-
-find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs nav_msgs cv_bridge image_transport)
-
-find_package(OpenCV REQUIRED)
-
-catkin_package()
-
-include_directories(
- ${catkin_INCLUDE_DIRS}
-)
-
-add_executable(data_generator
- src/data_generator_node.cpp
- src/data_generator.cpp
- )
-
-target_link_libraries(data_generator ${catkin_LIBRARIES} ${OpenCV_LIBS})
diff --git a/data_generator/package.xml b/data_generator/package.xml
deleted file mode 100644
index 8bc1e2b68..000000000
--- a/data_generator/package.xml
+++ /dev/null
@@ -1,55 +0,0 @@
-
-
- data_generator
- 0.0.0
- The data_generator package
-
-
-
-
- dvorak
-
-
-
-
-
- TODO
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- roscpp
- roscpp
-
-
-
-
-
-
-
-
-
-
-
diff --git a/data_generator/src/data_generator.cpp b/data_generator/src/data_generator.cpp
deleted file mode 100644
index 2876777a6..000000000
--- a/data_generator/src/data_generator.cpp
+++ /dev/null
@@ -1,344 +0,0 @@
-#include "data_generator.h"
-
-#define SEED 1
-#define Y_COS 2
-#define Z_COS 2 * 2
-#define IMU_NOISE 0
-#define IMG_NOISE 0
-#define BIAS_ACC 0
-#define BIAS_GYR 1
-
-DataGenerator::DataGenerator()
-{
- srand(SEED);
- t = 0;
- current_id = 0;
-
- for (int i = 0; i < NUM_POINTS; i++)
- {
- pts[i * 3 + 0] = rand() % (6 * MAX_BOX) - 3 * MAX_BOX;
- pts[i * 3 + 1] = rand() % (6 * MAX_BOX) - 3 * MAX_BOX;
- pts[i * 3 + 2] = rand() % (6 * MAX_BOX) - 3 * MAX_BOX;
- cout << "pts i " << i << " " << pts[i * 3 + 0] << " " << pts[i * 3 + 1] << " " << pts[i * 3 + 2] << endl;
- }
-
- ap[0] = Vector3d(MAX_BOX, -MAX_BOX, MAX_BOX);
- ap[1] = Vector3d(-MAX_BOX, MAX_BOX, MAX_BOX);
- ap[2] = Vector3d(-MAX_BOX, -MAX_BOX, -MAX_BOX);
- ap[3] = Vector3d(MAX_BOX, MAX_BOX, -MAX_BOX);
-
- Ric[0] << 0, 0, -1,
- -1, 0, 0,
- 0, 1, 0;
- Tic[0] << 0.0, 0.2, 0.6;
- if (NUMBER_OF_CAMERA >= 2)
- {
- Ric[1] = Ric[0];
- Tic[1] << 0.00, 0.00, 0.30;
- }
- if (NUMBER_OF_CAMERA >= 3)
- {
- Ric[2] << 0, 1, 0,
- -1, 0, 0,
- 0, 0, 1;
- Tic[2] << 0.00, 0.00, 0.15;
- }
-
- acc_cov = 0.01 * 0.01 * Matrix3d::Identity();
- gyr_cov = 0.001 * 0.001 * Matrix3d::Identity();
- pts_cov = (0.3 / 460) * (0.3 / 460) * Matrix2d::Identity();
-
- generator = default_random_engine(SEED);
- distribution = normal_distribution(0.0, 1);
- Axis[0] = Vector3d(10, 0, 0);
- Axis[1]= Vector3d(0, 10, 0);
- Axis[2] = Vector3d(0, 0, 10);
- Axis[3] = Vector3d(-10, 0, 0);
- Axis[4]= Vector3d(0, -10, 0);
- Axis[5] = Vector3d(0, 0, -10);
-}
-
-void DataGenerator::update()
-{
- t += 1.0 / FREQ;
-}
-
-double DataGenerator::getTime()
-{
- return t;
-}
-
-Vector3d DataGenerator::getPoint(int i)
-{
- return Vector3d(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2]);
-}
-
-Vector3d DataGenerator::getAP(int i)
-{
- return ap[i];
-}
-
-Vector3d DataGenerator::getPosition()
-{
- double x, y, z;
- if (t < MAX_TIME)
- {
- x = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI);
- y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * Y_COS);
- z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(t / MAX_TIME * M_PI * Z_COS);
- }
- else if (t >= MAX_TIME && t < 2 * MAX_TIME)
- {
- x = MAX_BOX / 2.0 - MAX_BOX / 2.0;
- y = MAX_BOX / 2.0 + MAX_BOX / 2.0;
- z = MAX_BOX / 2.0 + MAX_BOX / 2.0;
- }
- else
- {
- double tt = t - 2 * MAX_TIME;
- x = -MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI);
- y = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI * Y_COS);
- z = MAX_BOX / 2.0 + MAX_BOX / 2.0 * cos(tt / MAX_TIME * M_PI * Z_COS);
- }
-
- return Vector3d(x, y, z);
-}
-
-Matrix3d DataGenerator::getRotation()
-{
- //return Matrix3d::Identity();
- return (AngleAxisd(30.0 / 180 * M_PI * sin(t / MAX_TIME * M_PI * 2), Vector3d::UnitX()) * AngleAxisd(40.0 / 180 * M_PI * sin(t / MAX_TIME * M_PI * 2), Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitZ())).toRotationMatrix();
-}
-
-Vector3d DataGenerator::getAngularVelocity()
-{
- const double delta_t = 0.00001;
- Matrix3d rot = getRotation();
- t += delta_t;
- Matrix3d drot = (getRotation() - rot) / delta_t;
- t -= delta_t;
- Matrix3d skew = rot.inverse() * drot;
- if(IMU_NOISE)
- {
- Vector3d disturb = Vector3d(distribution(generator) * sqrt(gyr_cov(0, 0)),
- distribution(generator) * sqrt(gyr_cov(1, 1)),
- distribution(generator) * sqrt(gyr_cov(2, 2)));
- return disturb + Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0));
- }
- else
- {
-#if BIAS_GYR
- return Vector3d(skew(2, 1) + 0.02, -skew(2, 0) + 0.03, skew(1, 0) + 0.04);
-#endif
- return Vector3d(skew(2, 1), -skew(2, 0), skew(1, 0));
- }
-
-}
-
-Vector3d DataGenerator::getVelocity()
-{
- double dx, dy, dz;
- if (t < MAX_TIME)
- {
- dx = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI);
- dy = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * Y_COS) * (1.0 / MAX_TIME * M_PI * Y_COS);
- dz = MAX_BOX / 2.0 * -sin(t / MAX_TIME * M_PI * Z_COS) * (1.0 / MAX_TIME * M_PI * Z_COS);
- }
- else if (t >= MAX_TIME && t < 2 * MAX_TIME)
- {
- dx = 0.0;
- dy = 0.0;
- dz = 0.0;
- }
- else
- {
- double tt = t - 2 * MAX_TIME;
- dx = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI);
- dy = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI * Y_COS) * (1.0 / MAX_TIME * M_PI * Y_COS);
- dz = MAX_BOX / 2.0 * -sin(tt / MAX_TIME * M_PI * Z_COS) * (1.0 / MAX_TIME * M_PI * Z_COS);
- }
-
- return getRotation().inverse() * Vector3d(dx, dy, dz);
-}
-
-Vector3d DataGenerator::getLinearAcceleration()
-{
- double ddx, ddy, ddz;
- if (t < MAX_TIME)
- {
- ddx = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI);
- ddy = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * Y_COS) * (1.0 / MAX_TIME * M_PI * Y_COS) * (1.0 / MAX_TIME * M_PI * Y_COS);
- ddz = MAX_BOX / 2.0 * -cos(t / MAX_TIME * M_PI * Z_COS) * (1.0 / MAX_TIME * M_PI * Z_COS) * (1.0 / MAX_TIME * M_PI * Z_COS);
- }
- else if (t >= MAX_TIME && t < 2 * MAX_TIME)
- {
- ddx = 0.0;
- ddy = 0.0;
- ddz = 0.0;
- }
- else
- {
- double tt = t - 2 * MAX_TIME;
- ddx = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI) * (1.0 / MAX_TIME * M_PI);
- ddy = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI * Y_COS) * (1.0 / MAX_TIME * M_PI * Y_COS) * (1.0 / MAX_TIME * M_PI * Y_COS);
- ddz = MAX_BOX / 2.0 * -cos(tt / MAX_TIME * M_PI * Z_COS) * (1.0 / MAX_TIME * M_PI * Z_COS) * (1.0 / MAX_TIME * M_PI * Z_COS);
- }
- if(IMU_NOISE)
- {
- Vector3d disturb = Vector3d(distribution(generator) * sqrt(acc_cov(0, 0)),
- distribution(generator) * sqrt(acc_cov(1, 1)),
- distribution(generator) * sqrt(acc_cov(2, 2)));
- return getRotation().inverse() * (disturb + Vector3d(ddx, ddy, ddz + 9.805));
- }
- else
- {
-#if BIAS_ACC
- return getRotation().inverse() * Vector3d(ddx, ddy, ddz + 9.805) + Vector3d(0.01, 0.02, 0.03);
-#endif
- return getRotation().inverse() * Vector3d(ddx, ddy, ddz + 9.805);
-
- }
-
-}
-
-vector> DataGenerator::getImage()
-{
- vector> image;
- Vector3d position = getPosition();
- Matrix3d quat = getRotation();
- printf("max: %d\n", current_id);
-
- vector ids[NUMBER_OF_CAMERA], gr_ids[NUMBER_OF_CAMERA];
- vector cur_pts[NUMBER_OF_CAMERA];
- for (int k = 0; k < NUMBER_OF_CAMERA; k++)
- {
- for (int i = 0; i < NUM_POINTS; i++)
- {
- double xx = pts[i * 3 + 0] - position(0);
- double yy = pts[i * 3 + 1] - position(1);
- double zz = pts[i * 3 + 2] - position(2);
- Vector3d local_point = Ric[k].inverse() * (quat.inverse() * Vector3d(xx, yy, zz) - Tic[k]);
- xx = local_point(0);
- yy = local_point(1);
- zz = local_point(2);
-
- if (zz > 0.0 && std::fabs(atan2(xx, zz)) <= M_PI * FOV / 2 / 180 && std::fabs(atan2(yy, zz)) <= M_PI * FOV / 2 / 180)
- {
- xx = xx / zz;
- yy = yy / zz;
- zz = zz / zz;
-#if IMG_NOISE
- xx += distribution(generator) * sqrt(pts_cov(0, 0));
- yy += distribution(generator) * sqrt(pts_cov(1, 1));
-#endif
-
- int n_id = before_feature_id[k].find(i) == before_feature_id[k].end() ? -1 /*current_id++*/ : before_feature_id[k][i];
- ids[k].push_back(n_id);
- gr_ids[k].push_back(i);
- cur_pts[k].push_back(Vector3d(xx, yy, zz));
- }
- }
-
- for (int i = 0; i < 6; i++)
- {
- output_Axis[i].clear();
- Vector3d local_point;
- local_point = Ric[k].inverse() * (quat.inverse() * (Axis[i] - position) - Tic[k]);
- double xx = local_point(0);
- double yy = local_point(1);
- double zz = local_point(2);
- if(zz > 0.0 && std::fabs(atan2(xx, zz)) <= M_PI * FOV / 2 / 180 && std::fabs(atan2(yy, zz)) <= M_PI * FOV / 2 / 180)
- {
- xx = xx / zz;
- yy = yy / zz;
- zz = zz / zz;
- output_Axis[i].push_back(Vector3d(xx, yy, zz));
- local_point = Ric[k].inverse() * (quat.inverse() * (Axis[i] + Vector3d(1, 0, 0) - position) - Tic[k]);
- xx = local_point(0);
- yy = local_point(1);
- zz = local_point(2);
- xx = xx / zz;
- yy = yy / zz;
- zz = zz / zz;
- output_Axis[i].push_back(Vector3d(xx, yy, zz));
- local_point = Ric[k].inverse() * (quat.inverse() * (Axis[i] + Vector3d(0, 1, 0) - position) - Tic[k]);
- xx = local_point(0);
- yy = local_point(1);
- zz = local_point(2);
- xx = xx / zz;
- yy = yy / zz;
- zz = zz / zz;
- output_Axis[i].push_back(Vector3d(xx, yy, zz));
- local_point = Ric[k].inverse() * (quat.inverse() * (Axis[i] + Vector3d(0, 0, 1) - position) - Tic[k]);
- xx = local_point(0);
- yy = local_point(1);
- zz = local_point(2);
- xx = xx / zz;
- yy = yy / zz;
- zz = zz / zz;
- output_Axis[i].push_back(Vector3d(xx, yy, zz));
- }
- }
- }
-
- output_gr_pts.clear();
- for (auto i : gr_ids[0])
- {
- output_gr_pts.emplace_back(pts[i * 3 + 0], pts[i * 3 + 1], pts[i * 3 + 2]);
- }
-
- //for (int k = 0; k < 1/* NUMBER_OF_CAMERA - 1 */; k++)
- //{
- // for (int i = 0; i < int(ids[k].size()); i++)
- // {
- // if (ids[k][i] != -1)
- // continue;
- // for (int j = 0; j < int(ids[k + 1].size()); j++)
- // if (ids[k + 1][j] == -1 && gr_ids[k][i] == gr_ids[k + 1][j])
- // ids[k][i] = ids[k + 1][j] = current_id++;
- // }
- //}
-
- for (int k = 0; k < NUMBER_OF_CAMERA; k++)
- {
- for (int i = 0; i < int(ids[k].size()); i++)
- {
- if (ids[k][i] == -1)
- ids[k][i] = current_id++;
- current_feature_id[k][gr_ids[k][i]] = ids[k][i];
- }
- std::swap(before_feature_id[k], current_feature_id[k]);
- current_feature_id[k].clear();
- }
-
- for (int k = 0; k < NUMBER_OF_CAMERA; k++)
- {
- if (k != 1)
- {
- for (unsigned int i = 0; i < ids[k].size(); i++)
- image.push_back(make_pair(ids[k][i] * NUMBER_OF_CAMERA + k, cur_pts[k][i]));
- }
- else if (k == 1)
- {
- for (unsigned int i = 0; i < ids[k].size(); i++)
- {
- if (before_feature_id[0].find(gr_ids[k][i]) != before_feature_id[0].end())
- image.push_back(make_pair(before_feature_id[0][gr_ids[k][i]] * NUMBER_OF_CAMERA + k, cur_pts[k][i]));
- }
- }
- }
- return image;
-}
-
-vector DataGenerator::getCloud()
-{
- vector cloud;
- for (int i = 0; i < NUM_POINTS; i++)
- {
- double xx = pts[i * 3 + 0];
- double yy = pts[i * 3 + 1];
- double zz = pts[i * 3 + 2];
- cloud.push_back(Vector3d(xx, yy, zz));
- }
- return cloud;
-}
diff --git a/data_generator/src/data_generator.h b/data_generator/src/data_generator.h
deleted file mode 100644
index 390fc551d..000000000
--- a/data_generator/src/data_generator.h
+++ /dev/null
@@ -1,66 +0,0 @@
-#include
-#include
-#include
-#include
-#include