# VIO - Visual Inertial Odometry

A semi-direct monocular visual odometry pipeline with IMU and command integration using Extended Kalman Filter (EKF) for robust pose estimation. The system leverages GPU acceleration via OpenCL for real-time performance.

## Features

- **Semi-Direct Visual Odometry**: Combines feature-based and direct alignment approaches for robust tracking
- **GPU-Accelerated Processing**: OpenCL-based feature extraction and image alignment for real-time performance
- **Multi-Sensor Fusion**: EKF integration for IMU and command data fusion with visual odometry
- **ROS Integration**: Native ROS support for easy integration into robotic systems
- **Graph-Based Optimization**: G2O-based global and pose optimization backends
- **Monocular Input**: Works with standard monocular cameras

## System Requirements

- **OS**: Linux (tested on Ubuntu)
- **GPU**: OpenCL-compatible GPU with OpenCL drivers installed
- **C++ Standard**: C++17 or later
- **CMake**: Version 2.8.3 or later

## Dependencies

The following packages are required. Specific build instructions are provided for G2O:

1. **OpenCV** (4.x) - Computer vision library
2. **Eigen** - Linear algebra library
3. **G2O** - Graph optimization library
   ```bash
   # Build G2O with:
   cmake -DG2O_HAVE_OPENGL=ON -DBUILD_WITH_MARCH_NATIVE=ON ..
   # Recommended version: https://github.com/RainerKuemmerle/g2o/tree/memory_management
   ```
4. **Boost** - C++ libraries (requires thread and system components)
5. **OpenCL** - GPU computation framework
6. **ROS** - Robot Operating System (for ROS integration)
   - catkin, roscpp, roslib, image_transport, cv_bridge, tf, geometry_msgs

## Building

### Prerequisites

Ensure you have the OpenCL driver installed on your system:

```bash
# For NVIDIA GPUs
sudo apt-get install nvidia-opencl-icd

# For AMD/Intel, install appropriate drivers
```

### Build Instructions

```bash
# Create a catkin workspace (if using ROS)
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone <this-repo> vio
cd ~/catkin_ws

# Build
catkin_make
# or with specific options:
catkin_make -DCMAKE_BUILD_TYPE=Release
```

For standalone (non-ROS) builds:

```bash
cd vio
mkdir build && cd build
cmake ..
make -j$(nproc)
```

## Configuration

Algorithm parameters can be configured in two main files:

1. **Header Configuration**: `vio/include/vio/config.h`
   - Core algorithm parameters and feature extraction settings

2. **Runtime Configuration**: `vio/param/vo_fast.yaml`
   - Fast configuration profile for real-time operation

Additional configuration files:

- `vio/param/px30.yaml` - Configuration for specific hardware/robotics platform

## Usage

### As a ROS Node

```bash
rosrun vio vio
```

### Configuration via Parameters

Edit `vo_fast.yaml` before running or use ROS parameter server to override settings at runtime.

### Output

The system provides:
- Estimated pose (position and orientation)
- 3D map points
- Tracking statistics and debug information

Log files are written to the project directory. You can modify the logging path and debug mode in `CMakeLists.txt` (see line 89).

## Known Issues

1. **Orientation Estimation**: The estimated orientation may not be fully accurate under certain conditions
2. **Scale Ambiguity**: Semi-direct visual odometry inherently suffers from monocular scale ambiguity; the EKF sensor fusion helps but may not fully resolve it
3. **Feature Point Accumulation**: The number of tracked feature points can increase significantly over time; limiting strategy needed to prevent performance degradation and estimation errors

## Architecture Overview

### Core Components

- **Frame Handler** (`frame_handler_mono.cpp`): Main tracking loop for monocular sequences
- **Feature Detection** (`feature_detection.cpp`): GPU-accelerated FAST corner detection
- **Feature Alignment** (`feature_alignment.cpp`): Direct image alignment for sub-pixel accuracy
- **Matcher** (`matcher.cpp`): Feature correspondence matching
- **Reprojector** (`reprojector.cpp`): Map point reprojection and visibility management
- **Pose Optimizer** (`pose_optimizer.cpp`): Per-frame pose optimization
- **Global Optimizer** (`global_optimizer.cpp`): Graph-based optimization for drift correction
- **Visualizer** (`visualizer.cpp`): Real-time visualization of results

### GPU Acceleration

- **OpenCL Implementation** (`cl_class.cpp`, `sparse_img_align_gpu.cpp`): GPU kernels for feature extraction and alignment
- **Kernel Directory** (`kernel/`): OpenCL kernel source code

## System Diagram

```
Camera Input → Feature Detection (GPU) → Feature Alignment (GPU)
                                              ↓
                                        Frame Tracking
                                              ↓
                          ┌────────────────────┴──────────────────┐
                          ↓                                         ↓
                    Pose Optimization                        Map Management
                          ↓                                         ↓
                    EKF Sensor Fusion ←─── IMU & Command Input
                          ↓
                    Global Optimization (G2O)
                          ↓
                   Odometry Output (Pose, Map)
```

## Troubleshooting

### OpenCL Issues

- Verify OpenCL drivers are installed: `clinfo`
- Check GPU compatibility with OpenCL
- Set `VIO_DEBUG` flag in CMakeLists.txt for detailed GPU debugging

### Performance Issues

- Adjust feature point limits in configuration
- Enable/disable GPU features in `config.h`
- Check OpenCV and G2O build flags match the VIO build flags

## License

[Add appropriate license information]

## Contributing

[Add contributing guidelines if applicable]

## References

- Semi-direct Visual Odometry (SVO) concept
- EKF for sensor fusion
- G2O framework: https://github.com/RainerKuemmerle/g2o
- OpenCV: https://opencv.org/
- ROS: https://www.ros.org/