Robust Posegraph Optimization Using Proximity Points

This paper proposes a robust posegraph optimization (PGO) method for posegraphs with keypoints. In the conventional PGO formulation, a loop constraint is defined between a pair of nodes, whereas in the proposed method, it is defined between a pair of keypoints. In this manner, robust PGO based on switch variables can be realized in a more fine-grained manner. Loop constraint is defined based on the unique geometric property of proximity point, and implemented as a new edge type of the g2o solver. The proposed method is compared with other robust PGO methods using real world data recorded in Nakanoshima Robot Challenge 2021.