Unified Gaussian Preintegrated Measurements (UGPMs)

Related tags

Miscellaneous ugpm
Overview

author: [email protected] (Cedric)

Unified Gaussian Preintegrated Measurements (UGPMs)

and the Linear Preintegrated Measurements (LPMs)

This repository provides the C++ implementation of the preintegration methods presented in our RSS'21 paper titled Continuous Integration over SO(3) for IMU Preintegration (with video here ). If you are using that code for any purpose, please cite the corresponding work as explained at the end of this page.

In short preintegration is a way to combine IMU data into pseudo measurements called preintegrated measurements. This is especially useful in the context of optimisation-based state estimation.

This repository contains the implementation of two novel methods that are the UGPMs and LPMs, and an implementation of our previous work Gaussian Process Preintegration for Inertial-Aided Navigation Systems (GPMs).

Dependencies

This repository depends on cmake and g++ for the compilation.

Libraries needed (boost doesn't need all, but shouldn't harm to have it all :) ):

sudo apt-get install libeigen3-dev
sudo apt-get install libboost-all-dev
sudo apt-get install libceres-dev

The code uses OpenMP but should be part of your compiler.

Compile

You will need to compile the code in a build directory.

cd <this-repo>
mkdir build
cd build
cmake ../src
make

Run

This repository contains two executables:

  • app/ugpm_demo
  • app/paper_metrics
ugpm_demo

This allows you to run the different preintegration methods (UGPM, LPM, GPM) with different parameters over simulated IMU data. It then compute the error with respect to the ground truth. The parameters are:

  • -m, --method : choice of the preintegration method "ugpm", "lpm", or "gpm" (with "ugpm" being the default value)
  • -l, --length : length of the integration window (2 seconds by default)
  • -q, --quantum : controls the length of the chunks in the per-chunk mode of the preintegrated measurements. If the quantum is negative, the per-chunk mode is deactivated.
  • -n, -nb_inference : number of inference to compute to test/show the small marginal computational cost of additional inferences in the same integration window (useful when dealing with high framerate sensor fusion, e.g., lidar-inertial)
  • -t, --train : flag to activate the hyper-parameter training of the Gaussian Processes when applicable
  • -j, -jacobian : displays the Jacobian matrices produced by the preintegration method for postintegration correction vs. the numerical differentiation (most used for debugging, there is a full performance analysis of the postintegration corrections in the paper)
  • -h, --help: produces a succinct help message

Here is a typical output of the proposed program (ran from the build repository):

./app/ugpm_demo -l 1 -m ugpm

Preintegration demonstration with UGPM
Time elapsed: 109.725 ms
Preintegration errors over window of 1:
  Rotation [deg] = 0.00974787
  Velocity [m/s] = 0.00370596
  Position [m]   = 0.00291148

Covariance
 6.03032e-08  4.13616e-10 -3.47978e-10            0            0            0            0            0            0
 4.13616e-10  4.70761e-08  -1.3584e-09            0            0            0            0            0            0
-3.47978e-10  -1.3584e-09  5.14124e-08            0            0            0            0            0            0
           0            0            0  1.33392e-05 -1.02802e-06  2.03985e-06            0            0            0
           0            0            0 -1.02802e-06  1.51109e-05  1.59617e-06            0            0            0
           0            0            0  2.03985e-06  1.59617e-06   9.5349e-06            0            0            0
           0            0            0            0            0            0   8.7608e-07 -1.40623e-07   2.0043e-07
           0            0            0            0            0            0 -1.40623e-07  1.12362e-06   -2.819e-08
           0            0            0            0            0            0   2.0043e-07   -2.819e-08  8.60975e-07

paper_metrics

This executable prints out the experiments' results of the paper in a pseudo-latex format. It takes parameters for the number of Monte Carlo runs and to select which experiment to run. Please refer to ./app/paper_metrics -h for more information.

Ideas and stuff to have in mind if you want to use in your system:

Choice of preintegration method

If your main constraint is the computation time, I recommend the use of the LPMs.

For the rest the UGPMs per-chunk would be my method of choice (excellent accuracy in contained computation time). In my tests I generally use a quantum of 0.2 sec.

Preintegration object

