An Improved MbICP Algorithm for Mobile Robot Pose Estimation - MDPI

applied sciences Article

An Improved MbICP Algorithm for Mobile Robot Pose Estimation Lin Li 1,2 1 2

*

ID

, Jun Liu 1 , Xinkai Zuo 1 and Haihong Zhu 1, *

School of Resources and Environmental Science, Wuhan University, 129 Luoyu Road, Wuhan 430079, China; [email protected] (L.L.); [email protected] (J.L.); [email protected] (X.Z.) Collaborative Innovation Center of Geo Spatial Technology, Wuhan University, 129 Luoyu Road, Wuhan 430079, China Correspondence: [email protected]; Tel.: +86-27-6877-8879

Received: 20 December 2017; Accepted: 6 February 2018; Published: 12 February 2018

Featured Application: The metric-based iterative closest point algorithm is frequently used for mobile robot pose estimation by scan matching. The improved version can be used for matching scans that partially overlap. Abstract: This paper presents an improved version of the metric-based iterative closest point algorithm to estimate robot poses by matching 2D laser scans with different overlapping percentages. Because of the greatly varied density distribution of realistic point clouds, a resampling method is used to accelerate the iteration process and protect the calculation of the rejection threshold from being distorted by reducing dense but unhelpful points. A new procedure that combines point-to-point and point-to-line metrics is used to determine the correct correspondence between partially overlapping scans, which maintains both efficiency and accuracy. In addition, a rejection threshold that is based on the MAD-from-median method is utilized to discard correspondences with large distances, which are likely to be incorrect. Experiments show that the improved algorithm is more accurate and robust than the standard algorithm with respect to the existence of non-overlapping areas, and testing demonstrates that it is valid in practice. Keywords: 2D laser scans; robot pose; resampling; partially overlapping

