An Improved MbICP Algorithm for Mobile Robot Pose Estimation

.


Introduction
Simultaneous localization and mapping (SLAM) is one of the most important issues in mobile robotics research [1][2][3][4][5].SLAM is a process by which a mobile robot can build a map of its environment and simultaneously use this map to locate itself [6].Therefore, an accurate motion estimation of the robot will greatly facilitate the realization of this process and has attracted considerable attention from researchers in recent decades [1,2,[7][8][9].
Scan-matching techniques are widely used to estimate mobile robot motion by matching successive range measurements from a laser range finder (LRF) [1,2,4,[9][10][11][12][13][14][15][16][17].The iterative closest point (ICP) algorithm is one of the most popular scan-matching techniques because of its efficiency and simplicity [18][19][20][21][22].This algorithm iteratively establishes correspondences based on the criterion of the closest point and calculates a rigid transformation that minimizes the distance error until convergence.However, ICP tends to fall into local optima when large sensor rotation is introduced, which frequently arises when a robot rotates around itself or unexpected slippage occurs [1].Additionally, ICP utilizes a point-to-point metric to determine correspondences, which is less accurate because of the discreteness of laser scans.In addition, ICP relies on a questionable assumption that successive scans perfectly overlap [19,20], which can hardly stand because of changes in the environment and occlusions from from a laser scanner at the beginning and end of motion, the first scan is marked as the reference scan S re f and the second scan is treated as the object scan S obj (as shown in Figure 1b).successive scans from a laser scanner at the beginning and end of motion, the first scan is marked as the reference scan and the second scan is treated as the object scan (as shown in Figure 1b).The transformation T can be estimated by minimizing the distance of the corresponding points between and with the ICP algorithm, which is based on a three-step iterative process: 1. Establish correspondence.For each point in , search for the closest point in to establish correspondence.A K-D tree is recommended to accelerate the searching process.2. Calculate the transformation.The rigid transformation consisting of rotation matrix R and translation vector t is calculated by minimizing the mean square error of the correspondence pairs between and (Equation ( 1)).SVD-based least-squares minimization is commonly used because of its robustness and simplicity [31].
where is the number of points in , ∈ and ∈ . is the closest point of , which is defined as the correspondence.

Transform
. Apply a rigid transformation to and update the related parameters.The transformation T can be estimated by minimizing the distance of the corresponding points between S re f and S obj with the ICP algorithm, which is based on a three-step iterative process: 1.
Establish correspondence.For each point in S obj , search for the closest point in S re f to establish correspondence.A K-D tree is recommended to accelerate the searching process.

2.
Calculate the transformation.The rigid transformation T consisting of rotation matrix R and translation vector t is calculated by minimizing the mean square error of the correspondence pairs between S obj and S re f (Equation ( 1)).SVD-based least-squares minimization is commonly used because of its robustness and simplicity [31].
where N is the number of points in S obj , p i ∈ S obj and m j ∈ S re f .m j is the closest point of p i , which is defined as the correspondence.

3.
Transform S obj .Apply a rigid transformation to S obj and update the related parameters.
These three steps are repeated until convergence or the iteration number exceeds the maximum number of iterations.

Metric-based Distance
Assume that two successive laser scans with large rotation are given (Figure 2): the reference scan S re f with n p points, P = {p i , i = 1, . . ., n p }, and the object scan S obj with n m points, M = {m j , j = 1, . . ., n m }.When establishing the correspondence, the standard ICP algorithm searches for the closest point in M for each point in P, which is under the framework of Euclidean space.Points in P that are far from the center are also far from their actual correspondence points because of the angular difference, which easily creates false correspondence and ICP failure.Thus, finding the closest point in Euclidean space is not reliable for establishing correspondence when large rotation exists.These three steps are repeated until convergence or the iteration number exceeds the maximum number of iterations.

Metric-based Distance
Assume that two successive laser scans with large rotation are given (Figure 2): the reference scan with points, = { , = 1, … , }, and the object scan with points, = { , = 1, … , }.When establishing the correspondence, the standard ICP algorithm searches for the closest point in M for each point in , which is under the framework of Euclidean space.Points in that are far from the center are also far from their actual correspondence points because of the angular difference, which easily creates false correspondence and ICP failure.Thus, finding the closest point in Euclidean space is not reliable for establishing correspondence when large rotation exists.and the object point = ( , ).A set of rigid-body transformations = ( , , ) can be acquired that satisfies q( ) = (Equation (2)).
The metric distance ( , ) between and is defined as the minimum norm among the transformations (Equation (3)). Where L is a balance parameter that is homogeneous to a length, which is used to balance the translational and rotational components.
A set of iso-distance curves relative to and is depicted by using the metric-based distance and Euclidean distance to visually represent the geometric features of metric-based distance (Figure 3a).The iso-distance curves that use the metric-based distance are claimed to be ellipses in the literature [10], while those that use the Euclidean distance are inscribed circles, which provides the metric-based distance a larger search zone than that of the Euclidean distance for the same search radius.The farther the object point is from the center, the longer the long axis of the To overcome the defect of Euclidean distance in merely considering the translation component, MbICP defines a new distance measure called the metric-based distance, which simultaneously considers both translation and rotation.Given two points in R 2 , the reference point p i = p ix , p iy and the object point m j = m jx , m jy .A set of rigid-body transformations q = (x, y, θ) can be acquired that satisfies q m j = p i (Equation ( 2)).
The metric distance d p i , m j between m j and p i is defined as the minimum norm among the transformations (Equation (3)). where L is a balance parameter that is homogeneous to a length, which is used to balance the translational and rotational components.
A set of iso-distance curves relative to P i and M j is depicted by using the metric-based distance and Euclidean distance to visually represent the geometric features of metric-based distance (Figure 3a).The iso-distance curves that use the metric-based distance are claimed to be ellipses in the literature [10], while those that use the Euclidean distance are inscribed circles, which provides the metric-based distance a larger search zone than that of the Euclidean distance for the same search radius.The farther the object point is from the center, the longer the long axis of the ellipse becomes and, therefore, the more likely the object point will find its correspondence point in the rotational direction.This feature of the metric-based distance improves the probability of finding the correct correspondence for points that are far from the sensor, as shown in Figure 3b.
ellipse becomes and, therefore, the more likely the object point will find its correspondence point in the rotational direction.This feature of the metric-based distance improves the probability of finding the correct correspondence for points that are far from the sensor, as shown in Figure 3b.

Resampling
Laser scanners have a settled viewing angle and resolution, so a longer scanning distance increases the distance between neighboring scanned points.When a mobile robot moves forward in a long narrow corridor, the density distribution of point clouds from the laser scanner greatly varies according to the distance from the sensor.Sensors scan fewer points at greater distances (as shown in Figure 4a).However, such a different density distribution of point clouds may slow down the iteration process and distort the calculation of the rejection threshold, as mentioned in chapter 2.4.1.Therefore, we utilize a resampling method that is based on grid partitioning to realize a relatively even density distribution.

Resampling
Laser scanners have a settled viewing angle and resolution, so a longer scanning distance increases the distance between neighboring scanned points.When a mobile robot moves forward in a long narrow corridor, the density distribution of point clouds from the laser scanner greatly varies according to the distance from the sensor.Sensors scan fewer points at greater distances (as shown in Figure 4a).However, such a different density distribution of point clouds may slow down the iteration process and distort the calculation of the rejection threshold, as mentioned in chapter 2.4.1.Therefore, we utilize a resampling method that is based on grid partitioning to realize a relatively even density distribution.ellipse becomes and, therefore, the more likely the object point will find its correspondence point in the rotational direction.This feature of the metric-based distance improves the probability of finding the correct correspondence for points that are far from the sensor, as shown in Figure 3b.

Resampling
Laser scanners have a settled viewing angle and resolution, so a longer scanning distance increases the distance between neighboring scanned points.When a mobile robot moves forward in a long narrow corridor, the density distribution of point clouds from the laser scanner greatly varies according to the distance from the sensor.Sensors scan fewer points at greater distances (as shown in Figure 4a).However, such a different density distribution of point clouds may slow down the iteration process and distort the calculation of the rejection threshold, as mentioned in chapter 2.4.1.Therefore, we utilize a resampling method that is based on grid partitioning to realize a relatively even density distribution.First, we build a spatial grid for the object point cloud.We use a coordinate system that is centered at the sensor's position to calculate the coordinates of all the scanned points and then obtain the minimum bounding rectangle of the point cloud.The rectangle is partitioned into grids according to a given grid length (0.1 m is used according to the sensor's resolution), as shown in Figure 4b.
Second, we calculate a sampling percentage for points in each grid.Given two occupied grids whose row and column indices are (r 1 , c 1 ) and (r 2 , c 2 ), respectively, the distance d between the grids is defined as (r 1 − r 2 ) 2 + (c 1 − c 2 ) 2 .The sensor is contained in the center grid, so its row and column indices are both zero.We calculate the distance d i from each grid to the center grid according to the distance definition and obtain the max distance (d max ).The sampling ratio for points in each grid is thus defined as the ratio of d i and d max , which guarantees that distant grids from the center grid have a relatively high sampling percentage and nearby grids have a low sampling percentage.
Finally, we resample points in the units of the grids according to the assigned sampling percentage.Given a grid with n points and sampling percentage η, the sampling size n is calculated as the ceiling of n × η.To preserve as much topological information as possible for the points, the first and last points are preserved and the remaining n − 2 points are sampled at regular intervals, as shown in Figure 5a.
This resampling method is based on a grid partition, which determines the sampling percentage according to the distance from the occupied grid to the center grid and then samples points in the units of the grid according to the topological information.This method can effectively dilute dense points nearby and retain their topological information, therefore accelerating the iteration process and preventing the calculation of the rejection threshold from becoming distorted by the greatly varying density distributions of point clouds.
centered at the sensor's position to calculate the coordinates of all the scanned points and then obtain the minimum bounding rectangle of the point cloud.The rectangle is partitioned into grids according to a given grid length (0.1 m is used according to the sensor's resolution), as shown in Figure 4b.
Second, we calculate a sampling percentage for points in each grid.Given two occupied grids whose row and column indices are ( , ) and ( , ), respectively, the distance between the grids is defined as ( − ) +( − ) .The sensor is contained in the center grid, so its row and column indices are both zero.We calculate the distance from each grid to the center grid according to the distance definition and obtain the max distance ( ).The sampling ratio for points in each grid is thus defined as the ratio of and d , which guarantees that distant grids from the center grid have a relatively high sampling percentage and nearby grids have a low sampling percentage.
Finally, we resample points in the units of the grids according to the assigned sampling percentage.Given a grid with n points and sampling percentage , the sampling size n' is calculated as the ceiling of × .To preserve as much topological information as possible for the points, the first and last points are preserved and the remaining n' − 2 points are sampled at regular intervals, as shown in Figure 5a.This resampling method is based on a grid partition, which determines the sampling percentage according to the distance from the occupied grid to the center grid and then samples points in the units of the grid according to the topological information.This method can effectively dilute dense points nearby and retain their topological information, therefore accelerating the iteration process and preventing the calculation of the rejection threshold from becoming distorted by the greatly varying density distributions of point clouds.