In short, the overall use of the ImuPreintegration object corresponds to the instantiation of the object with the constructor celib::ImuPreintegration(data, start_t, t, preint_opt, prior) with:

  • data: an ImuData structure defined in library/include/common/types.h (basically just to vectors of accelerometer and gyroscope data).
  • start_t: a double that represents the timestamp of the beginning of the integration window
  • t: a std::vector > that contains the timestamps at which you want to infer the preintegrated measurements. Each of the vectors in t needs to be of increasing order (the vector of vector thing allows for passing multiple series of timestamp to ease the data management in the case of multi-modal systems).
  • preint_opt: a PreintOption structure that specifies the parameters of the preintegration method. Its definition can be found in library/include/imu_preintegration/preintegration.h
  • prior: a PreintPrior structure that specifies the prior knowledge of the IMU biases. Its definition can be found in library/include/imu_preintegration/preintegration.h. Warning: I didn't really test that feature yet. Might be bugged. (Let me know if there is an issue there).

Then, you can retrieve the preintegrated measurements using celib::ImuPreintegration::get(index_1, index_2) with index_1 and index_2 corresponding to the indexes in the vector of vector t[index_1][index_2]. The output will be a PreintMeas structure as defined in library/include/common/types.h

Heads-up

Among the things to keep in mind: the timing performances shown in the paper are based on 100Hz IMU data. Using faster IMU will imply more data therefore slower computations. Additionally, to perform optimally, the UGPMs (and GPMs) need a bit of data overlap between integration windows, in this example, I arbitrarily chose 0.15 sec which is pretty big. That can probably be reduced while maintaining high accuracy.

I "implemented" some minimum parallelisation using OpenMP but I am sure more efficient implementation are possible (even GPU acceleration should be possible especially if many inferences per integration window are needed, e.g. per-lidar-point preintegrated measurements). I don't plan to make this code computationally more efficient on my own, but happy to discuss/help anyone who wants.

Things I would check if I have spare time: while providing OK results in quantitative experiments, the Jacobians for the postintegration time-shift correction of the UGPM and GPM rotation parts seems a bit off. Not sure why, to be investigated.

Erratum in the RSS paper

In Table II-c), the first cells of the last two rows ("Fast Pos er." and "Slow Pos er." ) should be swapped (only the first cell, not the full row).

Citing

The UGPMs and LPMs have both been introduced in Continuous Integration over SO(3) for IMU Preintegration

@inproceedings{LeGentil2021,
	title={{Continuous Integration over SO(3) for IMU Preintegration}},
	author={{Le Gentil}, Cedric and {Vidal-Calleja}, Teresa},
  	booktitle={Robotics: Science and Systems},
	year={2021}
}

The GPMs have been presented in Gaussian Process Preintegration for Inertial-Aided Navigation Systems

@article{LeGentil2020,
	title = {{Gaussian Process Preintegration for Inertial-Aided State Estimation}},
	author = {{Le Gentil}, Cedric and Vidal-calleja, Teresa and Huang, Shoudong},
	journal = {IEEE Robotics and Automation Letters},
	number = {2},
	pages = {2108--2114},
	volume = {5},
	year = {2020} 
}
You might also like...
StarPU: A Unified Runtime System for Heterogeneous Multicore Architectures

StarPU: A Unified Runtime System for Heterogeneous Multicore Architectures coverage report What is StarPU? StarPU is a runtime system that offers supp

Pelikan is Twitter's unified cache backend

Pelikan Pelikan is Twitter's framework for developing cache services. It is: Fast: Pelikan provides high-throughput and low-latency caching solutions.

this repo will introduce you the mechanism and approach of gaussian blur
this repo will introduce you the mechanism and approach of gaussian blur

浅谈高斯模糊原理与实现 简介   早在高中,图像模糊就勾起我的兴趣:为什么近视眼看东西会模糊、透过毛玻璃的像为什么会模糊、以及win7的毛玻璃模糊特效是如何实现的,当时也有方式去查资料去实现这样的一个效果。转眼本科毕业,最近又出现一个比较热门的话题:国内安卓魔改系统的的实时模糊在高帧率下的表现,实时

Sampling Clear Sky Models using Truncated Gaussian Mixtures
Sampling Clear Sky Models using Truncated Gaussian Mixtures

Sampling Clear Sky Models using Truncated Gaussian Mixtures Overview This repository contains the source code that is part of the supplemental materia

