Particle filter refinement based on clustering procedures for high-dimensional localization and mapping systems

Developing safe autonomous robotic applications for outdoor agricultural environments is a research field that still presents many challenges. Simultaneous Localization and Mapping can be crucial to endow the robot to localize itself with accuracy and, consequently, perform tasks such as crop monitoring and harvesting autonomously. In these environments, the robotic localization and mapping systems usually benefit from the high density of visual features. When using filter-based solutions to localize the robot, such an environment usually uses a high number of particles to perform accurately. These two facts can lead to computationally expensive localization algorithms that are intended to perform in real-time. This work proposes a refinement step to a standard high-dimensional filter-based localization solution through the novelty of downsampling the filter using an online clustering algorithm and applying a scan-match procedure to each cluster. Thus, this approach allows scan-matchers without high computational cost, even in high dimensional filters. Experiments using real data in an agricultural environment show that this approach improves the Particle Filter performance estimating the robot pose. Additionally, results show that this approach can build a precise 3D reconstruction of agricultural environments using visual scans, i.e., 3D scans with RGB information.