Correspondence Establishment
The MbICP algorithm finds correspondence points by finding the nearest point through its defined metric-based distance, which is claimed to perform better than when using the Euclidean distance when large rotation exists.However, this algorithm is still based on a point-to-point metric, which is not as reliable because of mismatching from partial overlapping and the discreteness of laser scans.For example, given two successive laser scans that are captured at the beginning and end of robot motion, partial overlapping can be clearly observed in rectangular area 1 in Figure 6a and Figure 6b.This area contains no actual correspondence points for points in the object scan, thus

Correspondence Establishment
The MbICP algorithm finds correspondence points by finding the nearest point through its defined metric-based distance, which is claimed to perform better than when using the Euclidean distance when large rotation exists.However, this algorithm is still based on a point-to-point metric, which is not as reliable because of mismatching from partial overlapping and the discreteness of laser scans.For example, given two successive laser scans that are captured at the beginning and end of robot motion, partial overlapping can be clearly observed in rectangular area 1 in Figure 6a,b.This area contains no actual correspondence points for points in the object scan, thus producing many incorrect correspondences.Additionally, different points in the object scan incorrectly correspond to the same point in the reference scan because of the discrete nature of the scan, as shown in rectangular area 2 in Figure 6a.These incorrect correspondences will distort the follow-up calculation of the optimal transformation and thus negatively affect the final matching result.
producing many incorrect correspondences.Additionally, different points in the object scan incorrectly correspond to the same point in the reference scan because of the discrete nature of the scan, as shown in rectangular area 2 in Figure 6a.These incorrect correspondences will distort the follow-up calculation of the optimal transformation and thus negatively affect the final matching result.To reduce problems that are introduced by the point-to-point metric, this paper improves the procedure of determining the correspondence by combining the point-to-point and point-to-line metrics.First, the nearest point in the reference scan is determined for each point in the object scan by using the metric-based distance, and ( , ) is treated as a candidate correspondence pair.For those candidate correspondence pairs whose object points correspond to a reference point, only the pair with the smallest corresponding distance is preserved and the remaining pairs are modified by using an interpolation method.Taking ( , ) as an example, we search for the second-closest point for in the reference scan, and a segment that begins at and ends at is found.We To reduce problems that are introduced by the point-to-point metric, this paper improves the procedure of determining the correspondence by combining the point-to-point and point-to-line metrics.First, the nearest point m j in the reference scan is determined for each point p i in the object scan by using the metric-based distance, and p i , m j is treated as a candidate correspondence pair.For those candidate correspondence pairs whose object points correspond to a reference point, only the pair with the smallest corresponding distance is preserved and the remaining pairs are modified by using an interpolation method.Taking p i , m j as an example, we search for the second-closest point m j for p i in the reference scan, and a segment that begins at m j and ends at m j is found.We calculate the projection of p i onto the segment along the normal and use this interpolation point as a new corresponding point (as shown in Figure 7a).This interpolation is less risky because it only utilizes the surface information of the reference point clouds.
calculate the projection of onto the segment along the normal and use this interpolation point as a new corresponding point (as shown in Figure 7a).This interpolation is less risky because it only utilizes the surface information of the reference point clouds.This new procedure can improve the performance of correspondence establishment, especially when non-overlapping areas exist (as shown in Figure 8).However, a few incorrect correspondence pairs might be established because of the complexity of the environment.Thus, a rejection strategy will be explained in the next section.

