Treemap is an algorithm for feature based Gaussian SLAM. Actually it is an algorithm for incremental probabilistic inference in a high dimensional Gaussian defined as the product of many low dimensional Gaussians (incremental least square). Treemap can handle different variants of SLAM. Everything, that's specific to a SLAM variant or even to SLAM as a problem is contained in a small driver layer that can be adapted by the user.

This work has been supported by the Deutsche Forschungs Gemeinschaft, grant SFB-TR 8 / Spatial Cognition.
Further information

Udo Frese;

Get the Source Code via SVN
svn co

Long Description
The main contribution of treemap is extremely efficient Gaussian inference. For instance, it can compute a full probabilistic update on a map with 1032271 2D features in 442ms. So, to put it in a nutshell: Treemap does the same job as Levenberg-Marquardt, only much faster.

The treemap algorithm as well as this implementation consists of two layers. The treemap backend contains nearly all the difficulties of the algorithm. It performs inference in a high dimensional Gaussian defined as the product of many low-dimensional Gaussians. The low-dimensional Gaussians correspond to measurements. The second layer, the treemap driver converts the original measurements into these low dimensional Gaussians, basically by linearizing the measurement equations and passes them to the treemap backend. It also defines some application dependent approximation policy. The backend then incrementally computes the mean of the resulting Gaussian, which is in turn converted by the treemap driver into a map estimate. We have implemented drivers for 2D feature based SLAM, 2D/3DOF pose relation based SLAM and 3D/6DOF feature based SLAM without odometry.

So, as a user, you have to provide a set of measurements as a vector of real numbers, i.e. you have to do your own feature detection on the raw sensor data. And you have to provide the identity of features to which the measurements refer (data-association). You also have to provide covariance (uncertainties) for these measurements. This is often a hand tuned parameter. If you are doing one of the above mentioned SLAM variants, that's it. Otherwise you also have to implement your own driver that linearizes measurement equations and defines an approximation policy for the back-end.

Treemap provides a map estimate. It could in principle provide covariance information which would be useful for data-association. However, this is not implemented yet.

Example Images

Video showing how treemap closes a loop with a million landmarks.

Video showing a mobile robot mapping a building with treemap.

Two layered architecture with treemap back-end and driver.

Input Data
The input to treemap are measurements with covariance and known data-association. For 2D feature SLAM the input is the observed 2D position of the feature relative to the robot, a covariance (uncertainty) of that position and the id of the feature observed. Additionally for every step of the robot the 2D/3DOF pose of the robot after the step relative to the pose before the step must be given with covariance. For 2D pose relation based SLAM, a graph of 2D/3DOF poses is given, where each node corresponds to the pose of the robot at some point of time and an edge defines a measurement for one pose relative to another pose obtained from scan-matching or odometry. For 3D/6DOF SLAM the input is a 3D feature position relative to the robot with the identity of the feature. The current driver for 3D SLAM does not incorporate inertial data or any form of odometry.

Logfile Format
Proprietary human readable ASCII file.

Type of Map
The resulting map is a vector of feature positions (2D/3D feature based SLAM) or robot poses (2D/3DOF pose relation SLAM).

Hardware/Software Requirements
The code is entirely written in C++. It has been checked to compile with GCC 4.1.2. CMake is needed as a build tool, LAPACK for numerical computation, and gfortran for linking the LAPACK package.

Papers Describing the Approach
U. Frese, L. Schroeder : Closing a Million-Landmarks Loop, Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing , 2006 (link)

U. Frese: Efficient 6-DOF SLAM with Treemap as a Generic Backend, Proceedings of the Internation Conference on Robotics and Automation, Rome , 2007 (link)

License Information
This software is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
The authors allow the users of to use and modify the source code for their own research. Any commercial application, redistribution, etc has to be arranged between users and authors individually and is not covered by

Treemap is licenced under the Creative Commons (Attribution-NonCommercial-ShareAlike).

Further Information
The code contains a simulator for extremely large 2D SLAM datasets that have a more realistic topology than mowing the lawn. This may have an interest of its own.

*** is not responsible for the content of this webpage ***
*** Copyright and V.i.S.d.P.: Udo Frese; ***