In this example, we correct a largely bent map with loop closing, edge refinement, and plane constraints, then merge it with another map.

Example graph files:

• centrair_100_corrected.tar.gz (18MB)
• centrair_101_graph.tar.gz (200MB)

Index

1. Loading Map Data
2. Manual Loop Closing
3. Automatic Loop Closing
4. Edge Refinement
5. Plane Constraints
6. Multiple Map Merging

Loading Map Data

Open "centrair_101_graph":

Menubar -> File -> Open -> New map -> Choose "centrair_101_graph"

You see a largely distorted map generated from odometry data given by LeGO-LOAM (Disable z_clipping in the Shader setting window to see the entire map). Let‘s correct this using several map correction features. 

Manual Loop Closing

Choose two keyframes to make a loop constraint.

Right click a keyframe -> Loop begin -> Right click another one -> Loop end

You‘ll see a sub-window to estimate the relative pose between the selected keyframes.

First, make an initial guess using FPFH-based pose estimation: Auto align -> OK This global registration method may fail sometimes. If it fails, align the keyframes roughly using "PX, PY, PZ, RX, RY, RZ" pose controllers. Once the keyframes are roughly aligned, perform scan matching for fine registration: Scan matching -> OK Check if the alignment is fine, and then click Add edge to add a relative pose edge to the pose graph.

Note: see PCL documentation(FPFH, Registration) for the details of the parameters.

Once the edge is added to the pose graph, graph optimization is performed and you‘ll see that the global consistency of the map is greatly improved.

Automatic Loop Closing

To improve the local consistency of the map (resolving doubled walls), perform automatic loop closing.

Menubar -> Graph -> Automatic loop detection -> Start

During this process, keyframes are compared with their neighbor keyframes, and if they match well each other, an edge is added between them. Once all the keyframes are evaluated, press Stop to terminate the automatic loop detection.

Pose Edge Refinement

There are still large distortion caused by some errors in odometry data. Improve the map quality by refining the relative pose constraints.

Menubar -> Graph -> Edge refinement -> Start

This process chooses an edge with a large error and performs scan matching between the keyframes connected with the edge. We use the relative pose estimated by pose graph optimization as the initial guess. Since the estimated relative pose is expected to be close to the true pose, the scan matching with the good initial guess likely to converge to a good result. If the fitness score between the keyframes is improved after the re-scan matching process, the relative pose constraint is updated with the new pose.

Plane Constraints

We still see global distortion on the map. Let us create several plane constraints to make the floor plane flat: Right click a point on the ground -> Plane detection To extract plane candidate points, tweak the region growing parameters and click Perform. Orange points are points to be used for the following plane detection process. Check if they are all lying on the plane. If not, tweak "Smoothness threshold" and re-press Perform. Then, to perform RANSAC-based plane detection, click Detect. The rainbow points represent the points detected as the inliers of the plane. If they are all on the plane to be detected, press Add edge to add a plane vertex to the pose graph.

To make the plane the reference ground plane (nx=0, ny=0, nz=1, d=0), Right click the plane -> Prior -> Ground (UnixZ, Zero). Then, the map is optimized and aligned to the XY-plane.

Add a few more plane vertices to make the ground completely flat.

To make the corridor straight, add two new wall planes at the both sides of the corridor. Then, ```Right click a plane -> Loop begin -> Right click another one -> Loop end". Configure parameters as follows and press "Add Edge":

• Plane mode: PRALLEL
• Information scale: 1000.0

Now, the corridor looks straight. You can add more wall planes to further improve the map quality.

Multiple Map Merging

Menubar -> File -> Open -> Merge map

Add an edge between frames in each of the maps (All the vertices must be in a single graph for optimization).

The very first vertex of the second map has been fixed, and it prevents the map to be fused into the other map. Unfix that vertex and perform graph optimization.

Menubar -> Graph editor -> Unfix Vertex 343
Menubar -> Graph -> Optimize

Perform automatic loop closing to fuse the two maps into one.

Menubar -> Graph -> Automatic loop detection -> OK