Rejection of Incorrect Correspondences
After establishing correspondence based on the new procedure, a set of correspondence pairs are obtained, which may contain a few incorrect correspondence pairs with large distance.Therefore, a rejection strategy that is based on the corresponding distance is required to protect the follow-up calculation from transformation.
One of the most commonly used methods is the standard-deviations-from-mean approach, which removes values that are more than n standard deviations from the mean.The value of n is decided according to the practical situation.However, this method is unreliable because extreme values may distort the mean and standard deviation.
For example, given a sequence of correspondence distances, X = {12.281,12.270, 12.712, 11.932, 11.053, 10.768, 11.077, 11.685, 6.393, 6.001, 5.549, 38.760, 86.305, 34.497, 2.988, 3.227, 1.297, 3.539, 6.409, 12.477, 12.381}.The rejection result when using the standard-deviations-from-mean approach is depicted in a dot plot (as shown in Figure 9a).The mean is approximately 14.93, which obviously This new procedure can improve the performance of correspondence establishment, especially when non-overlapping areas exist (as shown in Figure 8).However, a few incorrect correspondence pairs might be established because of the complexity of the environment.Thus, a rejection strategy will be explained in the next section.This new procedure can improve the performance of correspondence establishment, especially when non-overlapping areas exist (as shown in Figure 8).However, a few incorrect correspondence pairs might be established because of the complexity of the environment.Thus, a rejection strategy will be explained in the next section.

