TORO - Tree-based netwORk Optimizer

TORO is an optimization approach for constraint-network. It provides an efficient, gradient descent-based error minimization procedure. There is a 2D and a 3D version of TORO available.
Further information

Giorgio Grisetti; Cyrill Stachniss; Slawomir Grzonka; Wolfram Burgard;

Get the Source Code!

Long Description
Recently, Olson et al. presented a novel approach to solve the graph-based SLAM problem by applying stochastic gradient descent to minimize the error introduced by constraints. TORO is an extension of Olson's algorithm. It applies a tree parameterization of the nodes in the graph that significantly improves the performance and enables a robot to cope with arbitrary network topologies. The latter allows us to bound the complexity of the algorithm to the size of the mapped area and not to the length of the trajectory.

Example Images

Correcting a sphere in 3D

A corrected network with 200k nodes

Small video (5k nodes/30k constraints)

Input Data
Nodes and edges of a graph.

Logfile Format
A set of simple text messages to represent nodes and edges of the graph. Note that examples files are in the repository. See folder data.

Format of the 2D graph files:

Every line in the file specifies either one vertex or one edge

The vertices are specified as follws: VERTEX2 id x y orientation (A 2D node in the graph)

EDGE2 observed_vertex_id observing_vertex_id forward sideward rotate inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr (A 2D-edge in the graph. inf_xx are the information matrix entries of the constraint)

EQUIV id1 id2 (Equivalence constraints between nodes. It merges the node id1 and id2 wrt to the constraint between both vertices.)

Format of the 3D graph files:

Every line in the file specifies either one vertex or one edge

The vertices are specified as follws: VETREX3 x y z phi theta psi

The edges are specified as follows: EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 (the information matrix is specified via its upper triangular block that means 21 values).

Type of Map
Graphs (nodes and edge)

Hardware/Software Requirements
Developed under Linux, GCC 4.0.2 but should work anywhere where GCC runs. 07/2008 Patch for compatability with gcc 4.3.x as well as MacOSX (thanks to P. Checchin)

Papers Describing the Approach
Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard: Non-linear Constraint Network Optimization for Efficient Map Learning., IEEE Transactions on Intelligent Transportation Systems, Volume 10, Issue 3, Pages 428-439, 2009 (link)

Grisetti Giorgio, Slawomir Grzonka, Cyrill Stachniss, Patrick Pfaff, and Wolfram Burgard: Efficient Estimation of Accurate Maximum Likelihood Maps in 3D., IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2007 (link)

Giorgio Grisetti, Cyrill Stachniss, Slawomir Grzonka, and Wolfram Burgard: A Tree Parameterization for Efficiently Computing Maximum Likelihood Maps using Gradient Descent., Robotics: Science and Systems (RSS), 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

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

Further Information
C++ code, quite compact, efficient, and stand-alone

*** is not responsible for the content of this webpage ***
*** Copyright and V.i.S.d.P.: Giorgio Grisetti; Cyrill Stachniss; Slawomir Grzonka; Wolfram Burgard; ***