gtsam-quadrics 0.3.0
Quadrics for GTSAM

<p align=center><strong>~ Please see our <a href="https://github.com/qcr/quadricslam">QuadricSLAM repository</a> for examples of full SLAM systems~</strong></p>

# GTSAM Quadrics: quadric landmarks for GTSAM

[![QUT Centre for Robotics Open Source](https://github.com/qcr/qcr.github.io/raw/master/misc/badge.svg)](https://qcr.github.io)
[![Best of ACRV Repository](https://img.shields.io/badge/collection-best--of--acrv-%23a31b2a)](https://roboticvision.org/best-of-acrv)
![Primary language](https://img.shields.io/github/languages/top/qcr/gtsam-quadrics)
[![PyPI package](https://img.shields.io/pypi/pyversions/gtsam-quadrics)](https://pypi.org/project/gtsam-quadrics/)
[![License](https://img.shields.io/github/license/qcr/gtsam-quadrics)](./LICENSE.txt)

This repository contains an extension to the popular [Georgia Tech Smoothing and Mapping (GTSAM)](https://github.com/borglab/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](https://github.com/qcr/quadricslam), where we use quadrics for simultaneous localisation and mapping (SLAM) problems.

![Demonstrations of GTSAM Quadrics in action](/doc/gtsam_quadrics.png)

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](#citing-our-work) if you use QuadricSLAM in your own research.

## Installation

Pre-build wheels of this library are [available on PyPI](https://pypi.org/project/gtsam-quadrics/) 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`](/setup.py), which uses CMake to build a custom C++ extension bound using both [PyBind11](https://github.com/pybind/pybind11) and Georgia Tech's [wrap meta-library](https://github.com/borglab/wrap).

### 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:

| **Target name** | **Description**                                |
| :-------------- | :--------------------------------------------- |
| check           | compile and run optional unit tests            |
| examples        | compiles the c++ examples                      |
| doc             | generates the doxygen documentation            |
| doc_clean       | removes the doxygen documentation              |
| install         | installs the gtsam_quadrics c++/python library |

_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:

```python
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](https://ieeexplore.ieee.org/document/8440105):

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](https://arxiv.org/abs/1804.04011).

```bibtex
@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},
}
```