This repository contains an extension to the popular GTSAM factor graph optimization 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 for both C++ and Python and are designed to be used in conjunction with GTSAM. Also provided are a number of C++ and Python examples that demonstrate how quadric landmarks can be used in the SLAM context, alongside doxygen documentation and unit tests.
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.
GTSAM Quadrics contains both C++ libraries, and Python wrappers for use in Python. We offer a number of different install methods, from single step methods to more involved depending on your desired use case.
Python wrappers can be installed via one of the following options:
- Through our Conda package: single command installs everything including system dependencies (recommended)
- Through our pip package: single command installs GTSAM and GTSAM Quadrics Python modules and Python dependences, but you take care of system dependencies
- Directly from source: allows easy editing and extension of our code, but you take care of building and all dependencies
Or you can use the C++ libraries directly by:
- Building with CMake: builds the library from scratch, and allows you to install into your system library paths
Please note that for all methods except the Conda method, you must have the following system dependencies installed beforehand:
- A C++ compiler: e.g.
sudo apt install build-essential
- CMake >= 3.0:
sudo apt install cmake
- Boost C++ libraries >= 1.65:
sudo apt install libboost-all-dev
- METIS matrix library:
sudo apt install libmetis-dev
TODO: actually make feedstock
The only requirement is that you have Conda installed on your system. We provide Conda packages through Conda Forge, which recommends adding their channel globally with strict priority:
conda config --add channels conda-forge
conda config --set channel_priority strict
Once you have access to the conda-forge
channel, GTSAM Quadrics is installed by running the following from inside a Conda environment:
conda install gtsam_quadrics
You can see a list of our Conda dependencies in the feedstock recipe for GTSAM Quadrics.
Pre-built Python modules for both GTSAM Quadrics and GTSAM can be installed via:
pip install gtsam_quadrics
We provide pre-built Python wheels, which have libraries like an appropriate version of Boost built-in. You shouldn't need to install system dependencies when installing via Pip, unless you are installing on an unsupported platform.
Installing from source is very similar to the pip
method above, accept we install from a local copy. This means it's important to ensure the system dependencies described above are installed.
Then clone the repository, and initialise the gtsam
submodule:
git clone --recurse-submodules https://github.com/best-of-acrv/gtsam-quadrics
Enter the gtsam_quadrics
directory, and simply install via pip
(the build process will take a while):
pip install .
This process will build the library from scratch using CMake. It is very similiar to the "from source" method above, we just use CMake directly to build instead of invoking it as part of the pip
install process.
Ensure required system dependencies are installed, then clone with the gtsam
submodule initialised:
git clone --recurse-submodules https://github.com/best-of-acrv/gtsam-quadrics
Create an out-of-source build directory:
cd gtsam_quadrics
mkdir build
cd build
Run the configuration and generation CMake steps, optionally building the Python wrapper using the BUILD_PYTHON_WRAP
variable:
cmake -DBUILD_PYTHON_WRAP=ON ..
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
)
For example, to install the library into system paths run:
cmake --build . --target install
TODO check these all still work???
GTSAM Quadrics and GTSAM can be used like native Python packages. Below are some examples to help get you started with using GTSAM Quadrics:
# Note: at this stage you MUST import gtsam before importing 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)
TODO check these are up-to-date
- High-level SLAM front-end (akin to ORBSLAM2)
- Support for expmap/logmap
- Tools to visualize and evaluate quadric landmarks
How is the GTSAM dependency handled: This repository includes GTSAM as a submodule, so you can see precisely which version we build against (currently it is post 4.1.1 due to Python wrapper changes). The GTSAM Quadrics library is then built and linked against this copy of GTSAM to ensure compatibility. The Python module is also linked against the GTSAM Python module built from our submodule.
TODO need to check we handle this correctly If using GTSAM 4.0.3 or exponential-map rotations: gtsam 4.0.3 moved to exponential map by default to parametrize rotations. The analytical derivatives we've calculated from this library are based on the cayley transform. Please either select cayley rotations in the gtsam CMakelists or use numerical derivatives (defined in boundingboxfactor.cpp).
If you plan to use gtsam_quadrics in C++: You can find the installed C++ headers using the cmake command find_package(GTSAM_QUADRICS REQUIRED)
which will load GTSAM_QUADRICS_INCLUDE_DIR
. The default header installation is /usr/local/include/gtsam_quadrics/
, and by default library is installed to /usr/local/lib/libgtsam_quadrics.so
.
Adding Quadrics to gtsam::Values
: When using the python interface, ConstrainedDualQuadrics can be added or retrieved from Values using the following. Since GTSAM 4.0 the python interface for Values manually specializes each type. When supported by GTSAM, we plan to derive the gtsam::Values class and add the insert/at methods for ConstrainedDualQuadric.
quadric.addToValues(values, key)
quadric = gtsam_quadrics.ConstrainedDualQuadric.getFromValues(values, key)
TODO ensure this is still relevant?
AttributeError: module 'gtsam_quadrics' has no attribute 'ConstrainedDualQuadric'
: If you import gtsam_quadrics and find it does not contain any attributes, or receive the above, ensure that gtsam_quadrics is built with BUILD_PYTHON_WRAP set ON, the correct python version is used, and that the generated gtsam_quadrics.so shared library is on your PYTHONPATH. I.e, if you have installed gtsam_quadrics, that you have the following line in your ~/.bashrc
export PYTHONPATH=$PYTHONPATH:/usr/local/cython/gtsam_quadrics
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},
}