A lightweight and drift-free floor plan-assisted solution for LiDAR localization.
Ling Gao and Laurent Kneip
Mobile Perception Lab, SIST, ShanghaiTech University
Robust, accurate, and efficient localization is the backbone of many mobile autonomous systems such as smart vehicles and robots. In this context, Simultaneous Localization And Mapping (SLAM) is often pro-claimed as an essential algorithm to be run on data captured incrementally by exteroceptive sensors such as LiDARs or cameras, especially in GPS-denied indoor environments. SLAM relies on the assumption that no prior knowledge about the environment is given, and a map of the surrounding scene needs to be constructed alongside tracking the pose. This assumption often does not have to be made!
Priors about the architecture of indoor environments may often be available and given for example in the form of a dense 3D point cloud generated by a dedicated 3D content capturing solution (e.g. Polar by our industry partner Stereye , which generates high-quality dense 3D maps of the environment as illustrated in the figure beneath). By using this solution, we can not only generate a high-quality colored 3D point cloud of the environment, but in fact we can obtain a fully automatic generation of building floor-plans or Building Information Models (BIM). In the future, given the increased connectivity of devices, it is reasonable to assume that priors for example in the form of such readily available or automatically generated floorplans will simply be available. Hence, rather than running SLAM with no prior assumptions, we should invest more heavily into prior-based localization methods. This trend has already taken place in the autonomous driving community since many years, but definitely needs further push to be extended to indoor environments.
In this work, we present the first approach to perform full six degree-of-freedom tracking of 3D LiDARs in indoor environments using floor plan priors. Our method relies on the existence of horizontal ceiling and ground planes, and robustly measures those planes in 3D LiDAR scans. Ceiling and ground floor plane sensing is used to identify the roll and pitch angles as well as the height of the sensor. We furthermore use this information to compensate for rotation and extract vertical wall features from the scans. The wall points are finally projected onto a horizontal plane, after which they can be aligned with the features of a floor plan. This work in particular is focusing on the continuous tracking of the absolute pose of the sensor.
Block diagram illustrating the full pipeline of the proposed Floor Plan-assisted Indoor Localization (FP-Loc). The system uses readings from a 3D LiDAR and prior knowledge of a floor plan. It generates a six DoF trajectory from a two-stage optimization procedure. The first stage of our optimization—single-frame registration—optimizes only a single pose based on the floor plan prior. We then consult the distance between the optimized pose and the previous key pose to perform keyframe selection. The second stage of the optimization is illustrated by the below factor graph, a windowed optimization problem over multiple keyframes. The green nodes show the poses corresponding to each LiDAR scan, the orange rectangle is the prior knowledge given by the floor plan (represented in the form of an ANNF), and the grey squares are approximating linear or angular velocities based on the relative pose and the elapsed time between key poses. The latter are regularized over time to smoothify the motion.
The work leverages a novel data structure for floor plans in the form of an approximate nearest neighbor fields (ANNF), which enables an efficient look-up of the nearest geometric floor plan elements (e.g. wall segments) for any given point in the localization space. The data structure can be constructed upfront, and can be used as a look-up table. This method greatly assists in reducing the required computation time for correspondence matching and makes the final algorithm run comfortably on embedded computing hardware. We outperform traditional SLAM-based algorithms and run in 5x real-time speed on a regular laptop. The results in the paper furthermore demonstrate that the algorithm is suitable for accurate, long-term localization in challenging scenarios such as long corridors.
Adaptively sampled Approximate Nearest Neighbor Fields (ANNF) (quad-tree) for efficient look-up of nearest floor plan elements. Every quadratic subfield stores two nearest elements from the floor plan. For example, the highlighted subfield marked with a cross stores a blue line segment as its nearest neighbor and a pink line segment as its second nearest neighbor. Orange dots indicate a tested position that has the second nearest element from the ANNF as its closest element. Green dots indicate a location where the ANNF search did not include the ground truth closest element.
An interactive demo can be found at https://mpl-annf.github.io/
Please check-out the video showing our method in action:
The full paper can be read here:
https://arxiv.org/pdf/2203.00292.pdf
FYI: At MPL, we have recently released a massive dataset capturing various indoor environments with normal cameras, event cameras, an IMU, a depth camera, and a lidar. We are currently working on an extension of FP-Loc that will allow the tracking of any exteroceptive sensor based on indoor HD maps, either given in the form of a floor-plan, a dense 3D point cloud, or a 3D surface representation such as a mesh. The framework will achieve optimal, drift-free localization and tracking in all situations and be capable of online mode-switching. We believe the software package will be of great benefit to both research and industrial communities, as it will provide a standard solution for the localization of any sensor as soon as prior information about the environment is available. Please do not hesitate to reach out should you be interested in funding or collaborating with us on this effort!
The Mobile Perception Lab (MPL) under the direction of Professor Laurent Kneip was founded in 2017 and is part of the Robotics and Automation Center within the School of Information Science and Technology (SIST) at ShanghaiTech. MPL's mission is to develop next-generation 3D perception systems for mobile applications, such as robots, intelligent vehicles, or intelligence augmentation devices (e.g. augmented reality headsets). Through their research, MPL hopes to provide smart mobile machines and devices with the ability to see, to understand their environment using the sense of vision, and to perform useful tasks in the complex, unstructured, and dynamically changing environments in which we live and work. The lab also collaborates with local startup companies, such as Stereye, towards fully automatic semantic mapping solutions for the construction, AR, and robotics industries. MPL warmly welcomes anyone interested to get in touch!
Copyright©MPL 2022
https://mpl.sist.shanghaitech.edu.cn
Click "Read More"
to join us