1. Introduction Simultaneous localization and mapping (SLAM) is one of the most important issues in mobile robotics research [1–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–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–17]. The iterative closest point (ICP) algorithm is one of the most popular scan-matching techniques because of its efficiency and simplicity [18–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

Appl. Sci. 2018, 8, 272; doi:10.3390/app8020272

www.mdpi.com/journal/applsci

Appl. Sci. 2018, 8, 272

2 of 17

vehicle motion [3,20,23]. False correspondences will arise in non-overlapping areas and thus cause the algorithm to converge upon an incorrect solution [20]. To solve the problem of scan matching with large rotation, Minguez proposed a metric-based ICP algorithm (MbICP) [10] that establishes correspondences and calculates transformations based on its defined metric-based distance, which simultaneously considers both translation and rotation. This algorithm is claimed [1,2,10] to be robust and accurate when the rotation change is large but still limited to matching scans that perfectly overlap. Considerable research has been conducted to improve ICP and its variants in matching partially overlapping scans by enhancing the performance of correspondence establishment and rejecting incorrect correspondences [19,20,22–26]. In terms of establishing correspondence, Chen proposed a point-to-plane approach, which determines the correspondence by finding the projection of the source point onto the tangent plane at a destination surface point that is the intersection of the normal vector of the source point [27], but this approach is only available for scan matching with three-dimension (3D) laser scans. Zhang proposed a biunique correspondence ICP (BC-ICP) [20], which guarantees unique correspondence by searching for multiple closest points instead of one point. However, this strategy assumes that the corresponding point search begins from an overlapping area, which cannot be guaranteed in practice. In terms of rejecting incorrect correspondences, Druon proposed a rejection strategy that was based on a statistical analysis of the correspondence distance, which adopted µ + σ as the rejection threshold [28–30]. Nevertheless, this strategy assumes that the distribution of distances is Gaussian, which is not a reasonable assumption in a dynamic environment [22]. Chetverikov proposed a trimmed ICP algorithm (TrICP) [26], which uses golden-section searching to estimate an overlapping ratio and uses a trimmed least-squares approach to discard incorrect correspondence pairs. Despite the claimed robustness of this algorithm, TrICP is time consuming because it repeatedly searches for the best overlapping ratio [22,24]. This algorithm may also be misled by a large change in the overlap because of occlusions from a moving platform. Pomerleau proposed an adaptive rejection technique called the relative motion threshold (RMT) [22], which calculates the rejection threshold with a simulated annealing ratio. This technique is flawed in that the rejection threshold heavily relies on the translation vector, so a large sensor rotation may produce an incorrect rejection [20]. This paper proposes an improved MbICP algorithm for mobile robot pose estimation by using partially overlapping laser scans from a 2D laser scanner, which is commonly used because of its low cost and effectiveness in terms of data processing. Before matching, a resampling step is executed in the object scan to make the density distribution of point clouds relatively even. In terms of scan matching, finding the nearest point is still based on a metric distance, but the procedure of determining the correspondence is improved by combining the point-to-point and point-to-line metrics. Then, a distance threshold that is based on the MAD-from-median method is calculated to reject correspondences with distances above the threshold. Experiments are designed to compare the performance of the improved algorithm and standard algorithm in terms of robustness, accuracy and convergence. Furthermore, a series of real-world experiments are conducted to test its validity in realistic situations. The paper is organized as follows. In Section 2, the principle of the ICP algorithm and the metric-based distance as defined by MbICP are outlined, followed by a detailed explanation of the resampling method and improved algorithm. The experimental results are presented in Section 3 and discussed in Section 4. The last section describes several conclusions. 2. Scan-Matching Algorithm 2.1. ICP Algorithm Robot motion between time steps can be represented by a transformation vector T, which consists of a translation component ∆t = (∆x, ∆y) and a rotational component ∆θ. Given two successive scans

Appl. Sci. 2018, 8, 272 Appl. Sci. 2018, 8, x FOR PEER REVIEW

3 of 17

3 of 18

successive scans from a laser scanner at the beginning and end of motion, the first scan is marked as

from the a laser scanner beginning andscan endisof motion, theobject first scan as the reference reference scanat the and the second treated as the scan is marked (as shown in Figure 1b). scan Sre f and the second scan is treated as the object scan Sobj (as shown in Figure 1b).

(a)

(b)

(c)

(d)

Figure 1. (a) Two laser scans that were taken at times

and

; (b) object scan

(blue) that was

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

The transformation T can estimatedby by minimizing minimizing the of of thethe corresponding points The transformation T can bebeestimated thedistance distance corresponding points between and with the ICP algorithm, which is based on a three-step iterative process: between Sre f and Sobj with the ICP algorithm, which is based on a three-step iterative process:

1. 2.

thethe closest point in in Sto establish 1. Establish correspondence.For Foreach each point point in Establish correspondence. in Sobj, ,search searchforfor closest point re f to establish correspondence. A K-D tree is recommended to accelerate the searching process. 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 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 translation vector t is calculated by minimizing mean square error of minimization the correspondence pairs between and (Equation (1)). the SVD-based least-squares is pairscommonly between used Sobj and S (Equation (1)). SVD-based least-squares minimization is commonly re f of its robustness and simplicity [31]. because

used because of its robustness and simplicity [31]. =( , )=

min{

−

−

}

N 2 T = ( R, t) = argmin{ ∑ m j − Rpi − t }

(1)

(1)

where is the number of points in , ∈ i=1 and ∈ . is the closest point of , which is defined as the correspondence. where N is the number points in Sobj , pi ∈toSobj and m j is the closest point of pi , j ∈ Sre f . related 3. Transform . Applyof a rigid transformation andm update the parameters.

3.

which is defined as the correspondence. Transform Sobj . Apply a rigid transformation to Sobj and update the related parameters.

These three steps are repeated until convergence or the iteration number exceeds the maximum number of iterations.

Appl. Sci. 2018, 8, x FOR PEER REVIEW

4 of 18

Appl. Sci. 2018, 8, 272

4 of 17

These three steps are repeated until convergence or the iteration number exceeds the maximum number of iterations.

2.2. Metric-based Distance

2.2. Metric-based Distance

Assume that two successive laser scans with large rotation are given (Figure 2): the reference Assume two successive rotation are given (Figure 2): the reference scan Sre f with n p that points, P = {laser pi , i scans = 1, .with . . , nlarge p }, and the object scan Sobj with nm points, scan with points, = { , = 1, … , }, and the object scan with points, = M = {m j , j = 1, . . . , nm }. When establishing the correspondence, the standard ICP algorithm searches { , = 1, … , }. When establishing the correspondence, the standard ICP algorithm searches for for the closest point in M each point theframework framework Euclidean space. Points in the closest point in for M for each pointin inP, which , whichis isunder under the of of Euclidean space. Points P that are far from the center are also far from their actual correspondence points because of the in that are far from the center are also far from their actual correspondence points because of theangular angular difference, which easily false correspondence ICP failure. finding closestpoint in difference, which easily creates falsecreates correspondence and ICPand failure. Thus,Thus, finding thethe closest point in Euclidean space is not for establishing correspondence when large rotationexists. exists. Euclidean space is not reliable for reliable establishing correspondence when large rotation

Figure 2. Two 2.scans captured at the beginning robotmotion. motion. The blue Figure Two that scanswere that were captured at the beginningand andend end of of robot The blue scanscan is is the object scan (S ) and the red scan is the reference scan (S ). The dotted lines are the correspondence ) and the red scan is the reference ). The dotted lines are the the object obj scan ( re f scan ( based on the criterionbased of theonclosest point.of the closest point. correspondence the criterion To overcome the defect of Euclidean distance in merely considering the translation component,

To MbICP overcome the defect of Euclidean distance in merely considering the translation component, defines a new distance measure called the metric-based distance, which simultaneously MbICP considers defines both a new distance calledtwo thepoints metric-based distance,point which=simultaneously translation andmeasure rotation. Given in , the reference ( , ) 2 , the reference point p = p , p considers both translation and rotation. Given two points in R and the object point =( , ). A set of rigid-body transformations = ( , i, ) canix be iy and acquired (Equation (2)). the object pointthat m j satisfies = m jx ,q(m jy) = . A set of rigid-body transformations q = ( x, y, θ ) can be acquired that satisfies q m j = pi (Equation (2)). + × − × (

)=

+

×

+

(2)

×

!

The metric distance q (m, j transformations (Equation (3)).

x + cosθand × p1x is −defined sinθ ×as p1y ) between the minimum norm among the = y + sinθ × p1x + cosθ × p1y

(2)

( ), ( , )= = (3) The metric distance d pi , m j between m j and pi is defined as the minimum norm among the Where transformations (Equation (3)). (4) ( )= + + d p , m = min { norm q , q m = p } ( ) j i L is a balance parameteri thatj is homogeneous to a length, which is used to balance the

(3)

translational and rotational components. is depicted by using the metric-based A set of iso-distance curves relative to q and x 2 + y2 + L2 (4) (q) = represent distance and Euclidean distancenorm to visually theθ 2geometric features of metric-based distance (Figure 3a). The iso-distance curves that use the metric-based distance are claimed to be L isellipses a balance parameter thatwhile is homogeneous a length, which is used to balance the which translational in the literature [10], those that useto the Euclidean distance are inscribed circles, and rotational components. provides the metric-based distance a larger search zone than that of the Euclidean distance for the same radius. The farther the object the center,by theusing longerthe themetric-based long axis of the A set of search iso-distance curves relative to Pipoint and isMfrom distance j is depicted

where

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.


5 of 18


5 of 18

ellipse becomes and, therefore, the more likely the object point will find its correspondence point in rotational This feature the metric-based distance probability of finding Appl. the Sci.ellipse 2018, 8, 272direction. 5 of 17 becomes and, therefore, theofmore likely the object pointimproves will find the its correspondence point in thethe correct correspondence for points aremetric-based far from the distance sensor, asimproves shown inthe Figure 3b. rotational direction. This featurethat of the probability of finding the correct correspondence for points that are far from the sensor, as shown in Figure 3b.

(a)

(b)

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

2.3. Resampling

2.3. Resampling 2.3. Resampling

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

a resampling method that is based on grid partitioning to realize a relatively even density distribution. even density distribution.

(a)

(b)

(a) (b) Figure 4. (a) Different density distributions of point clouds; (b) spatial partition. Figure 4. (a) Different density distributions of point clouds; (b) spatial partition.

Figure 4. (a) Different density distributions of point clouds; (b) spatial partition.

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 (r1 , c1 ) and (r2 , c2 ), respectively, the distance d between the grids 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. Appl. Sci. 2018, 8,we 272calculate a sampling percentage for points in each grid. Given two occupied 6grids of 17 Second, whose row and column indices are ( , ) and ( , ), respectively, the distance between the q grids is defined as 2( − ) +( 2 − ) . The sensor is contained in the center grid, so its row and defined (r1 −are r2 )both + (czero. . The sensor is contained center grid, so its column 1 − c2 )We column as indices calculate the distancein the from each grid to row the and center grid indices are both zero. We calculate the distance d from each grid to the center grid according to the according to the distance definition and obtain i the max distance ( ). The sampling ratio for distance definition and obtain the max distance (d ). The sampling ratio for points in each grid is max points in each grid is thus defined as the ratio of and d , which guarantees that distant grids thus defined as the ratio of d and d , which guarantees that distant grids from the center grid have max i from the center grid have a relatively high sampling percentage and nearby grids have a low asampling relativelypercentage. high sampling percentage and nearby grids have a low sampling percentage. Finally, points in the of the of grids to the assigned Finally,we weresample resample points in units the units theaccording grids according to thesampling assignedpercentage. sampling 0 is calculated as the ceiling Given a grid with n points and sampling percentage η, the sampling size n percentage. Given a grid with n points and sampling percentage , the sampling size n’ is of n × η. To as of much×topological information possible for the points,asthe first and calculated aspreserve the ceiling . To preserve as muchastopological information possible for last the 0 − points are preserved and the remaining n 2 points are sampled at regular intervals, as shown in points, the first and last points are preserved and the remaining n’ − 2 points are sampled at regular Figure 5a. intervals, as shown in Figure 5a. This is based on a grid which determines the sampling This resampling resamplingmethod method is based on apartition, grid partition, which determines thepercentage sampling according to the distance from the occupied grid to the center grid and then samples points in the percentage according to the distance from the occupied grid to the center grid and then samples units of the grid according to the topological information. This method can effectively dilute dense points in the units of the grid according to the topological information. This method can effectively points retain their and topological therefore accelerating the iteration process and dilute nearby dense and points nearby retain information, their topological information, therefore accelerating the preventing the calculation of the rejection threshold from becoming distorted by the greatly varying iteration process and preventing the calculation of the rejection threshold from becoming distorted density distributions point clouds. by the greatly varyingofdensity distributions of point clouds.

(a)

(b)

Figure 5. (a) in grid grid form; form; (b) (b) resampled resampled point point clouds. clouds. Figure 5. (a) Resampling Resampling process process in

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


7 of 18

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 Appl. Sci. 2018, 8,area 272 2 in Figure 6a. These incorrect correspondences will distort the follow-up calculation7of of 17 the optimal transformation and thus negatively affect the final matching result.

(a)

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

To reduce problems that are introduced by the point-to-point metric, this paper improves the To reduce thatthe arecorrespondence introduced by by thecombining point-to-point metric, this paper improves the procedure of problems determining the point-to-point and point-to-line procedure of determining the correspondence by scan combining the point-to-point metrics. First, the nearest point in the reference is determined for each point andinpoint-to-line the object scan byFirst, using thenearest metric-based distance, and ( , ) is treated as a candidate correspondence metrics. the point m in the reference scan is determined for each point p in thepair. object j i Forby those candidate correspondence pairsand whose correspond to a reference point, only scan using the metric-based distance, pi ,object m j ispoints treated as a candidate correspondence pair. paircandidate with the smallest corresponding distance is preserved and the remaining pairs are modified Forthe those correspondence pairs whose object points correspond to a reference point, only the bywith usingthe ansmallest interpolation method. Taking ( ,is preserved ) as an example, search forpairs the second-closest pair corresponding distance and thewe remaining are modified by point for in the reference scan, and a segment that begins at and ends at is found. We using an interpolation method. Taking pi , m j as an example, we search for the second-closest point 0 0 m j for pi in the reference scan, and a segment that begins at m j and ends at m j is found. We calculate the projection of pi 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.


8 of 18

calculate the projection of onto the segment along the normal and use this interpolation point as a Appl. Sci. 2018, 8, x FOR PEER REVIEW 8 of 18 new Appl. corresponding Sci. 2018, 8, 272 point (as shown in Figure 7a). This interpolation is less risky because it only 8 of 17 utilizes the surface information thethe reference calculate the projection of of onto segmentpoint along clouds. 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.

(a)

(b)

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

This new procedure can improve the performance of correspondence establishment, especially This This new new procedure can improve the performance of correspondence establishment, procedure can improve the performance of correspondence establishment,especially especiallywhen when non-overlapping areas exist (as shown in Figure 8). However, a few incorrect correspondence when non-overlapping areas (as shown in Figure 8). However, a few incorrect correspondencepairs non-overlapping areas exist (as exist shown in Figure 8). However, a few incorrect correspondence pairs might be established because of the complexity of the environment. Thus, a rejection strategy might be established of the complexity the environment. Thus, a rejection strategy mightpairs be established because because of the complexity of the of environment. Thus, a rejection strategy will be will bewill explained in the next section. be explained in the next section. explained in the next section.

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

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

AfterAfter establishing basedonon procedure, set of correspondence establishingcorrespondence correspondence based thethe newnew procedure, a set ofacorrespondence pairs 2.4.2. Rejection of Incorrect Correspondences are obtained, which may contain a few incorrect correspondence pairs with large distance. pairs are obtained, which may contain a few incorrect correspondence pairs with large distance. Therefore, a rejection strategy thatisisbased based on the corresponding distance is required to protect thepairs After establishing correspondence based on the new procedure, a set of correspondence Therefore, a rejection strategy that on the corresponding distance is required to protect the follow-up calculation from transformation. are obtained, which from may transformation. contain a few incorrect correspondence pairs with large distance. follow-up calculation One of the most commonly used methods is the standard-deviations-from-mean approach, Therefore, a rejection that is based on the corresponding distance is required to protect the One of the moststrategy 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 follow-up calculation from transformation. which removes values that are more than n standard deviations from the mean. The value of n decided according to the practical situation. However, this method is unreliable because extreme One ofaccording the distort most to commonly methods is the standard-deviations-from-mean approach, is decided the practical situation. However, this method is unreliable because extreme values may the mean andused standard deviation. which values that more than n standard deviations the mean. value of n is valuesremoves may thegiven meanare standard deviation. Fordistort example, a and sequence of correspondence distances, Xfrom = {12.281, 12.270, The 12.712, 11.932, decided according to the practical situation. However, this method is unreliable because extreme 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, values may distort the mean and standard deviation. 12.477, 12.381}. The rejection result when using the standard-deviations-from-mean approach 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,is6.409, depicted in aThe dot plot shown in when Figure 9a). The mean is approximately 14.93, which12.712, obviously For example, given a (as sequence of correspondence distances, X = {12.281, 12.270, 11.932,is 12.477, 12.381}. rejection result using the standard-deviations-from-mean approach 11.053, 10.768, 6.393,in 6.001, 5.549, 86.305, 34.497, 2.988, 3.227, 1.297, 3.539, 6.409, depicted in a 11.077, dot plot11.685, (as shown Figure 9a).38.760, The mean is approximately 14.93, which obviously 12.477, 12.381}. Thecenter rejection result using the standard-deviations-from-mean approach is deviates from the of the datawhen set, and the standard deviation is also considerably distorted. depicted in aisdot plot (as shown in Figure The mean is 34.497 approximately 14.93, which obviously Only 86.305 rejected as an extreme value, 9a). while 38.760 and are ignored even though they are obviously also extreme values.


9 of 18

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 Appl. Sci. 2018, 8, 272 9 of 17 are obviously also extreme values.

(a)

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

The MAD-from-median method is another rejection method, which removes values that are The MAD-from-median method is another rejection method, which removes values that are more more than two median absolute deviations (MAD) from the median. Instead of utilizing the mean than two median absolute deviations (MAD) from the median. Instead of utilizing the mean and and standard deviation to measure the center and discreteness degree of the data set, this method standard deviation to measure the center and discreteness degree of the data set, this method uses the uses the median and MAD to represent the clustering and statistical dispersion of the data set. The median and MAD to represent the clustering and statistical dispersion of the data set. The median is median is not greatly skewed by extreme values and thus can provide a better idea of the typical not greatly skewed by extreme values and thus can provide a better idea of the typical value than the value than the mean can. The MAD, which is defined as the median of the absolute deviations from mean can. The MAD, which is defined as the median of the absolute deviations from the median of the median of the data set, is also robust to extreme values. 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 The rejection result when using the MAD-from-median method is depicted in Figure 9b. median is 11.07, which is a good representation of the center. Here, 38.760, 86.305 and 34.497 are all The median is 11.07, which is a good representation of the center. Here, 38.760, 86.305 and 34.497 are rejected as extreme values. Thus, the MAD-from-median method can more clearly differentiate 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. extreme values and is herein adopted to reject correspondences with large distances. d = median( ) + 2 × (5) drej = median( X ) + 2 × MAD (5) Therefore, we calculate the rejection threshold d by using Equation (5) and reject correspondences with distances above d , which are likely incorrect correspondences. This Therefore, we calculate the rejection threshold drej by using Equation (5) and reject rejection strategywith is risky because d dmay be distorted by the greatly varying density distributions correspondences distances above rej , which are likely incorrect correspondences. This rejection of point clouds. When a mobile platform moves forward in varying a long narrow pointofclouds strategy is risky because drej may be distorted by the greatly density corridor, distributions point tend to be quite dense near the sensor but sparse in more distant regions from the sensor. However, clouds. When a mobile platform moves forward in a long narrow corridor, point clouds tend to be quite the object cloudsbut cannot bein moved the reference point clouds becausethe of object the absence dense nearpoint the sensor sparse more closer distanttoregions from the sensor. However, point of an initial pose from the odometer. The dense points that are distributed within the corridor clouds cannot be moved closer to the reference point clouds because of the absence of an initial pose (rectangle 2 in Figure 10a) can hardly find actual correspondence points and are likely to establish from the odometer. The dense points that are distributed within the corridor (rectangle 2 in Figure 10a) incorrect with minor distances, which unhelpful but arecorrespondences in the majority.with The can hardlycorrespondences find actual correspondence points and are likelyare to establish incorrect sparse distances, points thatwhich are distributed within in Figure likely to minor are unhelpful butthe arefrontal in the obstacles majority. (rectangle The sparse1 points that10a) are are distributed establish correct correspondences with relatively large distances, which are vital to bring two point within the frontal obstacles (rectangle 1 in Figure 10a) are likely to establish correct correspondences clouds closer but are in the minority. Therefore, directly rejecting correspondence pairs that have 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 drej but that are vital for


10 of 18

Appl. Sci. 2018, 8, 272

10 of 17

distances above d 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 calculating the transformation, may cause potential local-minima problems. Fortunately, this risk is d can effectively reject correspondences with extremely large distances and preserve vital correct reduced by the resampling method in chapter correspondences (rectangle 1 in Figure 10b) 2.3, so drej can effectively reject correspondences with

extremely large distances and preserve vital correct correspondences (rectangle 1 in Figure 10b).

(a)

(b)

Figure 10. (a) Rejection result without the resampling step (red dots-reference scan, blue dots-object

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,scan, black lines-correspondences); (b) rejection result with the resampling step (red dots-reference blue dots-object scan, black lines-correspondences). scan, blue dots-object scan, black lines-correspondences).

