```
In [1]:
```from graphslam.graph import Graph
from graphslam.load import load_g2o_se2

For a complete derivation of the Graph SLAM algorithm, please see graphSLAM_formulation.pdf.

This notebook illustrates the iterative optimization of a real-world $SE(2)$ dataset. The code can be found in the `graphslam`

folder. For simplicity, numerical differentiation is used in lieu of analytic Jacobians. This code originated from the python-graphslam repo, which is a full-featured Graph SLAM solver. The dataset in this example is used with permission from Luca Carlone and was downloaded from his website.

```
In [2]:
```g = load_g2o_se2("data/input_INTEL.g2o")
print("Number of edges: {}".format(len(g._edges)))
print("Number of vertices: {}".format(len(g._vertices)))

```
```

```
In [3]:
```g.plot(title=r"Original ($\chi^2 = {:.0f}$)".format(g.calc_chi2()))

```
```

Each edge in this dataset is a constraint that compares the measured $SE(2)$ transformation between two poses to the expected $SE(2)$ transformation between them, as computed using the current pose estimates. These edges can be classified into two categories:

- Odometry edges constrain two consecutive vertices, and the measurement for the $SE(2)$ transformation comes directly from odometry data.
- Scan-matching edges constrain two non-consecutive vertices. These scan matches can be computed using, for example, 2-D LiDAR data or landmarks; the details of how these contstraints are determined is beyond the scope of this example. This is often referred to as
*loop closure*in the Graph SLAM literature.

We can easily parse out the two different types of edges present in this dataset and plot them.

```
In [4]:
```def parse_edges(g):
"""Split the graph `g` into two graphs, one with only odometry edges and the other with only scan-matching edges.
Parameters
----------
g : graphslam.graph.Graph
The input graph
Returns
-------
g_odom : graphslam.graph.Graph
A graph consisting of the vertices and odometry edges from `g`
g_scan : graphslam.graph.Graph
A graph consisting of the vertices and scan-matching edges from `g`
"""
edges_odom = []
edges_scan = []
for e in g._edges:
if abs(e.vertex_ids[1] - e.vertex_ids[0]) == 1:
edges_odom.append(e)
else:
edges_scan.append(e)
g_odom = Graph(edges_odom, g._vertices)
g_scan = Graph(edges_scan, g._vertices)
return g_odom, g_scan

```
In [5]:
```g_odom, g_scan = parse_edges(g)
print("Number of odometry edges: {:4d}".format(len(g_odom._edges)))
print("Number of scan-matching edges: {:4d}".format(len(g_scan._edges)))
print("\nχ^2 error from odometry edges: {:11.3f}".format(g_odom.calc_chi2()))
print("χ^2 error from scan-matching edges: {:11.3f}".format(g_scan.calc_chi2()))

```
```

```
In [6]:
```g_odom.plot(title="Odometry edges")

```
```

```
In [7]:
```g_scan.plot(title="Scan-matching edges")

```
```

Initially, the pose estimates are consistent with the collected odometry measurements, and the odometry edges contribute almost zero towards the $\chi^2$ error. However, there are large discrepancies between the scan-matching constraints and the initial pose estimates. This is not surprising, since small errors in odometry readings that are propagated over time can lead to large errors in the robot's trajectory. What makes Graph SLAM effective is that it allows incorporation of multiple different data sources into a single optimization problem.

```
In [8]:
```g.optimize()

```
```

```
In [9]:
```g.plot(title="Optimized")

```
```

```
In [10]:
```print("\nχ^2 error from odometry edges: {:7.3f}".format(g_odom.calc_chi2()))
print("χ^2 error from scan-matching edges: {:7.3f}".format(g_scan.calc_chi2()))

```
```

```
In [11]:
```g_odom.plot(title="Odometry edges")

```
```

```
In [12]:
```g_scan.plot(title="Scan-matching edges")

```
```