Rejection of Incorrect Correspondences
After establishing correspondence based on the new procedure, a set of correspondence pairs are obtained, which may contain a few incorrect correspondence pairs with large distance.Therefore, a rejection strategy that is based on the corresponding distance is required to protect the follow-up calculation from transformation.
One of the most commonly used methods is the standard-deviations-from-mean approach, which removes values that are more than n standard deviations from the mean.The value of n is decided according to the practical situation.However, this method is unreliable because extreme values may distort the mean and standard deviation.

Rejection of Incorrect Correspondences
After establishing correspondence based on the new procedure, a set of correspondence pairs are obtained, which may contain a few incorrect correspondence pairs with large distance.Therefore, a rejection strategy that is based on the corresponding distance is required to protect the follow-up calculation from transformation.
One of the most commonly used methods is the standard-deviations-from-mean approach, which removes values that are more than n standard deviations from the mean.The value of n is decided according to the practical situation.However, this method is unreliable because extreme values may distort the mean and standard deviation.
deviates from the center of the data set, and the standard deviation is also considerably distorted.Only 86.305 is rejected as an extreme value, while 38.760 and 34.497 are ignored even though they are obviously also extreme values.The MAD-from-median method is another rejection method, which removes values that are more than two median absolute deviations (MAD) from the median.Instead of utilizing the mean and standard deviation to measure the center and discreteness degree of the data set, this method uses the median and MAD to represent the clustering and statistical dispersion of the data set.The median is not greatly skewed by extreme values and thus can provide a better idea of the typical value than the mean can.The MAD, which is defined as the median of the absolute deviations from the median of the data set, is also robust to extreme values.
The rejection result when using the MAD-from-median method is depicted in Figure 9b.The median is 11.07, which is a good representation of the center.Here, 38.760, 86.305 and 34.497 are all rejected as extreme values.Thus, the MAD-from-median method can more clearly differentiate extreme values and is herein adopted to reject correspondences with large distances.
Therefore, we calculate the rejection threshold d by using Equation ( 5) and reject correspondences with distances above d , which are likely incorrect correspondences.This rejection strategy is risky because d may be distorted by the greatly varying density distributions of point clouds.When a mobile platform moves forward in a long narrow corridor, point clouds tend to be quite dense near the sensor but sparse in more distant regions from the sensor.However, the object point clouds cannot be moved closer to the reference point clouds because of the absence of an initial pose from the odometer.The dense points that are distributed within the corridor (rectangle 2 in Figure 10a) can hardly find actual correspondence points and are likely to establish incorrect correspondences with minor distances, which are unhelpful but are in the majority.The sparse points that are distributed within the frontal obstacles (rectangle 1 in Figure 10a) are likely to establish correct correspondences with relatively large distances, which are vital to bring two point clouds closer but are in the minority.Therefore, directly rejecting correspondence pairs that have The MAD-from-median method is another rejection method, which removes values that are more than two median absolute deviations (MAD) from the median.Instead of utilizing the mean and standard deviation to measure the center and discreteness degree of the data set, this method uses the median and MAD to represent the clustering and statistical dispersion of the data set.The median is not greatly skewed by extreme values and thus can provide a better idea of the typical value than the mean can.The MAD, which is defined as the median of the absolute deviations from the median of the data set, is also robust to extreme values.
The rejection result when using the MAD-from-median method is depicted in Figure 9b.The median is 11.07, which is a good representation of the center.Here, 38.760, 86.305 and 34.497 are all rejected as extreme values.Thus, the MAD-from-median method can more clearly differentiate extreme values and is herein adopted to reject correspondences with large distances.
Therefore, we calculate the rejection threshold d rej by using Equation ( 5) and reject correspondences with distances above d rej , which are likely incorrect correspondences.This rejection strategy is risky because d rej may be distorted by the greatly varying density distributions of point clouds.When a mobile platform moves forward in a long narrow corridor, point clouds tend to be quite dense near the sensor but sparse in more distant regions from the sensor.However, the object point clouds cannot be moved closer to the reference point clouds because of the absence of an initial pose from the odometer.The dense points that are distributed within the corridor (rectangle 2 in Figure 10a) can hardly find actual correspondence points and are likely to establish incorrect correspondences with minor distances, which are unhelpful but are in the majority.The sparse points that are distributed within the frontal obstacles (rectangle 1 in Figure 10a) are likely to establish correct correspondences with relatively large distances, which are vital to bring two point clouds closer but are in the minority.Therefore, directly rejecting correspondence pairs that have distances above d rej but that are vital for calculating the transformation, may cause potential local-minima problems.Fortunately, this risk is reduced by the resampling method in chapter 2.3, so d rej can effectively reject correspondences with extremely large distances and preserve vital correct correspondences (rectangle 1 in Figure 10b).

Results
The experimental results are outlined in this section.We tested the improved MbICP algorithm with data from a mobile wheelchair platform that was equipped with a Hokuyo UTM-30LX laser range finder (Hokuyo Co. Ltd., Osaka, Japan).The Hokuyo UTM-30LX is a 2-D laser range finder with a 30 m effective measurement range, 270° field of view (angular resolution of 0.25°), and 40 Hz scanning rate.We designed two types of experiments to compare our algorithm with the original algorithm.The first experiment evaluated the properties of the algorithms by matching a pair of scans that partially overlapped.The second experiment evaluated the global algorithm performance with a run within our academic building.

