Treemap as a Generic Open Source Backend for Different SLAM Variants
Treemap is a generic SLAM algorithm that has been successfully used to estimate extremely large 2D maps closing a loop over a million landmarks in 442ms. We are currently working on an open-source implementation that can handle most variants of SLAM. Here we show initial results demonstrating 6-DOF feature based SLAM.
This video shows the mapping process. The robot moves through a 20 story building with features on the rooms walls. Then it crosses a bridge on the 19th floor into another 20 story building and maps that building too. Finally it returns to the starting position and closes a loop over all feature. The overall map has n=106657 features and m=5319956 observations from p=488289 poses. Poses are not represented in the map. Computation time was at most 209ms.
The key point is a two layered architecture. The treemap backend contains nearly all the difficulties of the algorithm. It performs inference in a high dimensional Gaussian defined as the product of many low-dimensional Gaussians. The low-dimensional Gaussians correspond to measuremenets. The second layer, the treemap driver converts the original measurements into these low dimensional Gaussians, basically by linearizing the measurement equations and passes them to the treemap backend. It also defines some application dependent approximation policies. The backend then incrementally computes the mean of the resulting Gaussian, which is in turn converted by the treemap driver into a map estimate. We have implemented drivers for 2D feature based SLAM, 2D/3DOF pose relation based SLAM and 3D/6DOF feature based SLAM without odometry.