Tag Archives: Icp

Real-time and Bayesian-enabled ICP for mobile robot localization and mapping in a Bayesian framework

November 10, 2023 09:31 ,

Maken FA, Ramos F, Ott L. , Bayesian iterative closest point for mobile robot localization, The International Journal of Robotics Research. 2022;41(9-10):851-874 DOI: 10.1177/02783649221101417.

Accurate localization of a robot in a known environment is a fundamental capability for successfully performing path planning, manipulation, and grasping tasks. Particle filters, also known as Monte Carlo localization (MCL), are a commonly used method to determine the robot\u2019s pose within its environment. For ground robots, noisy wheel odometry readings are typically used as a motion model to predict the vehicle\u2019s location. Such a motion model requires tuning of various parameters based on terrain and robot type. However, such an ego-motion estimation is not always available for all platforms. Scan matching using the iterative closest point (ICP) algorithm is a popular alternative approach, providing ego-motion estimates for localization. Iterative closest point computes a point estimate of the transformation between two poses given point clouds captured at these locations. Being a point estimate method, ICP does not deal with the uncertainties in the scan alignment process, which may arise due to sensor noise, partial overlap, or the existence of multiple solutions. Another challenge for ICP is the high computational cost required to align two large point clouds, limiting its applicability to less dynamic problems. In this paper, we address these challenges by leveraging recent advances in probabilistic inference. Specifically, we first address the run-time issue and propose SGD-ICP, which employs stochastic gradient descent (SGD) to solve the optimization problem of ICP. Next, we leverage SGD-ICP to obtain a distribution over transformations and propose a Markov Chain Monte Carlo method using stochastic gradient Langevin dynamics (SGLD) updates. Our ICP variant, termed Bayesian-ICP, is a full Bayesian solution to the problem. To demonstrate the benefits of Bayesian-ICP for mobile robotic applications, we propose an adaptive motion model employing Bayesian-ICP to produce proposal distributions for Monte Carlo Localization. Experiments using both Kinect and 3D LiDAR data show that our proposed SGD-ICP method achieves the same solution quality as standard ICP while being significantly more efficient. We then demonstrate empirically that Bayesian-ICP can produce accurate distributions over pose transformations and is fast enough for online applications. Finally, using Bayesian-ICP as a motion model alleviates the need to tune the motion model parameters from odometry, resulting in better-calibrated localization uncertainty.

Posted in: Mobile Robot Localization , Tagged: ,

Probabilistic ICP (Iterative Closest Point) with an intro on classical ICP

October 27, 2023 05:45 ,

Breux Y, Mas A, Lapierre L. On-manifold probabilistic Iterative Closest Point: Application to underwater karst exploration, The International Journal of Robotics Research. 2022;41(9-10):875-902 DOI: 10.1177/02783649221101418.

This paper proposes MpIC, an on-manifold derivation of the probabilistic Iterative Correspondence (pIC) algorithm, which is a stochastic version of the original Iterative Closest Point. It is developed in the context of autonomous underwater karst exploration based on acoustic sonars. First, a derivation of pIC based on the Lie group structure of SE(3) is developed. The closed-form expression of the covariance modeling the estimated rigid transformation is also provided. In a second part, its application to 3D scan matching between acoustic sonar measurements is proposed. It is a prolongation of previous work on elevation angle estimation from wide-beam acoustic sonar. While the pIC approach proposed is intended to be a key component in a Simultaneous Localization and Mapping framework, this paper focuses on assessing its viability on a unitary basis. As ground truth data in karst aquifer are difficult to obtain, quantitative experiments are carried out on a simulated karst environment and show improvement compared to previous state-of-the-art approach. The algorithm is also evaluated on a real underwater cave dataset demonstrating its practical applicability.

See also: Maken FA, Ramos F, Ott L. Bayesian iterative closest point for mobile robot localization. The International Journal of Robotics Research. 2022;41(9-10):851-874. doi:10.1177/02783649221101417

Posted in: Mobile Robot Localization , Tagged: ,

Comparison of map-matching methods

February 25, 2019 10:24 ,

Héber Sobreira, Carlos M. Costa, Ivo Sousa, Luis Rocha, José Lima, P. C. M. A. Farias, Paulo Costa, A. Paulo Moreira, Map-Matching Algorithms for Robot Self-Localization: A Comparison Between Perfect Match, Iterative Closest Point and Normal Distributions Transform, Journal of Intelligent & Robotic Systems, March 2019, Volume 93, Issue 3–4, pp 533–546 DOI: 10.1007/s10846-017-0765-5.

The self-localization of mobile robots in the environment is one of the most fundamental problems in the robotics navigation field. It is a complex and challenging problem due to the high requirements of autonomous mobile vehicles, particularly with regard to the algorithms accuracy, robustness and computational efficiency. In this paper, we present a comparison of three of the most used map-matching algorithms applied in localization based on natural landmarks: our implementation of the Perfect Match (PM) and the Point Cloud Library (PCL) implementation of the Iterative Closest Point (ICP) and the Normal Distribution Transform (NDT). For the purpose of this comparison we have considered a set of representative metrics, such as pose estimation accuracy, computational efficiency, convergence speed, maximum admissible initialization error and robustness to the presence of outliers in the robots sensors data. The test results were retrieved using our ROS natural landmark public dataset, containing several tests with simulated and real sensor data. The performance and robustness of the Perfect Match is highlighted throughout this article and is of paramount importance for real-time embedded systems with limited computing power that require accurate pose estimation and fast reaction times for high speed navigation. Moreover, we added to PCL a new algorithm for performing correspondence estimation using lookup tables that was inspired by the PM approach to solve this problem. This new method for computing the closest map point to a given sensor reading proved to be 40 to 60 times faster than the existing k-d tree approach in PCL and allowed the Iterative Closest Point algorithm to perform point cloud registration 5 to 9 times faster.