Evaluation Method
The first experiment compared the accuracy and convergence of the improved MbICP algorithm with those of the original MbICP algorithm in matching partially overlapped scans.Before introducing more details regarding the experiment, we first explain the evaluation factors of the accuracy and convergence.Given two scans that were acquired in the same sensor location, we precisely know that the ground truth of the pose estimate is (0, 0, 0°).To quantitatively compare the algorithms' accuracy, a random displacement is added to the initial pose estimate, and one scan is also roto-translated by this value.This transformed scan is treated as the object scan, which is marked as , while the other scan is treated as the reference scan, which is marked as .After matching, the pose estimation is determined and statistical analysis can be performed.The translational and rotational deviation between the pose estimation from the algorithm and the ground truth is used as the evaluative factor of the algorithm's accuracy.The smaller the deviation,

Results
The experimental results are outlined in this section.We tested the improved MbICP algorithm with data from a mobile wheelchair platform that was equipped with a Hokuyo UTM-30LX laser range finder (Hokuyo Co. Ltd., Osaka, Japan).The Hokuyo UTM-30LX is a 2-D laser range finder with a 30 m effective measurement range, 270 • field of view (angular resolution of 0.25 • ), and 40 Hz scanning rate.We designed two types of experiments to compare our algorithm with the original algorithm.The first experiment evaluated the properties of the algorithms by matching a pair of scans that partially overlapped.The second experiment evaluated the global algorithm performance with a run within our academic building.

Evaluation Method
The first experiment compared the accuracy and convergence of the improved MbICP algorithm with those of the original MbICP algorithm in matching partially overlapped scans.Before introducing more details regarding the experiment, we first explain the evaluation factors of the accuracy and convergence.Given two scans that were acquired in the same sensor location, we precisely know that the ground truth of the pose estimate is (0, 0, 0 • ).To quantitatively compare the algorithms' accuracy, a random displacement δ is added to the initial pose estimate, and one scan is also roto-translated by this value.This transformed scan is treated as the object scan, which is marked as S obj , while the other scan is treated as the reference scan, which is marked as S re f .After matching, the pose estimation is determined and statistical analysis can be performed.The translational and rotational deviation between the pose estimation from the algorithm and the ground truth is used as the evaluative factor of the algorithm's accuracy.The smaller the deviation, the more closely S obj fits S re f and the better the accuracy of the algorithm.In addition, the number of iterations can intuitively represent the algorithm's convergence.
We conducted the first experiment by matching two partially overlapping scans with the improved algorithm and standard algorithm.To obtain experimental scans, we recorded data taken by UTM-30LX laser range finder in a corner by artificially adding or removing the trashcan in the field of view without moving the sensor.This way we acquired a pair of scans that partially overlap, one of which with the presence of the trashcan is shown in Figure 11a and the other is shown in Figure 11b.The scan with this trashcan was roto-translated with a given transformation (500 mm, 500 mm, 15 • ) and then used as the object scan, while the scan without the trashcan was treated as the reference scan (as shown in Figure 12a).The matching result from the standard algorithm is depicted in Figure 12b, which is far from satisfactory because of obvious deviation between two scans.The distance between the estimated pose and the ground truth was (75.75 mm, 34.69 mm, 2.71 • ), demonstrating awful accuracy.However, the matching result from the improved algorithm was relatively satisfactory because the two scans closely matched (as shown in Figure 12c).The deviation between the estimated pose and ground truth was nearly zero, demonstrating high accuracy.The improved algorithm also exhibited a faster convergence rate (18 iterations) because of the resampling step, while the standard algorithm satisfied the convergence condition after 22 iterations (Figure 12d).the more closely fits and the better the accuracy of the algorithm.In addition, the number of iterations can intuitively represent the algorithm's convergence.
We conducted the first experiment by matching two partially overlapping scans with the improved algorithm and standard algorithm.To obtain experimental scans, we recorded data taken by UTM-30LX laser range finder in a corner by artificially adding or removing the trashcan in the field of view without moving the sensor.This way we acquired a pair of scans that partially overlap, one of which with the presence of the trashcan is shown in Figure 11a and the other is shown in Figure 11b.The scan with this trashcan was roto-translated with a given transformation (500 mm, 500 mm, 15°) and then used as the object scan, while the scan without the trashcan was treated as the reference scan (as shown in Figure 12a).The matching result from the standard algorithm is depicted in Figure 12b, which is far from satisfactory because of obvious deviation between two scans.The distance between the estimated pose and the ground truth was (75.75 mm, 34.69 mm, 2.71°), demonstrating awful accuracy.However, the matching result from the improved algorithm was relatively satisfactory because the two scans closely matched (as shown in Figure 12c).The deviation between the estimated pose and ground truth was nearly zero, demonstrating high accuracy.The improved algorithm also exhibited a faster convergence rate (18 iterations) because of the resampling step, while the standard algorithm satisfied the convergence condition after 22 iterations (Figure 12d).the more closely fits and the better the accuracy of the algorithm.In addition, the number of iterations can intuitively represent the algorithm's convergence.
We conducted the first experiment by matching two partially overlapping scans with the improved algorithm and standard algorithm.To obtain experimental scans, we recorded data taken by UTM-30LX laser range finder in a corner by artificially adding or removing the trashcan in the field of view without moving the sensor.This way we acquired a pair of scans that partially overlap, one of which with the presence of the trashcan is shown in Figure 11a and the other is shown in Figure 11b.The scan with this trashcan was roto-translated with a given transformation (500 mm, 500 mm, 15°) and then used as the object scan, while the scan without the trashcan was treated as the reference scan (as shown in Figure 12a).The matching result from the standard algorithm is depicted in Figure 12b, which is far from satisfactory because of obvious deviation between two scans.The distance between the estimated pose and the ground truth was (75.75 mm, 34.69 mm, 2.71°), demonstrating awful accuracy.However, the matching result from the improved algorithm was relatively satisfactory because the two scans closely matched (as shown in Figure 12c).The deviation between the estimated pose and ground truth was nearly zero, demonstrating high accuracy.The improved algorithm also exhibited a faster convergence rate (18 iterations) because of the resampling step, while the standard algorithm satisfied the convergence condition after 22 iterations (Figure 12d).We first discuss the results in terms of robustness.A scan match was considered a failure when the solution was larger than 0.1 m in translation and 3.14° in rotation.Solutions with an error below the threshold were used in a follow-up precision analysis.True Positives (converged to the right solution), False Positives (converged to an incorrect solution), True Negatives (not converged but the solution is correct) and False Negatives (not converged and the solution is incorrect) are frequently used robustness characteristics.
For the standard algorithm, the true positives decreased and the false positives increased as the overlapping percentage decreased, as shown in Table 1.Thus, the robustness of the standard algorithm was influenced by non-overlapping areas between scans.The improved algorithm showed good robustness because all the solutions were true positives despite a decrease in the overlapping percentage, as shown in Table 2.We calculated the deviation between the estimated pose and ground truth to indicate the precision (only using True Positives), as shown in Table 3.Both algorithms were accurate when two We first discuss the results in terms of robustness.A scan match was considered a failure when the solution was larger than 0.1 m in translation and 3.14 • in rotation.Solutions with an error below the threshold were used in a follow-up precision analysis.True Positives (converged to the right solution), False Positives (converged to an incorrect solution), True Negatives (not converged but the solution is correct) and False Negatives (not converged and the solution is incorrect) are frequently used robustness characteristics.
For the standard algorithm, the true positives decreased and the false positives increased as the overlapping percentage decreased, as shown in Table 1.Thus, the robustness of the standard algorithm was influenced by non-overlapping areas between scans.The improved algorithm showed good robustness because all the solutions were true positives despite a decrease in the overlapping percentage, as shown in Table 2.We calculated the deviation between the estimated pose and ground truth to indicate the precision (only using True Positives), as shown in Table 3.Both algorithms were accurate when two scans perfectly overlapped but experienced increasing deviation in their translation and rotation as the overlap ratio decreased.However, the improved algorithm was more accurate than the standard algorithm when the scans partially overlapped.Moreover, a decline in the overlap ratio substantially decreased the performance of the standard algorithm but had no significant effect on the improved algorithm.

