MSCEqF: Multi State Constraint Equivariant Filter

MSCEqF is a multi-state constraint equivariant filter for visual-inertial navigation. It is based on the recent advances in equivaraint inertial navigation systems [1, 2, 3, 4].

Features

Design features

• Developed as a pure C++ library with ROS1 and ROS2 wrappers available

Filter features

• Supports online camera extrinsic and intrinsic parameters calibration
• Supports unit-plane projection method
• Supports anchored euclidean, anchored inverse depth and anchored polar feature representation methods
• Includes a static initialization routine as well as parametric initialization with custom origin
• Includes an equivariant zero velocity update routine

Vision frontend features

• OpenCV based
• Supports a grid-based multi-thread parallel feature extraction
• Supports different features detector including FAST and Shi-Tomasi
• Supports different image enhancment tecniques, including Histogram and CLAHE

Future roadmap

• ROS1 wrapper
• ROS2 wrapper
• Equivariant Zero velocity Update (EqZVU)
• Unit-sphere projection method support
• Equivariant Persistent (SLAM) features update support

Documentation

Doxygen documentation is available here: MSCEqF documentation

Dependencies

MSCEqF has the following dependencies which are automatically downloaded and linked against:

• Eigen
• Lie++
• yaml-cpp
• googletest
• Boost
• OpenCV

Getting started

ROS free setup

$ git clone <url> msceqf
$ cd msceqf
$ mkdir -p build/<build_type>
$ cd build/<build_type> && cmake -DCMAKE_BUILD_TYPE=<build_type> -DBUILD_TESTS=ON ../..
$ cmake --build . --config Debug --target all -j && cd ../..

Run tests

$ cd msceqf/build/<build_type>
$ ./msceqf_tests

Run example (Euroc)

After downloading the Euroc follows

$ cd msceqf/build/<build_type>
$ ./msceqf_euroc <sequence_name> <euroc_dataset_folder> <euroc_example_folder>

ROS1 setup

$ cd ws/src
$ git clone <url> msceqf
$ cd msceqf
$ catkin build -DCMAKE_BUILD_TYPE=<build_type> -DROS_BUILD=ON

ROS2 setup

$ cd ws/src
$ git clone <url> msceqf
$ cd msceqf
$ colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_BUILD_TYPE=<build_type> --cmake-args -DROS_BUILD=ON

Docker setup

$ sudo apt update
$ sudo apt install -y nvidia-docker2
$ sudo systemctl restart docker
$ cd <path_to_msceqf_folder>
$ docker build --network=host -t msceqf:ros<ros_version> -f docker/Dockerfile_ros<ros_version>
$ xhost +
$ docker run --net=host -it --gpus all --env="NVIDIA_DRIVER_CAPABILITIES=all" --env="DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" msceqf:ros<ros_version> .

If Nvidia drivere are not supported, simply run docker as follows

$ docker run --net=host -it --gpus all --env="NVIDIA_DRIVER_CAPABILITIES=all" --env="DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" msceqf:ros<ros_version> .

Usage with custom dataset and/or with ROS

Utilizing MSCEqF with a custom dataset or specific sensors is a straightforward process. Follow these steps for seamless integration:

Dataset/Sensor preparation

Ensure you possess the camera intrinsic and extrinsic parameters calibration if working with a custom dataset. In case you are working with real sensors, perform a camera calibration before starting. We recommend using Kalibr for efficient camera calibration.

MSCEqF configuration file

Navigate to the desired location to store the filter configuration file:

cd <path_where_to_store_the_filter_configfile>
nano <configfile_name>.yaml

Populate your configuration file with the following settings:

# Initial standard deviations (attitude, velocity, position, bias, extrinsics, instrinsics)
extended_pose_std: [1.0e-1, 1.0e-1, 1.0e-9, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-9, 1.0e-9, 1.0e-9]
bias_std: [1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1, 1.0e-1]
extrinsics_std: [1.0e-2, 1.0e-2, 1.0e-2, 1.0e-2, 1.0e-2, 1.0e-2]
intrinsics_std: [1.0, 1.0, 1.0, 1.0]

