Pose Graph Optimization

The algorithm optimizes pose graphs using motion averaging or graph-based least-squares solver.

Input

The input can be a set of point clouds or nothing. In the first case the algorithm will try to construct a pose graph using ICP. In the second case, it will expect the user to load a pose graph.

Output

Updates the poses, input to the graph.

Description

The algorithm has two methods to input a pose graph: from point clouds, using iterative closest point (ICP) to construct the graph; or from the pose graph loaded from the disk.

• Using Point Clouds to Construct the Pose Graph:

  This mode requires a set of point clouds as the input. The pose graph is constructed using ICP while the first point cloud in the set is selected as the origin. This is the default mode, and is selected unless Load pose graph option is checked. This mode also provides an interface to tune ICP parameters such as:

  • Matching strategy:

    Closest point matching strategies.

    • Nearest neighbor: Uses the nearest point as the match for each point. Usable for all kinds of point clouds.
    • Projective: Assumes that the point clouds are generated by cameras. Uses the camera intrinsics to project the source point cloud to the target and matches the points at the same position.

  • Number of iterations:

    Number of iterations to perform for ICP.

  • Min. overlap:

    Apriori knowledge on point cloud overlap ratio.

  • Max. matching distance:

    Maximum distance between two points to be considered as a match. The unit is mm.

  • Max. matching angle:

    Maximum angular distance between two points to be considered as a match. The unit is degrees.

  • Compute bidirectional:

    If set true, does the alignment from both directions for two point clouds.

  • Recompute overlap:

    If set true, recomputes the overlap at every iteration.

• Using the Pose Graph Saved on the Disk:

  When Load pose graph is checked, the designated pose graph can be loaded from the disk by clicking on Load Pose Graph. The file is assumed to be of .g2o extension and in the format as examples in: https://lucacarlone.mit.edu/datasets/. If Load with information matrices is unchecked, their values are replaced with identity matrices.

General Pose Graph Optimization Parameters:

• Max iterations:

  Maximum number of iterations to run the pose graph optimization unless the convergence is detected via the Gain threshold.

• Gain threshold:

  Convergence criteria for the pose error. If the new error’s ratio to the previous one is below the threshold, the optimization is considered to be converged.

• Method:

  • Motion Averaging: Works only with point clouds as inputs; does not work with loaded pose graphs. Iteratively runs ICP to compute relative motion between the point clouds and minimizes the average pose error by updating their global poses. These poses are set inside the matrix property of the point clouds.

  • Least Squares Solution: Runs a least squares optimization to optimize the poses. Can be used on input point clouds, similar to Motion Averaging or with loaded pose graphs. Least Squares Solution Parameters:

    • Robust kernel:

      Robust kernel for the pose error.

    • Robust kernel error window:

      Error window to be robust against within the Robust kernel.