Real-World Applications
The first experiment was ingenious because it allows for matching real-word scans without requiring the ground truth, but generating scans with a specific overlapping percentage by randomly removing a certain number of points is risky and does not always conform to reality.Therefore, we conducted a second experiment, which involved a run with a mobile robot on the 4th floor of the School of Resource and Environment Science Building, Wuhan University.This building consisted of three narrow corridors and two corners, forming a U-shaped room.Stationary structures, such as offices, stairs and toilets, were distributed along the corridors.In addition, dynamic obstacles such as umbrellas and walking people were present in this scenario.We applied the two algorithms to match each pair of successive scans and output many transformed cloud points and a trajectory.This experiment was challenging because the overlapping percentage of successive scans from rotation and range holes could not be estimated in advance.
Figure 14 depicts the matching results from the two algorithms.Obtaining the exact transformation data for each step during the experiment was difficult [1].The ground truth and numeric values are thus actually unavailable in real-world applications because of a lack of information regarding actual robot poses, which is commonly the case in current relevant studies [1,10].The results of the improved algorithm were clearly better than those of the standard algorithm because the former could adequately fit successive scans and the generated point clouds accurately presented the indoor structure.In contrast, the matching results from the standard algorithm suffered from several obvious mismatches around stairs and corners.Thus, the behavior of the improved algorithm was generally better than that of the standard algorithm under more realistic conditions.
Appl.Sci.2018, 8, x FOR PEER REVIEW 14 of 18 scans perfectly overlapped but experienced increasing deviation in their translation and rotation as the overlap ratio decreased.However, the improved algorithm was more accurate than the standard algorithm when the scans partially overlapped.Moreover, a decline in the overlap ratio substantially decreased the performance of the standard algorithm but had no significant effect on the improved algorithm.

