In autonomous driving, a prior LiDAR map(PLM) is used as a powerful tool for correcting SLAM drift, but finding robust and accurate correspondences between cross modal sensors is a challenging problem. To address this cross-modality issue, this paper proposes a real-time plane-based stereo localization system with a PLM. In the proposed system, drift in visual pose estimation is eliminated through plane-based joint optimization and the registration module. Two types of planes are employed in this system: surfel, ensuring accuracy in a narrow domain, and global plane, providing robustness in a wide area. For accurate and robust matching between the visual map point and PLM, surfels and global planes in PLM are utilized collaboratively based on point-to-PLM distance. To reduce the computational cost of the registration module, a plane-to-plane drift estimation module is proposed. The performance of the proposed system is extensively validated across synthetic simulation and real-world indoor and outdoor datasets. We validate the effectiveness of each module through ablation studies and also assess the robustness against error that may exist in PLM and initial pose. In most of the validation, the proposed system shows more accurate and robust performance compared to the state-of-the-art methods.

This article presents an online distributed trajectory planning algorithm for a quadrotor swarm in a maze-like dynamic environment. We utilize a dynamic linear safe corridor to construct the feasible collision constraints that can ensure interagent collision avoidance and consider the uncertainty of moving obstacles. We introduce mode-based subgoal planning to resolve deadlock faster in a complex environment using only previously shared information. For dynamic obstacle avoidance, we adopt heuristic methods such as collision alert propagation and escape point planning to deal with the situation where dynamic obstacles approach the agents clustered in a narrow corridor. We prove that the proposed algorithm guarantees the feasibility of the optimization problem for every replanning step. In an obstacle-free space, the proposed method can compute the trajectories for 60 agents on average 7.66 ms per agent with an Intel i7 laptop and shows the perfect success rate. Also, our method shows 64.5 <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\%$</tex-math></inline-formula> shorter flight time than buffered Voronoi cell and 34.6 <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\%$</tex-math></inline-formula> shorter than with our previous work. We conduct the simulation in a random forest and maze with four dynamic obstacles, and the proposed algorithm shows the highest success rate and shortest flight time compared to state-of-the-art baseline algorithms. In particular, the proposed algorithm shows over 97 <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\%$</tex-math></inline-formula> success rate when the velocity of moving obstacles is below the agent's maximum speed. We validate the safety and robustness of the proposed algorithm through a hardware demonstration with ten quadrotors and two pedestrians in a maze-like environment.