Kullback-leibler divergence based graph pruning in robotic feature mapping

In pose feature graph simultaneous localization and mapping, the robot poses and feature positions are treated as graph nodes and the odometry and observations are treated as edges. The size of the graph exerts an important influence on the efficiency of the graph optimization. Conventionally, the size of the graph is kept small by discarding the current frame if it is not spatially far enough from the previous one or not informative enough. However, these approaches cannot discard the already preserved frames when the robot re-visits the previously explored area. We propose a measure derived from Kullbach-Leibler divergence to decide whether a frame should be discarded, achieving an online implementation of the graph pruning algorithm for feature mapping, of which the pruned frame can be any of the preserved frames. The experimental results using real world datasets show that the proposed pruning algorithm can effectively reduce the size of the graph while maintaining the map accuracy.