3. Results

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

that partially overlapped. The second experiment evaluated the global algorithm performance with Evaluation Method building. a run3.1. within our academic 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 experiment, we first explain the The first experiment compared thethe accuracy and convergence of the the evaluation improved factors MbICPofalgorithm accuracy and convergence. Given two scans that were acquired in the same sensor location, we with those of the original MbICP algorithm in matching partially overlapped scans. Before introducing precisely know that the ground truth of the pose estimate is (0, 0, 0°). To quantitatively compare the more details regarding the experiment, we first explain the evaluation factors of the accuracy and algorithms’ accuracy, a random displacement is added to the initial pose estimate, and one scan convergence. Given two scans that were acquired in the same sensor location, we precisely know that is also roto-translated by this value. This transformed scan is treated as the object scan, which is the ground truth of, the pose estimate is (0, 0, 0◦ ). To quantitatively compare the algorithms’ accuracy, marked as while the other scan is treated as the reference scan, which is marked as . After a random displacement δ is added to the initial pose estimate, and one scan is also roto-translated matching, the pose estimation is determined and statistical analysis can be performed. The by this value. This transformed scan is treated as the object scan, which isfrom marked as Sobj , while translational and rotational deviation between the pose estimation the algorithm and the the other scan ground is treated asisthe reference scan, which as Sre f .accuracy. After matching, thethe pose estimation truth used as the evaluative factorisofmarked the algorithm’s The smaller deviation,

