LAGO: Linear Approximation for Graph Optimization


NEW! Lago is now implemented also in the GTSAM and g2o libraries!


This code implements a fast accurate linear approximation of the maximum likelihood pose graph configuration. LAGO does not require any initial guess for optimization and has a very simple implementation. The code relies on the CSparse library for sparse matrix manipulation. The source code was developed under Linux/Unix with GCC 4.05, but should also work under Windows with slight modifications.


Download the code:

Zip of our c++ implementation


Papers describing LAGO:

  • L. Carlone, R.Aragues, J.A.Castellanos, and B.Bona, “fast and accurate approximation for pose graph optimization, IJRR, 2014.

  • L. Carlone, R.Aragues, J.A.Castellanos, and B.Bona,linear approximation for graph based simultaneous localization and mappingin Proc. of the Int. Conf. Robotics: Science and Systems, PDF2011.

  • L.Carlone, R.Aragues, J.A.Castellanos, and B.Bona,first order solution to simultaneous localization and mapping with graphical modelsin Proc. of the Int. Conf. on Robotics and Automation, pp. 1764-1771, ISBN:9781612843865, PDF, 2011.


Instructions of use:

  1. Download and install CSparse
  2. Download and unzip our source code
  3. Modify our makefile, specifying the path to your Csparse folder
  4. Compile the code
  5. Run the code with the following syntax (Linux):     ./lago   input_file_name [d]   (adding the optional argument “d” will  
        run the code in debug mode, which will output additional files with intermediate results)
  6. The resulting pose graph estimate is written on the file “output_graph.txt” (the file is created in the same folder of the


Format of input and output files:

The current implementation of LAGO uses the same input file format of TORO (a detailed description of TORO format can be found here). LAGO requires that the nodes' indices start from zero and increase along robot trajectory (i.e., they follow a chronological order). Since LAGO requires no initial guess, the list of vertices in the input file is only kept for  compatibility with other approaches. For simplicity, also the edges are assumed in a specific order: odometric edges, followed by loop closing edges. Therefore, in a graph with n+1 nodes, the first n edges connect nodes with consecutive indices.

The format of the output file is similar to the one of the input file: a list of vertices, followed by a list of edges. The edges are exactly the same as the input file. The lines starting with “VERTEX2”, instead, contain the poses estimated by LAGO (while the corresponding lines in the input file contain the initial guess for that poses).