~ Please see our QuadricSLAM repository for examples of full SLAM systems~

GTSAM Quadrics: quadric landmarks for GTSAM

This repository contains an extension to the popular Georgia Tech Smoothing and Mapping (GTSAM) factor graph optimisation library. We introduce constrained dual quadrics as GTSAM variables, and support the estimation of the quadric parameters using 2-D bounding box measurements. These tools are available in both C++ and Python, and are designed to be used in conjunction with GTSAM. The extensions power our QuadricSLAM library, where we use quadrics for simultaneous localisation and mapping (SLAM) problems.

We expect this repository to be active and continually improved upon. If you have any feature requests or experience any bugs, don't hesitate to let us know. Our code is free to use, and licensed under BSD-3. We simply ask that you cite our work if you use QuadricSLAM in your own research.

Installation

Pre-build wheels of this library are available on PyPI for most Linux systems, as well as source distributions. Install the library with:

pip install gtsam_quadrics

The Python library is built from a custom setup.py, which uses CMake to build a custom C++ extension bound using both PyBind11 and Georgia Tech's wrap meta-library.

Building from source

You can build from source if you want closer access to the C++ libraries, or are having trouble finding a pre-compiled wheel for your system. There are two levels you can build the package from source: the Python level using pip, and C++ level using CMake.

All building from source methods expect the following system dependencies to be available:

• A C++ compiler
• CMake >= 3.0
• Boost C++ libraries >= 1.65
• METIS matrix library

Instructions for installing these dependencies vary across Linux systems, but the following should be sufficient on a relatively recent Ubuntu version:

sudo apt install build-essential cmake libboost-all-dev libmetis-dev

Building the Python package from source

Installing from source is very similar to the pip method above, accept installation is from a local copy:

1. Clone the repository, and initialise the gtsam submodule:

   git clone --recurse-submodules https://github.com/best-of-acrv/gtsam-quadrics
   

2. Enter the gtsam_quadrics directory, and simply install via pip (the build process will take a while):

   pip install .
   

Building the C++ package with CMake

1. Clone the repository, and initialise the gtsam submodule:

   git clone --recurse-submodules https://github.com/best-of-acrv/gtsam-quadrics
   

2. Create an out-of-source build directory:

   cd gtsam_quadrics
   mkdir build
   cd build
   

3. Run the configuration and generation CMake steps, optionally building the Python wrapper using the BUILD_PYTHON_WRAP variable:

   cmake -DBUILD_PYTHON_WRAP=ON ..
   

4. Run the build step:

   cmake --build . -j$(nproc)
   

Then optionally run any of the other supported targets as described below:

Note: documentation requires Doxygen (sudo apt install doxygen) and epstopdf (sudo apt install texlive-font-utils)

Using the GTSAM Quadrics and GTSAM Python APIs

GTSAM Quadrics and GTSAM can be used like native Python packages. Below are some examples to help get you started with using GTSAM Quadrics:

import gtsam
import gtsam_quadrics
import numpy as np

# setup constants
pose_key = int(gtsam.symbol(ord('x'), 0))
quadric_key = int(gtsam.symbol(ord('q'), 5))

# create calibration
calibration = gtsam.Cal3_S2(525.0, 525.0, 0.0, 160.0, 120.0)

# create graph/values
graph = gtsam.NonlinearFactorGraph()
values = gtsam.Values()

# create noise model (SD=10)
bbox_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([10]*4, dtype=np.float))

# create quadric landmark (pose=eye(4), radii=[1,2,3])
initial_quadric = gtsam_quadrics.ConstrainedDualQuadric(gtsam.Pose3(), np.array([1.,2.,3.]))

# create bounding-box measurement (xmin,ymin,xmax,ymax)
bounds = gtsam_quadrics.AlignedBox2(15,12,25,50)

# create bounding-box factor
bbf = gtsam_quadrics.BoundingBoxFactor(bounds, calibration, pose_key, quadric_key, bbox_noise)

# add landmark to values
initial_quadric.addToValues(values, quadric_key)

# add bbf to graph
graph.add(bbf)


# get quadric estimate from values (assuming the values have changed)
quadric_estimate = gtsam_quadrics.ConstrainedDualQuadric.getFromValues(values, quadric_key)

Citing our work

If you are using this library in academic work, please cite the publication:

L. Nicholson, M. Milford and N. Sünderhauf, "QuadricSLAM: Dual Quadrics From Object Detections as Landmarks in Object-Oriented SLAM," in IEEE Robotics and Automation Letters, vol. 4, no. 1, pp. 1-8, Jan. 2019, doi: 10.1109/LRA.2018.2866205. PDF.

@article{nicholson2019,
  title={QuadricSLAM: Dual Quadrics From Object Detections as Landmarks in Object-Oriented SLAM},
  author={Nicholson, Lachlan and Milford, Michael and Sünderhauf, Niko},
  journal={IEEE Robotics and Automation Letters},
  year={2019},
}