Robotic Navigation in Ceres

This repo contains a set of tools to help implement batch estimators and sliding-window filters for robotic state estimation using Ceres. It contains a set of common factors (such as odometry, vision, and preintegrated IMU factors) used in state estimation, along with common Lie group state definitions such as poses and extended poses. Additionally, both "left" and "right" state definitions are provided for Lie group states, allowing for the user to pick the error definition for Lie group states that best suits the problem at hand.

Prerequisites

This repo has been tested on Ubuntu 20.04 and has the following requirements:

• Ceres Solver 2.0.0 - can be built from source using

$ git clone [email protected]:ceres-solver/ceres-solver.git && cd ceres-solver
$ git checkout 2.0.0
$ mkdir build && cd build
$ cmake ..
$ make 
$ make install

• Eigen3 >= 3.3,

$ sudo apt install libeigen3-dev

• glog,

$ sudo apt install libgoogle-glog-dev

• Catch2 - to run the unit tests. Catch2 can be installed from source using

$ git clone https://github.com/catchorg/Catch2.git
$ cd Catch2
$ cmake -B build -S . -DBUILD_TESTING=OFF
$ sudo cmake --build build/ --target install

• Boost - for filesystem and program options. Can be installed using

$ sudo apt install libboost-all-dev

The dependencies can be also installed using the provided scripts/install_dependencies.sh script.

Build

After installing the dependencies, the ceres_nav library, tests, and examples can be built using

git clone https://github.com/mitchellcohen3/ceres_nav/
cd ceres_nav
mkdir build && cd build
cmake ..
make 
make install

Tests

To ensure that the build functions correctly, tests can be run after installing Catch2:

cd ceres_nav/build
ctest

Example Usage

An simulated example of running batch estimators and sliding window filters for GPS/IMU fusion is provided in gps_imu_example.cpp. In this example, the state to be estimated is the IMU state, consisting of

• Orientation, $\mathbf{C}_{ab} \in SO(3)$,
• Inertial velocity, $\mathbf{v}_a \in \mathbb{R}^3$
• Inertial position, $\mathbf{r}_a \in \mathbb{R}^3$,
• IMU biases $\mathbf{b}_b \in \mathbb{R}^6$.

This example showcases how this library can be utilized to test various state representations for estimation problems. For example, the orientation, velocity, and position can be parameterized as an element of $SE_2(3)$ as

$$ \mathbf{T}_{ab} = \left[\begin{array}{ccc} \mathbf{C}_{ab} & \mathbf{v}_a^{zw} & \mathbf{r}_a^{zw} \\\ \mathbf{0} & 1 & 0 \\\ \mathbf{0} & 0 & 1 \end{array}\right] \in SE_2(3), $$

where either a "left" or "right" perturbation scheme can be used. Additionally, a user can select a more traditional state representation, $SO(3) \times \mathbb{R}^3 \times \mathbb{R}^3$, where a multiplicative error is used for the orientation and additive errors are used for the velocity and position.

To see how to run estimators using these various configurations, see the script examples/python/run_gps_imu_fusion.py, which can be run as

$ python3 examples/python/run_gps_imu_fusion.py \ 
  --lie_direction "left" \
  --state_representation "decoupled"

which can be run for both left and right Lie directions or and both $SE_2(3)$ and decoupled state representations.

This additionally depends on the navlie Python library, used to generate simulated IMU and GPS measurements, and additionally evaluate the results of the optimization. To install navlie, run

$ git clone https://github.com/decargroup/navlie
$ cd navlie
$ pip install -e .

Implemented State Types and Factors

The following state types are currently implemented in the library:

• $\mathbb{R}^n$ states used to represent generic vector states,
• $SO(3)$ states used to represent 3D orientations,
• $SE(3)$ states used to represent 3D poses (orientation + position),
• $SE_2(3)$ states used to represent extended poses (orientation + velocity + position),

The following factors are currently implemented in the library:

• AbsolutePositionFactor for absolute position measurements from, for example, a GPS.
• RelativePoseFactor - used to represent relative pose measurements (e.g., from visual odometry or LiDAR odometry),
• RelativeLandmarkFactor - used to represent 3D landmarks measurements in the body frame of a sensor,
• PreintegratedIMUFactor - represents a preintegrated IMU measurement between two states.

Acknowledgements

The marginalization code in this repo is adapted from the libRSF library, another great library for robust estimation using Ceres. Parts of the structure of the repo are also inspired by libRSF. libRSF only supports autodiff cost functions, while this repo supports traditional Ceres cost functions with analytic differentiation and provides tools to additionally check Jacobians numerically. The use of analytical derivatives becomes important when implementing sliding window filters for specific problems, such as visual-inertial odometry where the evaluation point of the Jacobians must be artificially modified to ensure consistency after marginalization.

Disclaimer

Note: this repo is still very much a work in progress, and more complex examples will be added soon! Contributions and thoughts are always welcome :)