Program will decode a PNG file into an array and apply the gaussian blur filter. Blurring an image reduces noise by taking the average RGB values around a specific pixel and setting it’s RGB to the mean values you’ve just calculated. Fast glsl deNoise spatial filter, with circular gaussian kernel, full configurable
Fast glsl deNoise spatial filter, with circular gaussian kernel, full configurable

glslSmartDeNoise Fast glsl spatial deNoise filter, with circular gaussian kernel and smart/flexible/adaptable - full configurable: Standard Deviation

A dataset containing synchronized visual, inertial and GNSS raw measurements.
A dataset containing synchronized visual, inertial and GNSS raw measurements.

GVINS-Dataset Author/Maintainer: CAO Shaozu (shaozu.cao AT gmail.com), LU Xiuyuan (xluaj AT connect.ust.hk) This repository hosts dataset collected du

Helper C++ classes to quickly preintegrate IMU measurements between SLAM keyframes

mola-imu-preintegration Integrator of IMU angular velocity readings. This repository provides: IMUIntegrator and RotationIntegrator: C++ classes to in

The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.
The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.

The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.

A lightweight version of OrcVIO that uses monocular images, inertial data, as well as bounding box measurements
A lightweight version of OrcVIO that uses monocular images, inertial data, as well as bounding box measurements

OrcVIO-Lite About Object residual constrained Visual-Inertial Odometry (OrcVIO) is a visual-inertial odometry pipeline, which is tightly coupled with

This package provides localization in a pre-built map using ICP and odometry (or the IMU measurements).
This package provides localization in a pre-built map using ICP and odometry (or the IMU measurements).

Localization using ICP in a known map Overview This package localizes the lidar sensor in a given map using the ICP algorithm. It subscribes to lidar

Experimental and Comparative Performance Measurements of High Performance Computing Based on OpenMP and MPI
Experimental and Comparative Performance Measurements of High Performance Computing Based on OpenMP and MPI

High-Performance-Computing-Experiments Experimental and Comparative Performance Measurements of High Performance Computing Based on OpenMP and MPI 实验结

A run-time C++ library for working with units of measurement and conversions between them and with string representations of units and measurements

Units What's new Some of the CMake target names have changed in the latest release, please update builds appropriately Documentation A library that pr

📚🪛 Arduino library to calibrate and improve ADC measurements with the Raspberry Pi Pico.

Arduino-Pico-Analog-Correction Arduino library to calibrate and improve ADC measurements with the Raspberry Pi Pico. Can compensate ADC offsets, calcu

a unified framework for modeling chemically reactive systems

Reaktoro Reaktoro is a unified framework for modeling chemically reactive systems. It provides methods for chemical equilibrium and kinetic calculatio

Easy to use cryptographic framework for data protection: secure messaging with forward secrecy and secure data storage. Has unified APIs across 14 platforms.
Easy to use cryptographic framework for data protection: secure messaging with forward secrecy and secure data storage. Has unified APIs across 14 platforms.

Themis provides strong, usable cryptography for busy people General purpose cryptographic library for storage and messaging for iOS (Swift, Obj-C), An

The official Open-Asset-Importer-Library Repository. Loads 40+ 3D-file-formats into one unified and clean data structure.

Open Asset Import Library (assimp) A library to import and export various 3d-model-formats including scene-post-processing to generate missing render

Convenient unified display of the most relevant technical and tag data for video and audio files.

MediaInfoLib README MediaInfo(Lib) is a convenient unified display of the most relevant technical and tag data for video and audio files. MediaInfoLib