# IMU noise statistics
accelerometer_noise_density: 1.0-2
accelerometer_random_walk:   1.0e-3
gyroscope_noise_density: 1.0e-3
gyroscope_random_walk:   1.0e-4

# Camera calibration (according to kalibr format, both T_imu_cam and T_cam_imu)
distortion_coeffs: [0.0, 0.0, 0.0, 0.0]
distortion_model: radtan
resolution: [320, 240]
intrinsics: [250.0, 250.0, 160.0, 120.0]
T_imu_cam: 
  - [1.0, 0.0, 0.0, 0.0]
  - [0.0, -1.0, 0.0, 0.0]
  - [0.0, 0.0, -1.0, 0.0]
  - [0.0, 0.0, 0.0, 1.0]

# Initializer options
# For IMU only motion detection set static_initializer_disparity_threshold: 0.0
# For DISPARITY only motion detection set static_initializer_acc_threshold: 0.0
static_initializer_imu_window: 1.0
static_initializer_disparity_window: 0.5
static_initializer_acc_threshold: 0.25
static_initializer_disparity_threshold: 1.0

# Propagator options
# For numerical exponential computation (costly) set state_transition_order: -1
# For approximated first-order exponential computation (recommended on low-power hardware) set state_transition_order: 0
state_transition_order: 0
imu_buffer_max_size: 1000

# Updater options
# Possible options for zero_velocity_update are enabled, disabled, beginning
refine_traingulation: true
feature_min_depth: 0.1
feature_max_depth: 20
feature_refinement_max_iterations: 20
feature_refinement_tollerance: 1e-10
measurement_projection_method: unit_plane
feature_representation: anchored_inverse_depth
pixel_standerd_deviation: 1.0
curvature_correction: true
zero_velocity_update: enabled

# State options
enable_camera_intrinsic_calibration: false
gravity: 9.81
num_clones: 11

# Tracker options
# Possible options for feature_detector are fast and shi-tomasi
equalization_method: histogram
optical_flow_pyramid_levels: 3
detector_pyramid_levels: 1
feature_detector: fast
grid_x_size: 4
grid_y_size: 4
min_feature_pixel_distance: 15
min_features: 100
max_features: 120
fast_threshold: 20
shi_tomasi_quality_level: 0.75

# Track Manager
max_track_length: 400

# Logger level 
# Possible levels are 0: Full, 1: INFO, 2: WARN, 3: ERR, 4: INACTIVE
logger_level: 1

Adjust values as needed and customize settings thresholds based on your specific requirements.

License

This software is made available to the public to use (source-available), licensed under the terms of the BSD-2-Clause-License with no commercial use allowed, the full terms of which are made available in the LICENSE file.

Usage for academic purposes

If you use this software in an academic research setting, please cite the corresponding papers.

@article{fornasier2023msceqf,
  title={MSCEqF: A Multi State Constraint Equivariant Filter for Vision-aided Inertial Navigation},
  author={Fornasier, Alessandro and van Goor, Pieter and Allak, Eren and Mahony, Robert and Weiss, Stephan},
  journal={arXiv preprint arXiv:2311.11649},
  year={2023}
}

@article{fornasier2023equivariant,
  title={Equivariant Symmetries for Inertial Navigation Systems},
  author={Fornasier, Alessandro and Ge, Yixiao and van Goor, Pieter and Mahony, Robert and Weiss, Stephan},
  journal={arXiv preprint arXiv:2309.03765},
  year={2023}
}

References

[1] van Goor, Pieter, Tarek Hamel, and Robert Mahony. "Equivariant filter (eqf)." IEEE Transactions on Automatic Control (2022).

[2] Fornasier, Alessandro, et al. "Equivariant filter design for inertial navigation systems with input measurement biases." 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022.

[3] Fornasier, Alessandro, et al. "Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibration." IEEE Robotics and Automation Letters 7.4 (2022): 12118-12125.

[4] Fornasier, Alessandro, et al. "Equivariant Symmetries for Inertial Navigation Systems." ArXiv preprint.