Real-World Applications
The first experiment was ingenious because it allows for matching real-word scans without requiring the ground truth, but generating scans with a specific overlapping percentage by randomly removing a certain number of points is risky and does not always conform to reality.Therefore, we conducted a second experiment, which involved a run with a mobile robot on the 4th floor of the School of Resource and Environment Science Building, Wuhan University.This building consisted of three narrow corridors and two corners, forming a U-shaped room.Stationary structures, such as offices, stairs and toilets, were distributed along the corridors.In addition, dynamic obstacles such as umbrellas and walking people were present in this scenario.We applied the two algorithms to match each pair of successive scans and output many transformed cloud points and a trajectory.This experiment was challenging because the overlapping percentage of successive scans from rotation and range holes could not be estimated in advance.
Figure 14 depicts the matching results from the two algorithms.Obtaining the exact transformation data for each step during the experiment was difficult [1].The ground truth and numeric values are thus actually unavailable in real-world applications because of a lack of information regarding actual robot poses, which is commonly the case in current relevant studies [1,10].The results of the improved algorithm were clearly better than those of the standard algorithm because the former could adequately fit successive scans and the generated point clouds accurately presented the indoor structure.In contrast, the matching results from the standard algorithm suffered from several obvious mismatches around stairs and corners.Thus, the behavior of the improved algorithm was generally better than that of the standard algorithm under more realistic conditions.

Discussion
The improved MbICP algorithm is limited in that it may fall into local minima under specific scenarios, such as a long, narrow corridor with few frontal structures, indicating the need for further improvement.Frontal obstacles are very vital to bring two clouds together because their correspondences play an important role in calculating a ground transformation.As seen in Figure 15a, the left scan was captured in a long narrow corridor (corresponding to rectangle 2 in Figure 15b) where there are few frontal obstacles, while the right scan was captured in a long narrow corridor (corresponding to rectangle 1 in Figure 15b) where there are several frontal obstacles.As seen in Figure 15b, the matching result was satisfactory in rectangle 1 but demonstrated a "shortening" effect in rectangle 2. Bringing two point clouds closer was difficult because of lacking necessary frontal obstacles and insufficient correct correspondences that are vital for gaining an optimal transformation in each iteration, thus causing local-minima problems.

Discussion
The improved MbICP algorithm is limited in that it may fall into local minima under specific scenarios, such as a long, narrow corridor with few frontal structures, indicating the need for further improvement.Frontal obstacles are very vital to bring two clouds together because their correspondences play an important role in calculating a ground transformation.As seen in Figure 15a, the left scan was captured in a long narrow corridor (corresponding to rectangle 2 in Figure 15b) where there are few frontal obstacles, while the right scan was captured in a long narrow corridor (corresponding to rectangle 1 in Figure 15b) where there are several frontal obstacles.As seen in Figure 15b, the matching result was satisfactory in rectangle 1 but demonstrated a "shortening" effect in rectangle 2. Bringing two point clouds closer was difficult because of lacking necessary frontal obstacles and insufficient correct correspondences that are vital for gaining an optimal transformation in each iteration, thus causing local-minima problems.

Discussion
The improved MbICP algorithm is limited in that it may fall into local minima under specific scenarios, such as a long, narrow corridor with few frontal structures, indicating the need for further improvement.Frontal obstacles are very vital to bring two clouds together because their correspondences play an important role in calculating a ground transformation.As seen in Figure 15a, the left scan was captured in a long narrow corridor (corresponding to rectangle 2 in Figure 15b) where there are few frontal obstacles, while the right scan was captured in a long narrow corridor (corresponding to rectangle 1 in Figure 15b) where there are several frontal obstacles.As seen in Figure 15b, the matching result was satisfactory in rectangle 1 but demonstrated a "shortening" effect in rectangle 2. Bringing two point clouds closer was difficult because of lacking necessary frontal obstacles and insufficient correct correspondences that are vital for gaining an optimal transformation in each iteration, thus causing local-minima problems.

Conclusions
This paper presented an improved MbICP algorithm to estimate robot motion to overcome this method's inability to match partially overlapping 2D scans.A resampling step was executed to smooth the greatly varying density distributions of point clouds, which could accelerate the iteration process and protect the follow-up rejection method from becoming distorted.Then, correspondences were established by an improved procedure that combined the simplicity of the point-to-point metric and accuracy of the point-to-line metric.Finally, a rejection threshold that was based on a MAD-from-median approach was used to discard correspondences with large distances, which were likely incorrect correspondences.The results of the first experiment demonstrated that both algorithms were disturbed by a decrease in the overlapping percentage, but the improved algorithm still exhibited better accuracy and robustness.The results of the second experiment indicated that the improved algorithm is valid in practice and exhibits better performance than the standard algorithm.
In future work, our method needs further improvement to be applied under more scenarios, including long and narrow corridors where frontal obstacles are rare.Utilizing geometric features of the point cloud, such as curvature, normal and density, is a possible solution.In addition, our method could be extended to the scan matching of 3D point clouds needs further consideration.

Figure 1 .
Figure 1.(a) Two laser scans that were taken at times and ; (b) object scan (blue) that was taken at is represented in the coordinate system of the reference scan (red), and the dotted lines are the correspondences based on the criterion of the closest point; (c) (blue) is brought closer to (red) as the iterations progress; (d) (blue) closely fits to (red) at the end of the iteration process.

Figure 1 .
Figure 1.(a) Two laser scans that were taken at times t 1 and t 2 ; (b) object scan S obj (blue) that was taken at t 2 is represented in the coordinate system of the reference scan S re f (red), and the dotted lines are the correspondences based on the criterion of the closest point; (c) S obj (blue) is brought closer to S re f (red) as the iterations progress; (d) S obj (blue) closely fits to S re f (red) at the end of the iteration process.

Figure 2 .
Figure 2. Two scans that were captured at the beginning and end of robot motion.The blue scan is the object scan ( ) and the red scan is the reference scan ( ).The dotted lines are the correspondence based on the criterion of the closest point.To overcome the defect of Euclidean distance in merely considering the translation component, MbICP defines a new distance measure called the metric-based distance, which simultaneously considers both translation and rotation.Given two points in , the reference point = ( , )

