We first develop a customized function to read 2D Intel dataset from G2O format and output poses and edges. These poses and edges are used in later parts for 2D and 3D SLAM.

Below information summarizes the file format for the 2D SLAM vertices and edges, which is helpful to start with reading and understanding the data.

The vertex information represents the current state of the parameters.

VERTEX_SE2 i x y theta

Specifies a 2D robot pose x_i = (x, y, theta)^T where (x,y) represents the translation and theta the rotation.

VERTEX_XY i x y

Specifies a 2D landmark which is located at p_i = (x, y)^T.

The edges encode the measurements obtained by the robot.

The odometry of a robot connects subsequent vertices with a relative transformation which specifies how the robot moved according to its measurements. Additionally, non sequential poses can be related to each other by matching their observations.

EDGE_SE2 i j x y theta info(x, y, theta)

Where z_ij = (x, y, theta)^T is the measurement moving from x_i to x_j, i.e. x_j = x_i \oplus z_ij

Basically:

Batch Solution: A batch solution means when we construct the entire graph and solve it at the end altogether. We first load data/input_INTEL_g2o.g2o and construct a 2D nonlinear factor graph using GTSAM, using the Gauss-Newton solver.

Incremental Solution: Here, we use ISAM2 solver to optimize the trajectory incrementally i.e. as we build the graph gradually.

Here we first develop a function to read 3D Garage G2O file from G2O format and output poses and edges. Basically:

Batch Solution: We first load data/parking-garage.g2o and then construct a 3D nonlinear factor graph using GTSAM. Use the Gauss-Newton solver.

View 1	View 2	View 3

Incremental Solution: We Use ISAM2 solver to optimize the trajectory incrementally, then we visualize and compare the optimized trajectory against the initial trajectory.

View 1	View 2	View 3