Log-GPIS-MOP: A Unified Representation for Mapping, Odometry and Planning

TL;DR

The log-Gaussian process implicit surface for mapping, odometry, and planning (Log-GPIS-MOP) is proposed: a probabilistic framework for surface reconstruction, localization, and navigation based on a unified representation.

Abstract

Whereas dedicated scene representations are required for each different task in conventional robotic systems, this paper demonstrates that a unified representation can be used directly for multiple key tasks. We propose the Log-Gaussian Process Implicit Surface for Mapping, Odometry and Planning (Log-GPIS-MOP): a probabilistic framework for surface reconstruction, localisation and navigation based on a unified representation. Our framework applies a logarithmic transformation to a Gaussian Process Implicit Surface (GPIS) formulation to recover a global representation that accurately captures the Euclidean distance field with gradients and, at the same time, the implicit surface. By directly estimating the distance field and its gradient through Log-GPIS inference, the proposed incremental odometry technique computes the optimal alignment of an incoming frame and fuses it globally to produce a map. Concurrently, an optimisation-based planner computes a safe collision-free path using the same Log-GPIS surface representation. We validate the proposed framework on simulated and real datasets in 2D and 3D and benchmark against the state-of-the-art approaches. Our experiments show that Log-GPIS-MOP produces competitive results in sequential odometry, surface mapping and obstacle avoidance.

Figures (15)

• Figure 1: The proposed framework Log-GPIS-MOP incrementally builds the distance field (yellow to blue colour map) and estimates the implicit surface (red) of the first loop around the Intel Lab intel. Our incremental odometry and planning use the distance and gradients of the Log-GPIS as input to compute the current alignment and the white optimal trajectory for the robot to avoid colliding with the surfaces respectively.
• Figure 2: Block Diagram of Log-GPIS-MOP framework.Depth sensors capture raw measurements as a sequence of point clouds ${ }^{{C}_{{i}}} \mathcal{P}_i$. Incremental odometry computes the frame-to-frame poses ${ }^{C_{i-1}} \mathbf{T}_{{C}_{{i}}}$ and frame-to-map poses ${ }^{W} \mathbf{T}_{{C}_{{i}}}$ based on the local Log-GPIS representation $\bar{d}_{i}^{C_{i-1}}$ and the global Log-GPIS representation $\bar{d}_{i}^W$. Applying the estimated frame-to-map pose, the current point cloud ${ }^{W} \mathcal{P}_i$ in the global frame then fuses with the existing points ${ }^{W} \mathcal{P}_{\{0,...,i-1\}}$ into ${ }^{W} \mathcal{P}_{\{0,...,i\}}$ to update the global Log-GPIS representation $\bar{d}_{i}^W$. The full updated representation $\bar{d}^W$ with gradients $\nabla \bar{d}^W$ is used for optimisation-based path planning. The post-processing odometry optimises a batch of poses and then the post-processing mapping uses a sequence of point clouds and optimised poses as input to create the map.
• Figure 3: Factor graph representation of the frame-to-frame odometry. $\mathcal{X}_i=\{{ }^{W} \mathbf{T}_{{C}_{{1}}},...,{ }^{W} \mathbf{T}_{{C}_{{i}}}\}$ with $i=(0,...,L)$. Low opacity nodes are used for those nodes that are not used for current alignment. Black factors are the local Log-GPIS distance constraints.
• Figure 4: Factor graph for the frame-to-map odometry. Black factors represent the global Log-GPIS distance constraints.
• Figure 5: Factor graph representation of the pose SLAM problem. Green factors represent the frame-to-map constraints and red factors are the frame-to-frame constraints.