Figure 2 .
Figure 2. Two scans that were captured at the beginning and end of robot motion.The blue scan is the object scan (S obj ) and the red scan is the reference scan (S re f ).The dotted lines are the correspondence based on the criterion of the closest point.

Figure 3 .
Figure 3. (a) Iso-distance curves: the blue curve is from the metric-based distance, and the red curve is from the Euclidean distance; (b) associations between scans with large scans that are built by the metric-based distance.

Figure 3 .
Figure 3. (a) Iso-distance curves: the blue curve is from the metric-based distance, and the red curve is from the Euclidean distance; (b) associations between scans with large scans that are built by the metric-based distance.

Figure 3 .
Figure 3. (a) Iso-distance curves: the blue curve is from the metric-based distance, and the red curve is from the Euclidean distance; (b) associations between scans with large scans that are built by the metric-based distance.

Figure 6 .
Figure 6.(a) Correspondences that were established based on the point-to-point metric (red dots-reference scan, blue dots-object scan, black lines-correspondences); (b) correspondences that were established based on the point-to-line metric (red dots-reference scan, blue dots-object scan, black lines-correspondences).

Figure 6 .
Figure 6.(a) Correspondences that were established based on the point-to-point metric (red dotsreference scan, blue dots-object scan, black lines-correspondences); (b) correspondences that were established based on the point-to-line metric (red dots-reference scan, blue dots-object scan, black lines-correspondences).

Figure 7 .
Figure 7. (a) Interpolation process (red dots-scanned points in reference scan, blue dots-scanned points in object scan); (b) correspondences that were established by the new procedure (red dots-scanned points in reference scan, blue dots-scanned points in object scan).

Figure 8 .
Figure 8. Correspondences that were established based on the new procedure (red dots-reference scan, blue dots-object scan).

Figure 7 .
Figure 7. (a) Interpolation process (red dots-scanned points in reference scan, blue dots-scanned points in object scan); (b) correspondences that were established by the new procedure (red dots-scanned points in reference scan, blue dots-scanned points in object scan).

Figure 7 .
Figure 7. (a) Interpolation process (red dots-scanned points in reference scan, blue dots-scanned points in object scan); (b) correspondences that were established by the new procedure (red dots-scanned points in reference scan, blue dots-scanned points in object scan).

Figure 8 .
Figure 8. Correspondences that were established based on the new procedure (red dots-reference scan, blue dots-object scan).

Figure 8 .
Figure 8. Correspondences that were established based on the new procedure (red dots-reference scan, blue dots-object scan).

Figure 9 .
Figure 9. (a) Rejection result via the standard-deviations-from-mean approach; (b) rejection result via the MAD-from-median method.

Figure 9 .
Figure 9. (a) Rejection result via the standard-deviations-from-mean approach; (b) rejection result via the MAD-from-median method.

Figure 10 .
Figure 10.(a) Rejection result without the resampling step (red dots-reference scan, blue dots-object scan, black lines-correspondences); (b) rejection result with the resampling step (red dots-reference scan, blue dots-object scan, black lines-correspondences).

Figure 10 .
Figure 10.(a) Rejection result without the resampling step (red dots-reference scan, blue dots-object scan, black lines-correspondences); (b) rejection result with the resampling step (red dots-reference scan, blue dots-object scan, black lines-correspondences).

Figure 11 .Figure 11 .
Figure 11.Two partially overlapping scans taken by sensor at the same location: (a) The first scan was captured with the presence of a trashcan; (b) the second scan was captured without the presence of a trashcan.

Figure 11 .Figure 12 .Figure 13 .
Figure 11.Two partially overlapping scans taken by sensor at the same location: (a) The first scan was captured with the presence of a trashcan; (b) the second scan was captured without the presence of a trashcan.

Figure 15 .
Figure 15.(a) The left scenario is a corridor without frontal obstacles, and the right scenario is a corridor with frontal obstacles; (b) matching result for scans from a long narrow corridor, with frontal obstacles in rectangle 1 and few frontal obstacles in rectangle 2.

Figure 14 .
Figure 14.(a) Matching result from the standard MbICP algorithm; (b) matching result from the improved MbICP algorithm.

Figure 14 .
Figure 14.(a) Matching result from the standard MbICP algorithm; (b) matching result from the improved MbICP algorithm.

Figure 15 .
Figure 15.(a) The left scenario is a corridor without frontal obstacles, and the right scenario is a corridor with frontal obstacles; (b) matching result for scans from a long narrow corridor, with frontal obstacles in rectangle 1 and few frontal obstacles in rectangle 2.

Figure 15 .
Figure 15.(a) The left scenario is a corridor without frontal obstacles, and the right scenario is a corridor with frontal obstacles; (b) matching result for scans from a long narrow corridor, with frontal obstacles in rectangle 1 and few frontal obstacles in rectangle 2.

Table 1 .
Solutions that were generated by the standard algorithm.

Table 2 .
Solutions that were generated by the improved algorithm.

Table 1 .
Solutions that were generated by the standard algorithm.

Table 2 .
Solutions that were generated by the improved algorithm.

Table 3 .
Deviation in the translation and rotation.

Table 3 .
Deviation in the translation and rotation.