Posted in: Mobile Robot Localization , Tagged: ,

Globally optimal ICP

June 19, 2017 11:55 ,

J. Yang, H. Li, D. Campbell and Y. Jia, “Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 38, no. 11, pp. 2241-2254, Nov. 1 2016. DOI: 10.1109/TPAMI.2015.2513405.

The Iterative Closest Point (ICP) algorithm is one of the most widely used methods for point-set registration. However, being based on local iterative optimization, ICP is known to be susceptible to local minima. Its performance critically relies on the quality of the initialization and only local optimality is guaranteed. This paper presents the first globally optimal algorithm, named Go-ICP, for Euclidean (rigid) registration of two 3D point-sets under the $L_2$ error metric defined in ICP. The Go-ICP method is based on a branch-and-bound scheme that searches the entire 3D motion space $SE(3)$ . By exploiting the special structure of $SE(3)$ geometry, we derive novel upper and lower bounds for the registration error function. Local ICP is integrated into the BnB scheme, which speeds up the new method while guaranteeing global optimality. We also discuss extensions, addressing the issue of outlier robustness. The evaluation demonstrates that the proposed method is able to produce reliable registration results regardless of the initialization. Go-ICP can be applied in scenarios where an optimal solution is desirable or where a good initialization is not always available.

Posted in: Mobile Robot Localization , Tagged: ,

Integration of the ICP algorithm with a Kalman filter to improve relative localization, with a good state-of-the-art of ICP algorithms

June 8, 2017 11:07 ,

F. Aghili and C. Y. Su, “Robust Relative Navigation by Integration of ICP and Adaptive Kalman Filter Using Laser Scanner and IMU,” in IEEE/ASME Transactions on Mechatronics, vol. 21, no. 4, pp. 2015-2026, Aug. 2016.DOI: 10.1109/TMECH.2016.2547905.

This paper presents a robust six-degree-of-freedom relative navigation by combining the iterative closet point (ICP) registration algorithm and a noise-adaptive Kalman filter in a closed-loop configuration together with measurements from a laser scanner and an inertial measurement unit (IMU). In this approach, the fine-alignment phase of the registration is integrated with the filter innovation step for estimation correction, while the filter estimate propagation provides the coarse alignment needed to find the corresponding points at the beginning of ICP iteration cycle. The convergence of the ICP point matching is monitored by a fault-detection logic, and the covariance associated with the ICP alignment error is estimated by a recursive algorithm. This ICP enhancement has proven to improve robustness and accuracy of the pose-tracking performance and to automatically recover correct alignment whenever the tracking is lost. The Kalman filter estimator is designed so as to identify the required parameters such as IMU biases and location of the spacecraft center of mass. The robustness and accuracy of the relative navigation algorithm is demonstrated through a hardware-in-the loop simulation setting, in which actual vision data for the relative navigation are generated by a laser range finder scanning a spacecraft mockup attached to a robotic motion simulator.

Posted in: Mobile Robot Localization , Tagged: ,

Improvements on the ICP algorithm to point cloud registration from a low precision RGB-D sensor

March 24, 2017 13:04 ,

Rogério Yugo Takimoto, Marcos de Sales Guerra Tsuzuki, Renato Vogelaar, Thiago de Castro Martins, André Kubagawa Sato, Yuma Iwao, Toshiyuki Gotoh, Seiichiro Kagei, 3D reconstruction and multiple point cloud registration using a low precision RGB-D sensor, Mechatronics, Volume 35, May 2016, Pages 11-22, ISSN 0957-4158, DOI:j.mechatronics.2015.10.014.

A 3D reconstruction method using feature points is presented and the parameters used to improve the reconstruction are discussed. The precision of the 3D reconstruction is improved by combining point clouds obtained from different viewpoints using structured light. A well-known algorithm for point cloud registration is the ICP (Iterative Closest Point) that determines the rotation and translation that, when applied to one of the point clouds, places both point clouds optimally. The ICP algorithm iteratively executes two main steps: point correspondence determination and registration algorithm. The point correspondence determination is a module that, if not properly executed, can make the ICP converge to a local minimum. To overcome this drawback, two techniques were used. A meaningful set of 3D points using a technique known as SIFT (Scale-invariant feature transform) was obtained and an ICP that uses statistics to generate a dynamic distance and color threshold to the distance allowed between closest points was implemented. The reconstruction precision improvement was implemented using meaningful point clouds and the ICP to increase the number of points in the 3D space. The surface reconstruction is performed using marching cubes and filters to remove the noise and to smooth the surface. The factors that influence the 3D reconstruction precision are here discussed and analyzed. A detailed discussion of the number of frames used by the ICP and the ICP parameters is presented.

Posted in: Mobile robot mapping , Tagged: , ,