Firebase Arduino Client Library for ESP8266 and ESP32. The unified version of Firebase-ESP8266 and Firebase-ESP32 Realtime database libraries with Cloud Firestore, Firebase and Google Cloud Storage, Cloud messaging and Cloud Functions supports.
Comments
  • Covariance for Delta R

    Covariance for Delta R

    In the paper it says "The variance of ∆R_t1^t is computed as per (13)"; does this imply that the covariance is expressed in terms of Delta R or is it described in terms of rotation angles/Euler angles?

    opened by ecbaum 0
  • Curiosity about prior bias

    Curiosity about prior bias

    Hello, I have been studying SLAM and have a question about prior bias in the code.

    I looked up the paper_metrics.cpp, and it compares the affect of bias term. Exploiting the Taylor first order expansion, it could successfully correct the position and rotation. However, if we exploited prior bias instead of Taylor expansion, its errors become larger.

    I tested using this code.

    celib::ImuPreintegration imu_preint(data, start_t, t, preint_opt, prior); celib::PreintMeas preint = imu_preint.get(0,0); std::vector acc = {acc_bias_norm[ibf], acc_bias_norm[ibf], acc_bias_norm[ibf]} prior.acc_bias = acc; celib::ImuPreintegration imu_preint2(data, start_t, t, preint_opt, prior); celib::PreintMeas preint_corrected = imu_preint2.get(0,0);

    The results :

    \scriptsize Bias norm & \scriptsize 0.01 & \scriptsize 0.05 & \scriptsize 0.1 & \scriptsize 0.2 & \scriptsize 0.4 & \scriptsize 0.6 & \scriptsize 0.8 & \scriptsize 1

    \scriptsize Slow Pos er. & \scriptsize 0.312 & \scriptsize 0.466 & \scriptsize 1.17 & \scriptsize 2.12 & \scriptsize 2.31 & \scriptsize 4.24 & \scriptsize 4.61 & \scriptsize 8.47 & \scriptsize 9.21 & \scriptsize 17 & \scriptsize 13.8 & \scriptsize 25.4 & \scriptsize 18.4 & \scriptsize 33.9 & \scriptsize 23 & \scriptsize 42.4 \scriptsize Fast Pos er. & \scriptsize 0.106 & \scriptsize 0.134 & \scriptsize 0.253 & \scriptsize 0.473 & \scriptsize 0.491 & \scriptsize 0.932 & \scriptsize 0.982 & \scriptsize 1.86 & \scriptsize 1.97 & \scriptsize 3.71 & \scriptsize 2.96 & \scriptsize 5.57 & \scriptsize 3.95 & \scriptsize 7.42 & \scriptsize 4.94 & \scriptsize 9.28

    I will be waiting for your reply.

    opened by minwoo0611 1
Owner
Centre for Autonomous Systems, University of Technology Sydney
Public and private code related to research in CAS. More information can be found below: https://github.com/UTS-CAS/Info/wiki
Centre for Autonomous Systems, University of Technology Sydney
A dataset containing synchronized visual, inertial and GNSS raw measurements.

GVINS-Dataset Author/Maintainer: CAO Shaozu (shaozu.cao AT gmail.com), LU Xiuyuan (xluaj AT connect.ust.hk) This repository hosts dataset collected du

HKUST Aerial Robotics Group 134 Dec 21, 2022
Helper C++ classes to quickly preintegrate IMU measurements between SLAM keyframes

mola-imu-preintegration Integrator of IMU angular velocity readings. This repository provides: IMUIntegrator and RotationIntegrator: C++ classes to in

The MOLA SLAM framework 12 Nov 21, 2022
The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.

The MLX90614 is an Infra Red thermometer for noncontact temperature measurements.

Shifeng Li 23 Dec 23, 2022
This package provides localization in a pre-built map using ICP and odometry (or the IMU measurements).

Localization using ICP in a known map Overview This package localizes the lidar sensor in a given map using the ICP algorithm. It subscribes to lidar

Robotic Systems Lab - Legged Robotics at ETH Zürich 131 Jan 3, 2023
A run-time C++ library for working with units of measurement and conversions between them and with string representations of units and measurements

Units What's new Some of the CMake target names have changed in the latest release, please update builds appropriately Documentation A library that pr

Lawrence Livermore National Laboratory 112 Dec 14, 2022
📚🪛 Arduino library to calibrate and improve ADC measurements with the Raspberry Pi Pico.

Arduino-Pico-Analog-Correction Arduino library to calibrate and improve ADC measurements with the Raspberry Pi Pico. Can compensate ADC offsets, calcu

NuclearPhoenix 11 Jan 3, 2023
null 313 Dec 31, 2022
KernInfra, a unified kernel operation framework

KernInfra KernInfra is a developer-friendly kernel read-write framework. Why KernInfra KernInfra is built to address the following engineering issues:

null 30 Dec 14, 2022
Provide a unified trading framework and connectors to popular trading venues

Boost.connector Provide a unified trading framework and connectors to popular trading venues This is currently NOT an official Boost library. Introduc

Richard Hodges 6 Nov 24, 2021
XTAO Unified Distributed Storage

Anna - A branch project from CEPH Anna is a XTAO project branched from CEPH distributed storage. CEPH is a nice opensource project for unified distrib

XTAO Technolgy 3 Nov 12, 2021