3.1. Evaluation Method

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 Sobj fits Sre f and the better the accuracy of the algorithm. In addition, the number of iterations can intuitively represent the algorithm’s convergence.


11 of 18

the closely fits and the better the accuracy of the algorithm. In addition, the number Appl.more Sci. 2018, 8, 272 11 of 17 Appl. Sci.can 2018,intuitively 8, x FOR PEERrepresent REVIEW 11 of 18 of iterations the algorithm’s convergence. We conducted the first experiment by matching two partially overlapping scans with the theconducted more closely fits and the the accuracy of the overlapping algorithm. In scans addition, the number We first experiment by better matching two partially with the improved improved algorithmthe and standard algorithm. To obtain experimental scans, we recorded data taken of iterations can intuitively represent the algorithm’s convergence. algorithm and standard algorithm. To obtain experimental scans, we recorded data taken by UTM-30LX by UTM-30LX laser range finder in a corner by artificially adding or removing the trashcan in the We conducted the first experiment by matching two partially overlapping scans with the laserof range finder in amoving corner by artificially adding orwe removing the trashcan in the field of viewoverlap, without field view without the sensor. This way acquired a pair of scans that partially improved algorithm and standard algorithm. To obtain experimental scans, we recorded data taken moving the sensor. This way we acquired a pair of scans that partially overlap, oneother of which with the one ofbywhich with the of the shown in Figure and the is shown UTM-30LX laserpresence range finder in atrashcan corner byisartificially adding or 11a removing the trashcan in the in presence of the trashcan is shown in Figure 11a and the other is shown in Figure 11b. The scan with Figurefield 11b.ofThe withmoving this trashcan was roto-translated with a given transformation (500 mm, viewscan without the sensor. This way we acquired a pair of scans that partially overlap, ◦ thismm, trashcan wasthen roto-translated a scan, given transformation (500 mm, 500 mm, 15 )isand thenas one15°) of which withused the presence of the trashcan is the shown inwithout Figure 11a the other shown inused 500 and as thewith object while scan theand trashcan was treated the as the object scan, while the scan without the trashcan was treated as the reference scan (as shown in Figure 11b. The scan with this trashcan was roto-translated with a given transformation (500 mm, reference scan (as shown in Figure 12a). The matching result from the standard algorithm is depicted 500 mm, 15°) and is then assatisfactory the object whilealgorithm the without the trashcan was treated as theisThe Figure 12a). matching result from the scan, standard is depicted inbetween Figure 12b, far in Figure 12b,The which farused from because of scan obvious deviation twowhich scans. reference scan (as shown in Figuredeviation 12a). The matching result from the standard algorithm isthe depicted from satisfactory because of obvious between two scans. The distance between estimated distance between the estimated pose and the ground truth was (75.75 mm, 34.69 mm, 2.71°), ◦ ), demonstrating in Figure 12b, which iswas far from satisfactory because of obvious deviation between two scans.However, The pose and the ground (75.75 mm, 34.69 mm, 2.71 awful accuracy. demonstrating awfultruth accuracy. However, the matching result from the improved algorithm was distance between the estimated pose and the ground truth was (75.75 mm, 34.69 mm, 2.71°), the matching result from the improved algorithm relatively satisfactory because the deviation two scans relatively satisfactory because the two scans closelywas matched (as shown in Figure 12c). The demonstrating awful accuracy. However, the matching result from the improved algorithm was closely matched (as shown in Figure 12c). The deviation between the estimated pose and ground between the estimated pose and the ground truthclosely was nearly demonstrating high relatively satisfactory because two scans matchedzero, (as shown in Figure 12c). Theaccuracy. deviation The truth was nearly zero, demonstrating high accuracy. The improved algorithm also exhibited aoffaster improved algorithm also pose exhibited a faster rate (18 iterations) because The the between the estimated and ground truth convergence was nearly zero, demonstrating high accuracy. convergence rate (18 iterations) because of the resampling step, while the standard algorithm satisfied resampling step,algorithm while the also standard algorithm satisfied the convergence conditionbecause after 22of iterations improved exhibited a faster convergence rate (18 iterations) the the convergence condition after 22 iterations (Figure 12d). resampling step, while the standard algorithm satisfied the convergence condition after 22 iterations (Figure 12d). (Figure 12d).

(a)

(b)

(a)

(b)

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

of a trashcan. a trashcan.

(a)

(b) Figure 12. Cont.

(a)

(b)

2018, 8, xx FOR FORPEER PEERREVIEW REVIEW Appl. Sci. 2018, 8, Appl.Appl. Sci. Sci. 2018, 8, 272

12 of 18 12 of 18 12 of 17

(c)

(d)

(c)

(d)

Figure 12. (a) Object scan (blue) and reference scan (red); (b) matching result from the standard Figure 12. (a) Object scan (blue) and reference scan (red); (b) matching result from the standard Figure (a) Object (blue) and reference scan (red); (b) object matching result from theresult standard MbICP 12. algorithm (red scan dots-reference scan, blue dots-transformed scan); (c) matching MbICP algorithm (red dots-reference scan, blue object (c) matching matching result MbICP dots-reference scan, bluedots-transformed dots-transformed object scan); scan); (c) from thealgorithm improved(red MbICP algorithm (red dots-reference scan, blue dots-transformed object scan);result fromfrom the improved MbICP algorithm (red objectscan); scan); the improved MbICP algorithm (reddots-reference dots-referencescan, scan, blue blue dots-transformed dots-transformed object (d) iteration process.

(d) iteration process. (d) iteration process.

Another group of experiments was conducted to quantitatively evaluate the algorithms’ accuracy matching scans with different overlapping ratios. Given an overlapping percentage η% Another group of experiments was conducted to quantitatively evaluate the algorithms’ Anotheringroup of experiments was conducted to quantitatively evaluate the algorithms’ accuracy and two identical scans, one scan had 1 − η%overlapping of the pointsratios. artificially removed, while the percentage other scan η% accuracy in matching scans with different Given an overlapping in matching scans with different overlapping ratios. Given an overlapping percentage η% and two remained unchanged from the object scan. Thus, we points obtained pairs of scans withwhile an overlapping and two identical scans, one scan 1 −points η% of the artificially scan identical scans, one scan had 1− η%had of the artificially removed, removed, while the other the scanother remained percentage η, which varied from 60% to 100%. These scans were acquired under different scenarios, remained unchanged from the object scan. Thus, we obtained pairs of scans with an overlapping unchanged fromand the corners, object scan. Thus, we obtained pairs of scans withofan overlapping percentage η, such as stairs where frequently occurs because range holes and rotation, percentage η, which varied fromoverlapping 60% to 100%. These scans were acquired under different scenarios, which varied from 60% to 100%. These scans were acquired under different scenarios, such as stairs and corridors, where dynamic obstacles are common (as shown Figureof13). We holes repeated such as stairs and corners, where overlapping frequently occurs in because range and each rotation, and matching corners, where overlapping frequently occurs because range holes andpercentage rotation, and and each corridors, experiment 10 times with these two algorithms forofeach overlapping and corridors, where dynamic obstacles are common (as shown in Figure 13). We repeated each where dynamic obstacles are common shown in Figure were 13). We repeated each matching experiment scenario (a total of 400 runs), and the (as common parameters identical.

matching experiment 10 times with these two algorithms for each overlapping percentage and each 10 times with these two algorithms for each overlapping percentage and each scenario (a total of scenario (a total of 400 runs), and the common parameters were identical. 400 runs), and the common parameters were identical.

(a)

(b)

(a)

(b) Figure 13. Cont.

Appl. Sci. 2018, 8, x FOR PEER REVIEW Appl. Sci. 2018, 8, 272

(c)

13 of 18

13 of 17

(d)

Figure 13. (a) Scans obtained in corridor 1 (red dots-reference scan, blue dots-object scan); (b) scans Figure 13. (a) Scans obtained in corridor 1 (red dots-reference scan, blue dots-object scan); (b) scans obtained near stairs (red dots-reference scan, blue dots-object scan); (c) scans obtained in corridor 2 obtained near stairs (red dots-reference scan, blue dots-object scan); (c) scans obtained in corridor 2 (red dots-reference scan, blue dots-object scan); (d) scans obtained in the corner (red dots-reference (red dots-reference scan, blue dots-object scan); (d) scans obtained in the corner (red dots-reference scan, blue dots-object scan). scan, blue dots-object scan).

We first discuss the results in terms of robustness. A scan match was considered a failure when Wesolution first discuss the results inm terms of robustness. A scan match was considered a error failure when the was larger than 0.1 in translation and 3.14° in rotation. Solutions with an below ◦ the the solution was larger than in 0.1amfollow-up in translation and analysis. 3.14 in rotation. Solutions with antoerror below threshold were used precision True Positives (converged the right the solution), thresholdFalse were used in a follow-up precision to the Positives (converged to an incorrectanalysis. solution), True True Positives Negatives (converged (not converged but right the solutionFalse is correct) and False Negatives converged and the solution is incorrect) are frequently solution), Positives (converged to an(not incorrect solution), True Negatives (not converged but the used robustness characteristics. solution is correct) and False Negatives (not converged and the solution is incorrect) are frequently used For characteristics. the standard algorithm, the true positives decreased and the false positives increased as the robustness overlapping percentage decreased, as shown in Table 1. Thus, of the standardas For the standard algorithm, the true positives decreased andthe therobustness false positives increased algorithm was influenced by non-overlapping areas between scans. The improved algorithm the overlapping percentage decreased, as shown in Table 1. Thus, the robustness of the standard showedwas good robustness all the solutions were true positives despite aalgorithm decrease showed in the algorithm influenced by because non-overlapping areas between scans. The improved overlapping percentage, as shown in Table 2. good robustness because all the solutions were true positives despite a decrease in the overlapping

percentage, as shown in Table 2.

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

(%) Overlapping TableRatio 1. Solutions that were generatedSolutions by the standard algorithm. (%) True Positive False Positive True Negative False Negative 100 100 0 0 0 Solutions (%) Overlapping Ratio 90 (%) 100 0 0 0 True Positive False Positive True Negative False Negative 80 90 10 0 0 100 70 100 0 0 70 25 0 0 5 90 60 100 0 0 55 37.50 0 7.5 80 90 10 0 0 70 70 25 0 5 Table 2. Solutions that were generated by the improved algorithm. 60 55 37.5 0 7.5 Solutions (%) Overlapping Ratio (%) True Positive False Positive Negative False Negative Table 2. Solutions that were generated by theTrue improved algorithm. 100 100 0 0 0 90 100 0 0 0 Solutions (%) 80 (%) 100 0 0 0 Overlapping Ratio True Positive False True False Negative 70 92.5 7.5Positive 0 Negative 0 90 7.5 0 0 2.5 100 60 100 0 0 90 100 0 0 0 80 0 0 0 We calculated the deviation 100 between the estimated pose and ground truth to indicate the 70 92.5 7.5 0 0 precision (only using True Positives), as shown in Table 3. Both algorithms were accurate when two 60 90 7.5 0 2.5

Appl. Sci. 2018, 8, 272

14 of 17

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 Appl. Sci. 2018, 8, x FOR PEER REVIEW 14 of 18 the scans partially overlapped. Moreover, a decline in the overlap ratio substantially decreased the performance of the standard but had no significant effect on the improved algorithm. scans perfectly overlappedalgorithm but experienced increasing deviation in their translation and rotation as the overlap ratio decreased. However, the improved algorithm was more accurate than the standard Table 3. Deviation in the translation and rotation. 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. Translation Error (mm) Rotation Error (◦ )

Overlapping Ratio (%)

Improved

Standard

0

0

Improved

Standard

0

0

Table 3. Deviation in the translation and rotation.

100

Overlapping Ratio 90 (%) 80 100 70 6090 80 70 3.2. Real-World Applications 60

Translation Error (mm) 1.373 6.518 Improved Standard 5.289 38.940 0 0 12.142 43.226 1.373 6.518 18.582 64.040 5.289 38.940 12.142 43.226 18.582 64.040

Rotation Error (°) 0.022 0.279 Improved Standard 0.113 0.832 0 01.528 0.283 0.022 0.279 0.657 1.892 0.113 0.832 0.283 1.528 0.657 1.892

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

(a)

Figure 14. Cont.


15 of 18


15 of 18

Appl. Sci. 2018, 8, 272

15 of 17

(b)

(b)

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

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

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

(a)

(b)

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.

(a)

(b)

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

4. 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

Appl. Sci. 2018, 8, 272

16 of 17

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, how our method could be extended to the scan matching of 3D point clouds needs further consideration. Acknowledgments: This study is funded by the National Key R&D Program of China (2017YFB0503701), the Scientific and Technological Leading Talent Fund of the National Administration of Surveying, Mapping and Geo-information (2014) and the Wuhan ‘Yellow Crane Excellence’ (Science and Technology) program (2014). Author Contributions: This study was completed by the co-authors, and the major experiments and analyses were undertaken by Jun Liu. Lin Li supervised and guided this study, Haihong Zhu designed the proposal for the experiments and conducted the analyses, Xinkai Zuo aided in the collection and analysis of the data, and Jun Liu wrote the paper. All the authors have read and approved the final manuscript. Conflicts of Interest: The authors declare no conflicts of interest.

References 1.

2.

3.

4. 5.

6. 7.

8.

9. 10. 11.

Lv, J.; Yukinori, K.; Ravankar, A.A.; Ravankar, A.; Emaru, T. A solution to estimate robot motion with large rotation by matching laser scans. In Proceedings of the 54th Annual Conference of the Society of Instrument and Control Engineers of Japan, Hangzhou, China, 28–30 July 2015. Liu, Y.; Sun, Y. Mobile robot instant indoor map building and localization using 2d laser scanning data. In Proceedings of the International Conference on System Science and Engineering, Dalian, China, 30 June–2 July 2012. Yin, J.; Carlone, L.; Rosa, S.; Anjum, M.L.; Bona, B. Scan matching for graph slam in indoor dynamic scenarios. In Proceedings of the International Conference of the Florida Artificial Intelligence Research Society, Pensacola Beach, FL, USA, 21–23 May 2014. Diosi, A.; Kleeman, L. Fast laser scan matching using polar coordinates. Int. J. Rob. Res. 2007, 26, 1125–1153. [CrossRef] Dissanayake, M.W.M.G.; Newman, P.; Clark, S.; Durrant-Whyte, H.F.; Csorba, M. A solution to the simultaneous localization and map building (slam) problem. IEEE Trans. Rob. Autom. 2001, 17, 229–241. [CrossRef] Berns, K.; Puttkamer, E.V. Simultaneous Localization and Mapping (Slam); Springer: Berlin, Germany, 2009; pp. 146–172. Pedrosa, E.; Pereira, A.; Lau, N. Efficient localization based on scan matching with a continuous likelihood field. In Proceedings of the IEEE International Conference on Autonomous Robot Systems and Competitions, Coimbra, Portugal, 26–28 April 2017. Wang, X.; Jia, Y.; Xi, N.; Zhen, J.; Xu, F. Mobile robot pose estimation using laser scan matching based on fourier transform. In Proceedings of the IEEE International Conference on Robotics and Biomimetics, Bali, Indonesia, 5–10 December 2014. Park, S.; Park, S.K. Global localization for mobile robots using reference scan matching. Int. J. Control Autom. Syst. 2014, 12, 156–168. [CrossRef] Minguez, J.; Montesano, L.; Lamiraux, F. Metric-based iterative closest point scan matching for sensor displacement estimation. IEEE Trans. Rob. 2006, 22, 1047–1054. [CrossRef] Montesano, L.; Minguez, J.; Montano, L. Probabilistic scan matching for motion estimation in unstructured environments. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Edmonton, AB, Canada, 2–6 August 2005.

Appl. Sci. 2018, 8, 272

12.

13. 14.

15.

16. 17. 18. 19. 20.

21.

22. 23. 24. 25. 26.

27. 28. 29.

30.

31.

17 of 17

Burguera, A.; Oliver, G.; Tardos, J.D. Robust scan matching localization using ultrasonic range finders. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Edmonton, AB, Canada, 2–6 August 2005. Park, S.; Kim, D.H.; Park, M.; Park, S.K. Spectral scan matching for robot pose estimation. Electron. Lett. 2009, 45, 1076–1077. [CrossRef] Pfister, S.T.; Kriechbaum, K.L.; Roumeliotis, S.I.; Burdick, J.W. Weighted range sensor matching algorithms for mobile robot displacement estimation. In Proceedings of the IEEE International Conference on Robotics and Automation, Washington, DC, USA, 11–15 May 2002. Bengtsson, O.; Baerveldt, A.J. Localization by matching of range scans—Certain or uncertain? In Proceedings of the Eurobot’01—Fourth European Workshop on Advanced Mobile Robots, Lund, Sweden, 19–21 September 2001. Feng, L.; Milios, E. Robot pose estimation in unknown environments by matching 2d range scans. J. Intell. Robot. Syst. 1997, 18, 249–275. Jensen, B.; Siegwart, R. Scan alignment with probabilistic distance metric. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Sendai, Japan, 28 September–2 October 2004. Donoso, F.A.; Austin, K.J.; McAree, P.R. How do ICP variants perform when used for scan matching terrain point clouds? Robots Autom. Syst. 2017, 87, 147–161. [CrossRef] Pomerleau, F.; Colas, F.; Siegwart, R.; Magnenat, S. Comparing ICP variants on real-world data sets. Auton. Robots 2013, 34, 133–148. [CrossRef] Zhang, L.; Choi, S.I.; Park, S.Y. Robust icp registration using biunique correspondence. In Proceedings of the International Conference on 3d Imaging, Modeling, Processing, Visualization and Transmission, Hangzhou, China, 16–19 May 2011. Zhu, J.; Zheng, N.; Yuan, Z.; Du, S. Point-to-line metric based iterative closest point with bounded scale. In Proceedings of the 4th IEEE Conference on Industrial Electronics and Applications, Xian, China, 25–27 May 2009. Pomerleau, F.; Colas, F.; Ferland, F.; Michaud, F. Relative Motion Threshold for Rejection in ICP Registration; Springer: Berlin/Heidelberg, Germany, 2009; pp. 229–238. Phillips, J.M.; Liu, R.; Tomasi, C. Outlier robust icp for minimizing fractional rmsd. In Proceedings of the Sixth International Conference on 3-D Digital Imaging and Modeling, Montreal, QC, Canada, 21–23 August 2007. Zhu, J.; Meng, D.; Li, Z.; Du, S.; Yuan, Z. Robust registration of partially overlapping point sets via genetic algorithm with growth operator. IET Image Proc. 2014, 8, 582–590. [CrossRef] Kim, H.Y.; Lee, S.O.; You, B.J. Robust laser scan matching in dynamic environments. In Proceedings of the 2009 IEEE International Conference on Robotics and Biomimetics, Guilin, China, 19–23 December 2009. Chetverikov, D.; Svirko, D.; Stepanov, D.; Krsek, P. The trimmed iterative closest point algorithm. In Proceedings of the 16th International Conference on Pattern Recognition, Quebec City, QC, Canada, 11–15 August 2002. Chen, Y.; Medioni, G. Object modeling by registration of multiple range images. In Proceedings of the IEEE International Conference on Robotics and Automation, Sacramento, CA, USA, 9–11 April 1991. Rusinkiewicz, S.; Levoy, M. Efficient variants of the ICP algorithm. In Proceedings of the Third International Conference on 3-D Digital Imaging and Modeling, Quebec City, QC, Canada, 28 May–1 June 2001. Druon, S.; Aldon, M.J.; Crosnier, A. Color constrained ICP for registration of large unstructured 3d color data sets. In Proceedings of the IEEE International Conference on Information Acquisition, Seogwipo-si, Korea, 8–11 July 2007. Zinsser, T.; Schmidt, J.; Niemann, H. A refined ICP algorithm for robust 3-d correspondence estimation. In Proceedings of the International Conference on Image Processing, Barcelona, Spain, 14–17 September 2003. Arun, K.S.; Huang, T.S.; Blostein, S.D. Least-squares fitting of two 3-d point sets. IEEE Trans. Pattern Anal. Mach. Intell. 2009, PAMI-9, 698–700. [CrossRef] © 2018 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).