Enhanced ICP for the Registration of Large-Scale 3D Environment Models: An Experimental Study

One of the main applications of mobile robots is the large-scale perception of the outdoor environment. One of the main challenges of this application is fusing environmental data obtained by multiple robots, especially heterogeneous robots. This paper proposes an enhanced iterative closest point (ICP) method for the fast and accurate registration of 3D environmental models. First, a hierarchical searching scheme is combined with the octree-based ICP algorithm. Second, an early-warning mechanism is used to perceive the local minimum problem. Third, a heuristic escape scheme based on sampled potential transformation vectors is used to avoid local minima and achieve optimal registration. Experiments involving one unmanned aerial vehicle and one unmanned surface vehicle were conducted to verify the proposed technique. The experimental results were compared with those of normal ICP registration algorithms to demonstrate the superior performance of the proposed method.


Introduction
One of the main applications of mobile robots is large-scale perception of the outdoor environment. Recently, many studies have focused on fusing environmental data from multiple robots, especially heterogeneous robots such as unmanned aerial vehicles (UAVs), unmanned ground vehicles (UGVs), unmanned surface vehicles (USVs), and even remoted operated vehicles (ROVs) to achieve better and complementary perception. However, the huge differences in experimental data obtained by heterogeneous robots, such as in the view angles and resolution, make combining the data difficult, especially with the demand for highly accurate perception.
The data fusion technique of 3D model registration (3D-MR) is extensively used in medical image registration [1,2], simultaneous localization and mapping (SLAM) of mobile robots [3][4][5], remote sensing and image processing [6], etc. Existing 3D-MR algorithms may be categorized into two classes: Featured-Based and featureless. Feature-based 3D-MR predefines some offline descriptors, such as Harris corners [7], Susan corners [8], and spin-images [9]. These descriptors are then used as features to find correspondences between the two 3D point clouds that need to be fused. Of these descriptors, the spin-image has been proven to be accurate and robust [10][11][12]. However, approaches that use it suffer from a heavy computational burden [10]. Moreover, the performance of feature-based MR depends on the accuracy of the preselected descriptors. This may limit its applicability to large-scale outdoor environment registration, for which accurate descriptors may be impossible to obtain. In order to reduce the feature dependency, Bao et al. [13] proposed using semantic prior information for Sensors 2016, 16, 228 2 of 15 dense object reconstruction. Ho and Gibbins used semantic features to align city-scale LiDAR point clouds [14].
In contrast to feature-based MR, the featureless scheme can be used to model unstructured environments where accurate features are difficult to predefine [14]. Iterative closest point (ICP) [15][16][17] is one of the most commonly used featureless 3D-MR algorithms. With ICP, one point cloud (normally called the reference or target) is fixed, while the other (called the source) is transformed to match the reference. The algorithm iteratively revises the transformation to minimize the distance between the two point clouds. Point-to-point ICP calculates the distance between two paired points and optimizes the distance by gradient descent. Thus, its performance closely depends on a good initial estimate. On the other hand, point-to-plane ICP takes advantage of the surface normal information to improve the robustness to the initial estimate. Plane-to-plane ICP uses the surface structure to measure the distance and has been proved to be more robust with respect to a large initial transformation error [18].
ICP algorithms have also been applied to take advantages of multi-resolution data. For example, Jost et al. [19,20] used multi-resolution ICP (M-ICP) to accelerate the registration procedure by scattering the point cloud at a lower resolution level. This scheme can also improve the robustness against the initial estimation. However, when used for large-scale outdoor environment model registration, most ICP algorithms may suffer from the local minimum problem. This is mainly due to the gradient-descent-based optimization procedure, which cannot guarantee a global optimal resolution. A normal approach to resolving the local minimum problem is deliberately selecting the initial estimation so that the iterative calculation will completely avoid local minima. However, how to guarantee that the initial value is sufficiently accurate is still an open problem, especially when the data are from different view angles and have different resolutions. Most recently, Yang et al. [21] proposed global optimal ICP (GO-ICP) to solve the local minimum problem. GO-ICP combines the ICP framework with a branch-and-bound (BnB) scheme to try to search the space more efficiently and thus guarantee global optimization. However, the high computational burden may be a problem when GO-ICP is used for the model registration of a large-scale outdoor environment, where unstructured datasets may involve large amounts of sensory data.
In this paper, we propose an enhanced ICP algorithm for the fusion of cloud points obtained by heterogeneous robots. Three enhancements are presented: (1) a hierarchical searching scheme that is combined with the octree-based 3D modeling technique to improve the robustness with respect to the initial modeling error and realize coarse-to-fine registration of large-scale multi-resolution data; (2) an early warning mechanism to perceive the local minimum problem; and (3) a heuristic escape scheme based on sampling potential transformation vectors to avoid local minima. Experiments using one UAV and one USV, both carrying cameras onboard, to measure a riverside environment were conducted to verify the proposed technique. The contents of this paper are organized as follows: first, the ICP algorithms are introduced in Section 2. Then, the proposed enhancement techniques are explained in Section 3. In Section 4, the experimental setup is introduced, and an analysis of the results along with a comparison with the results of normal ICP algorithms is presented for an evaluation of the performance of the proposed method. Finally, the conclusions and future work are discussed in Section 5.

Preliminaries
Let P P {R N } and Q P {R M } represent the 3D datasets of the scan and global models, respectively, where N and M are the point numbers inside P and Q, respectively. Without losing generality, if M > N, the standard ICP is to find the sub-point sets {q i } N i"1 in model Q that are most similar to the scan model P, i.e., to solve the problem: where T P T 4ˆ4 is the combination of the rotation matrix R and translation vector t. Thus, the registration problem of two 3D models is converted into an optimization problem. However, the standard ICP cannot guarantee an optimal match and may suffer from local minima when a bad initial registration is used. Furthermore, the ICP itself cannot indicate whether or not it has been trapped into a local minimum. Generalized-ICP (G-ICP) [18] uses the plane-to-plane scheme to improve the robustness. In G-ICP, all of the points in P and Q can be remodeled as a Gaussian distribution: where p m i and q m i are the measured points and {C P i } and {C Q i } are the covariance matrices associated with the measured points. Usually, p i and q i are assumed to be independent of each other.
For the transformation T, a new transformation error for p i and q i can be defined as: Thus, d pTq i is also a stochastic variant with the following Gaussian distribution: By using the maximum likelihood estimate, the ICP in Equation (1) can be transformed into a probabilistic model: If we set C Q i = I and C p i = 0, the above equation can be converted to the original ICP form: G-ICP computes the covariance matrices along the direction normal to the local surface of each point, and the searching regions are larger compared with that of the standard point-to-point ICP. Thus, the possibility of G-ICP falling into a local minimum is reduced, and the robustness against measurement noise is improved. However, G-ICP increases the computational burden because of the stochastic calculations. Figure 1 shows a flowchart of our new proposed algorithm. It includes the following four steps:

New Proposed Registration Algorithm
Step I. Point cloud standardization and extraction Transform both the scan point cloud P (i.e., local 3D model with N P points) and model point cloud Q (i.e., global 3D model with N Q points) to the same resolution level through the use of the OctoMap [22] data structure. All different resolution level of a single point cloud can be generated from the same OctoMap. Step II. Coarse to fine iteration (a) Align the scan point cloud P and model point cloud Q at the current resolution level. (b) Calculate the efficiency of the current ICP registration by using the registration index I k cur and tendency index Trend k cur, where k represents the kth resolution level and cur represents the current ICP registration.
Step III. Early warning mechanism (a) Adjust the resolution level based on the value of Trend k cur. (b) If Trend k cur is bigger than a given positive threshold, go directly to Step II. If Trend k cur is just bigger than zero, update the resolution to a higher level and then go to Step II. (c) If I k cur is bigger than the given threshold, the algorithm has found a global optimal result, and go to Step V. Otherwise, the early warning has been triggered, and go to Step IV.
Step IV. Heuristic escape (a) Cluster the current aligned scan point cloud P based on distances between each point in P with its closest point in the model point cloud Q. Then, extract the biggest clustered point cloud Pmerge with the distance below the given threshold Threscluster. Step II. Coarse to fine iteration (a) Align the scan point cloud P and model point cloud Q at the current resolution level.
Calculate the efficiency of the current ICP registration by using the registration index I k cur and tendency index Trend k cur , where k represents the kth resolution level and cur represents the current ICP registration.
Step III. Early warning mechanism (a) Adjust the resolution level based on the value of Trend k cur .
If Trend k cur is bigger than a given positive threshold, go directly to Step II. If Trend k cur is just bigger than zero, update the resolution to a higher level and then go to Step II. (c) If I k cur is bigger than the given threshold, the algorithm has found a global optimal result, and go to Step V. Otherwise, the early warning has been triggered, and go to Step IV.
Step IV. Heuristic escape (a) Cluster the current aligned scan point cloud P based on distances between each point in P with its closest point in the model point cloud Q. Then, extract the biggest clustered point cloud P merge with the distance below the given threshold Thres cluster . Estimate the potential translation T escape based on the weighted translation vectors and then transform the scan point cloud P according to the estimated translation vector. Go to Step III.

Octree-Based 3D Map Extraction
3D point cloud models obtained by using distinct devices or different platforms usually differ in scale, noise, and especially resolution. We used OctoMap to unify the point clouds with different resolution levels and generate multi-resolution maps based on the hierarchical octree data structure.
According to Hornung et al. [22], the occupancy of each OctoMap lead node or the highest-resolution map is updated according to the observations {s1:i} and the initial occupancy estimation: L pn|s 1:t q 0 " L pn|s 1:t´1 q 0`L pn|s t q 0 where L(n|s 1:t ) 0 represents the nth leaf node of OctoMap and L(n|s t ) 0 is the log-odds measurement of the nth leaf node based on current observations. The lower-resolution level node is generated directly from a higher-resolution level node: where L(m|s 1:t ) I`1 refers to eight lower-resolution nodes L(m|s 1:t ) I`1 because of the 3D octree structure [23]. Thus, multi-resolution maps can be generated from and saved in a single OctoMap structure.

Early Warning Mechanism
Because traditional ICP methods cannot tell whether or not they have been trapped in a local minimum, we introduced the early warning mechanism to perceive the local minimum situation. We defined the registration index I k cur for the current resolution level k: where D k is the sum of the Euclidean distances between the scan point p k i and its nearest model point q k i . T opt is the optimal transformation matrix based on the current alignment. Actually, the registration index I k cur defines the registration error on the kth resolution level. If the registration index is beyond the given error Thres I , the current registration has meet the error tolerating band.
However, a single registration index cannot be used to perceive the local minimum situation. We defined the tendency index Trend k cur to describe the tendency of the current kth resolution level's registration procedure: (10) where I k pre is the previous registration index on the kth resolution level and I k cur is the current registration index on the kth resolution level. T k cur is the computation time of the current registration Sensors 2016, 16, 228 6 of 15 loop on the kth resolution level. The tendency index Trend k cur measures the velocity of the current registration procedure at the kth resolution. Usually, a higher velocity means an efficient local optimal, while a lower or in some cases even negative velocity may mean the registration procedure has reached a local minimum situation. The resolution level level re is determined by the tendency index Trend k cur : level re , Trend k cur ą Thres trend level re 2 , Thres tend ě Trend k cur ě δ¨Thres trend level re¨2 , Trend k cur ă δ¨Thres trend (11) where δ is a constant parameter between 0 and 1. If the tendency index is beyond the given threshold Thres Trend , the registration may achieve more accurate results based on the current resolution level. Then, the coarse-to-fine registration scheme can continue. However, if this index is only bigger than δ times Thres Tend , the registration process may not have been able to achieve efficient improvement at the current resolution level. Then, both point clouds are transformed into a higher resolution level through the use of OctoMap, and the registration process continues. Otherwise, the registration process has hit a local optimum. Then the optimum is determined to be a local minimum or global optimum according to the registration index I k cur . If the registration index is beyond Thres I , the registration process ends, and the aligned point clouds are output. Otherwise, the current registration may be a local minimum. The early warning is triggered, and the algorithm enters the heuristic escape scheme.

Heuristic Escape
The heuristic escape scheme helps the scan point cloud exit the local minimum by estimating the proper rotation angle R escape and translation T escape based on sampled potential rotation angles and transformation vectors. Both rotation angles and translation vectors are relayed to the merged scan and model point clouds.
To extract the merged point cloud, the distance between each point in the scan point cloud P and the closest point in the model point cloud Q is evaluated, and the scan points with a distance greater than the constant threshold Thres cluster are filtered out. The remaining points in the scan point cloud are clustered by the k-means method, and the biggest clustered point cloud P merge is extracted as the merged point cloud. According to Rusu et al. [24], the normal vector of P merge can be estimated from the eigenvalue of the covariance matrix of the point cloud: where N is the number of points in the point cloud, λ j is the jth eigenvalue of the matrix Cov, v t is its corresponding eigenvector, and j is from 1 to 3. We assumed the eigenvalue is ordered by λ 1 ě λ 2 ě λ 3 , so v 3 can be taken as the normal vector of the point cloud P merge . Then, the normal surface of P merge can also be determined by v 3 . Based on the current position, we can measure the registration index for six equally divided rotation angles on the normal surface. The escape rotation R escape is the rotation matrix R j of the relative angle j with the highest registration index: R escape " tR j |j " max i"0,60,120...300 where I i is the registration index of the corresponding rotation angle i in degree.
To estimate the escape translation, the scan point cloud is transformed to temporary scan point clouds P pnq temp along the six transformation matrices T potential n . Two matrixes are along the normal vector (i.e., blue axis), and the other four are from the normal surface P merge (i.e., red and green axes), where n is from 1 to 6. Then, each temporary scan point cloud is aligned to the model point cloud by the standard ICP method. The corresponding potential transformation vector t n is generated by where T ICP n is the relative ICP registration matrix. Figure 2 shows the six potential transformation vectors as the green lines. Then, t n is weighted to the registration index of P pnq temp .
where T ICP n is the relative ICP registration matrix. Figure 2 shows the six potential transformation vectors as the green lines. Then, tn is weighted to the registration index of P (n) temp. Finally, the proper escape translation Tescape can be estimated based on the six weighted potential transformation vectors: where θn is the normalized weight of the kth potential transformation. The escape translation is shown as the yellow line in Figure 2. Then, the scan point cloud is transformed according to the escape translation. Simultaneously, the resolution level of the point clouds is transformed to a higher resolution level, and the coarse-to-fine iteration scheme is continued.

Experiment Setup
To verify the performance improvement of the proposed algorithm, experiments were conducted on the cooperation of multiple heterogeneous UAV and USV at a river bank. Both robots were equipped with a navigation system for pose measurement and LiDAR for environment perception, as shown in Figure 3. The pose of each platform was estimated with the inertial measurement unit (IMU) and differential Global Positioning System (GPS) at an output frequency of 100 Hz.
where θ n is the normalized weight of the kth potential transformation. The escape translation is shown as the yellow line in Figure 2. Then, the scan point cloud is transformed according to the escape translation. Simultaneously, the resolution level of the point clouds is transformed to a higher resolution level, and the coarse-to-fine iteration scheme is continued.

Experiment Setup
To verify the performance improvement of the proposed algorithm, experiments were conducted on the cooperation of multiple heterogeneous UAV and USV at a river bank. Both robots were equipped with a navigation system for pose measurement and LiDAR for environment perception, as shown in Figure 3. The pose of each platform was estimated with the inertial measurement unit (IMU) and differential Global Positioning System (GPS) at an output frequency of 100 Hz. As shown in Figure 3a,b, the model was extracted from a bay. The points were collected by a Velodyne VLP-16 LiDAR mounted on the UAV that generated 300,000 points per second. The model point cloud was generated through an offline SLAM method. The scan point cloud was collected by a Velodyne 32e LiDAR mounted on USV that generated about 700,000 points per second. In our experiments, all of the environmental data were gathered online on an embedded board, and the registration algorithm ran offline on a laptop (Think-pad x220: Intel i5-2410 m 4 core @ 2.30 GHz CPU and 8 GB RAM) from Lenovo. The software was programmed in C++ with the Robot Operating System (ROS) [25] framework and Point Cloud Library (PCL) [26].
The small initial translation error was [10 m, 15 m, 10 m] on the roll, pitch, and yaw axes, and the large initial translation error was about 20 m. The small initial rotation error was about [5.7°, 5.7°, 15.6°], and the large initial rotational error was about 30°.
We tested the registration methods in two different registration experiments: A slender bank with complex terrain, which is circled in yellow in Figure 4b; and a triangular area with diverse elevations, which is circled in blue in the same figure. Table 1 gives the details of each experiment. To verify the robustness against different resolution levels, the model point clouds were kept at the same resolution level in both experiments, but the resolution level of the scan point clouds was set to 0.5 m for the slender bank and 0.2 m for the triangular area. To quantitatively evaluate the registration accuracy, the following registration error index ΔT was defined: where Tr is the corresponding transformation obtained through using different registration algorithms and Te is the initial transformation error. As shown in Figure 3a,b, the model was extracted from a bay. The points were collected by a Velodyne VLP-16 LiDAR mounted on the UAV that generated 300,000 points per second. The model point cloud was generated through an offline SLAM method. The scan point cloud was collected by a Velodyne 32e LiDAR mounted on USV that generated about 700,000 points per second. In our experiments, all of the environmental data were gathered online on an embedded board, and the registration algorithm ran offline on a laptop (Think-pad x220: Intel i5-2410 m 4 core @ 2.30 GHz CPU and 8 GB RAM) from Lenovo. The software was programmed in C++ with the Robot Operating System (ROS) [25] framework and Point Cloud Library (PCL) [26].
The small initial translation error was [10 m, 15 m, 10 m] on the roll, pitch, and yaw axes, and the large initial translation error was about 20 m. The small initial rotation error was about [5.7˝, 5.7˝, 15.6˝], and the large initial rotational error was about 30˝.
We tested the registration methods in two different registration experiments: A slender bank with complex terrain, which is circled in yellow in Figure 4b; and a triangular area with diverse elevations, which is circled in blue in the same figure. Table 1 gives the details of each experiment. To verify the robustness against different resolution levels, the model point clouds were kept at the same resolution level in both experiments, but the resolution level of the scan point clouds was set to 0.5 m for the slender bank and 0.2 m for the triangular area. To quantitatively evaluate the registration accuracy, the following registration error index ∆T was defined: ∆T " where T r is the corresponding transformation obtained through using different registration algorithms and T e is the initial transformation error.
The followed subsections present detailed analysis of the experiment results to demonstrate the improvement in performance.  At the 10-m resolution level, the maps were not sensitive to noise and outliers. Everything was transformed into a simple data structure. This property improved the robustness against the initial registration error.

Warning and Escape
We then verified the robustness of our proposed heuristic escape scheme against the local minimum problem. Figure 6a-e lists five local minimum registrations without our heuristic escape, Figure 6f presents one global optimal registration with our escape scheme. Figure 7 gives a detailed registration process for the ExII triangular area case with an initial translation error of [10 m, 30 m, 10 m] Similarly, the translation error e t and rotation error e r can be defined based on the Euclidean norm of the translation vector and geodesic distance as follows: The followed subsections present detailed analysis of the experiment results to demonstrate the improvement in performance.

Multi-Resolution Map Generation
The followed subsections present detailed analysis of the experiment results to demonstrate the improvement in performance.  At the 10-m resolution level, the maps were not sensitive to noise and outliers. Everything was transformed into a simple data structure. This property improved the robustness against the initial registration error.

Warning and Escape
We then verified the robustness of our proposed heuristic escape scheme against the local minimum problem. Figure 6a-e lists five local minimum registrations without our heuristic escape, Figure 6f presents one global optimal registration with our escape scheme. Figure 7 gives a detailed registration process for the ExII triangular area case with an initial translation error of [10 m, 30 m, 10 m] At the 10-m resolution level, the maps were not sensitive to noise and outliers. Everything was transformed into a simple data structure. This property improved the robustness against the initial registration error.

Warning and Escape
We then verified the robustness of our proposed heuristic escape scheme against the local minimum problem. Figure 6a-e lists five local minimum registrations without our heuristic escape, Figure 6f presents one global optimal registration with our escape scheme. Figure 7 gives a detailed registration process for the ExII triangular area case with an initial translation error of [10 m, 30 m, 10 m] and rotation error of [´10˝, 10˝, 55˝]. Figure 7a shows the initial point clouds of both scan (colored in black) and model (colored in red). Figure 7b,c shows the coarse-to-fine registration scheme. At the resolution level of 5 m, as shown by the blue circle of Figure 7c, the tendency index Trend k cur according to Equation (10) was below the expected δ times Thres Tend . The registration index I k cur evaluated by Equation (9) was also lower than Thres I . Thus, the early warning was triggered, and the registration process entered the heuristic escape step. The resolution level was lowered to 10 m, as shown in Figure 7d, and a new transformation was generated by the heuristic escape estimation based on Equations (9)- (11). Finally, the registration result reached a global optimum, as shown in Figure 7f.
Different initial transformation errors led to variations in the escape times. Figure 8 shows the relation between the root mean square (RMS) of the initial transformation error and the escape times. As the RMS error increased, the escape time grew with a ladder pattern.  Figure 7a shows the initial point clouds of both scan (colored in black) and model (colored in red). Figure 7b,c shows the coarse-to-fine registration scheme. At the resolution level of 5 m, as shown by the blue circle of Figure 7c, the tendency index Trend k cur according to Equation (10) was below the expected δ times ThresTend. The registration index I k cur evaluated by Equation (9) was also lower than ThresI. Thus, the early warning was triggered, and the registration process entered the heuristic escape step. The resolution level was lowered to 10 m, as shown in Figure 7d, and a new transformation was generated by the heuristic escape estimation based on Equations (9)- (11). Finally, the registration result reached a global optimum, as shown in Figure 7f. Different initial transformation errors led to variations in the escape times. Figure 8 shows the relation between the root mean square (RMS) of the initial transformation error and the escape times. As the RMS error increased, the escape time grew with a ladder pattern.   Figure 7a shows the initial point clouds of both scan (colored in black) and model (colored in red). Figure 7b,c shows the coarse-to-fine registration scheme. At the resolution level of 5 m, as shown by the blue circle of Figure 7c, the tendency index Trend k cur according to Equation (10) was below the expected δ times ThresTend. The registration index I k cur evaluated by Equation (9) was also lower than ThresI. Thus, the early warning was triggered, and the registration process entered the heuristic escape step. The resolution level was lowered to 10 m, as shown in Figure 7d, and a new transformation was generated by the heuristic escape estimation based on Equations (9)- (11). Finally, the registration result reached a global optimum, as shown in Figure 7f. Different initial transformation errors led to variations in the escape times. Figure 8 shows the relation between the root mean square (RMS) of the initial transformation error and the escape times. As the RMS error increased, the escape time grew with a ladder pattern.

Convergence
Besides our proposed enhanced ICP, we also evaluated three other ICP-based methods for comparison: Standard-ICP, M-ICP, and G-ICP. Figures 9 and 10 show the results for the slender bank and triangular area experiments. The red point cloud represents the model point cloud Q. The green, yellow, purple, and pink point clouds represent the registration results of the enhanced ICP, standard-ICP, M-ICP, and G-ICP, respectively. Each method was tested for both the normal initial transformation error and abrupt turn case in the experiment.
For the normal initial transformation error case, the initial translation error was randomly sampled within [±10, ±10, ±10], and the rotation error was sampled within [±20°, ±20°, ±40°]. Although all methods obtained acceptable results for the triangular area test, as shown in Figure 9b, the other methods failed to match the model point cloud for the slender bank test owing to the complexity of the terrain as shown in Figure 9a. In contrast, our proposed enhanced ICP method guaranteed correct results with a final RMS error of 1.1 m and rotation error of 1.5°. Table 2 summarizes the registration results of 40 normal initial transformation error tests on the slender bank and triangular area in Detail 2. On average, our proposed method could achieve an accurate match even with a rough initial error.
To verify the abrupt turn problem with the different ICP-based methods, we set the initial rotation error on the z-axis around 160°-220°, as shown in Figure 10. The Standard ICP, M-ICP, and G-ICP became trapped in a local minimum in both the slender bank and triangular area test. Our proposed enhanced ICP could efficiently estimate the local minimum problem by using the early warning mechanism and eventually escape with a proper transformation by using our heuristic escape scheme.

Convergence
Besides our proposed enhanced ICP, we also evaluated three other ICP-based methods for comparison: Standard-ICP, M-ICP, and G-ICP. Figures 9 and 10 show the results for the slender bank and triangular area experiments. The red point cloud represents the model point cloud Q. The green, yellow, purple, and pink point clouds represent the registration results of the enhanced ICP, standard-ICP, M-ICP, and G-ICP, respectively. Each method was tested for both the normal initial transformation error and abrupt turn case in the experiment.
For the normal initial transformation error case, the initial translation error was randomly sampled within [˘10,˘10,˘10], and the rotation error was sampled within [˘20˝,˘20˝,˘40˝]. Although all methods obtained acceptable results for the triangular area test, as shown in Figure 9b, the other methods failed to match the model point cloud for the slender bank test owing to the complexity of the terrain as shown in Figure 9a. In contrast, our proposed enhanced ICP method guaranteed correct results with a final RMS error of 1.1 m and rotation error of 1.5˝. Table 2 summarizes the registration results of 40 normal initial transformation error tests on the slender bank and triangular area in Detail 2. On average, our proposed method could achieve an accurate match even with a rough initial error.
To verify the abrupt turn problem with the different ICP-based methods, we set the initial rotation error on the z-axis around 160˝-220˝, as shown in Figure 10. The Standard ICP, M-ICP, and G-ICP became trapped in a local minimum in both the slender bank and triangular area test. Our proposed enhanced ICP could efficiently estimate the local minimum problem by using the early warning mechanism and eventually escape with a proper transformation by using our heuristic escape scheme.

Convergence
Besides our proposed enhanced ICP, we also evaluated three other ICP-based methods for comparison: Standard-ICP, M-ICP, and G-ICP. Figures 9 and 10 show the results for the slender bank and triangular area experiments. The red point cloud represents the model point cloud Q. The green, yellow, purple, and pink point clouds represent the registration results of the enhanced ICP, standard-ICP, M-ICP, and G-ICP, respectively. Each method was tested for both the normal initial transformation error and abrupt turn case in the experiment.
For the normal initial transformation error case, the initial translation error was randomly sampled within [±10, ±10, ±10], and the rotation error was sampled within [±20°, ±20°, ±40°]. Although all methods obtained acceptable results for the triangular area test, as shown in Figure 9b, the other methods failed to match the model point cloud for the slender bank test owing to the complexity of the terrain as shown in Figure 9a. In contrast, our proposed enhanced ICP method guaranteed correct results with a final RMS error of 1.1 m and rotation error of 1.5°. Table 2 summarizes the registration results of 40 normal initial transformation error tests on the slender bank and triangular area in Detail 2. On average, our proposed method could achieve an accurate match even with a rough initial error.
To verify the abrupt turn problem with the different ICP-based methods, we set the initial rotation error on the z-axis around 160°-220°, as shown in Figure 10. The Standard ICP, M-ICP, and G-ICP became trapped in a local minimum in both the slender bank and triangular area test. Our proposed enhanced ICP could efficiently estimate the local minimum problem by using the early warning mechanism and eventually escape with a proper transformation by using our heuristic escape scheme.  Escape times RMS error (m) Figure 9. Registration results for the normal initial error test. In the slender bank test, the translation error was [5,4,4], and the rotation error was [17.1˝, 20˝, 34.2˝], our proposed method make the registration with the RMS error at 1.1 m and rotation error at 1.5˝. In the triangular area test, the translation error was [4,3,10], and the rotation error was [10˝, 20˝, 34.2˝], our proposed method guarantee the registration result with the RMS error at 0.4 m and rotation error at 1.1˝.  (a) Slender bank test (b) Triangular area test Figure 10. Registration results of the abrupt turn test. In the slender bank test, the translation error was [15,14,20], and the rotation error was [17°, 20°, 190°], our proposed method make the registration with the RMS error at 1.5 m and rotation error at 2.3°. In the triangular area test, the translation error was [14,15,20], and the rotation error was [10°, 20°, 171°], our proposed method guarantee the registration result with the RMS error at 2.3 m and rotation error at 1.7°. Table 3 summarizes the results of 40 abrupt turn tests in detail. Even with a tough abrupt turn, our proposed enhanced ICP method was able to guarantee matching accuracy, while the other method all became trapped in a local minimum. The rotation error along the z-axis was around 160°-220°. Figure 10. Registration results of the abrupt turn test. In the slender bank test, the translation error was [15,14,20], and the rotation error was [17˝, 20˝, 190˝], our proposed method make the registration with the RMS error at 1.5 m and rotation error at 2.3˝. In the triangular area test, the translation error was [14,15,20], and the rotation error was [10˝, 20˝, 171˝], our proposed method guarantee the registration result with the RMS error at 2.3 m and rotation error at 1.7˝. Table 3 summarizes the results of 40 abrupt turn tests in detail. Even with a tough abrupt turn, our proposed enhanced ICP method was able to guarantee matching accuracy, while the other method all became trapped in a local minimum.  Figure 11a presents the average registration times of the methods in both experiments with normal initial transformation errors. The proposed method was not as fast as the single standard ICP or M-ICP method. This is because of the heuristic escape scheme, which combines potential direction estimation and G-ICP to escape from the local minimum. On average, our proposed method reduced the registration time by 30% compared to G-ICP while guaranteeing registration accuracy at the same time. For the abrupt turn case, we only evaluated the relation between the RMS error and registration time of our proposed method. As shown in Figure 11b, the registration time for the abrupt case was closely related to the heuristic escape time, as given in Figure 8. A larger initial transformation error increased the heuristic escape time, which also increased the computation time.  Figure 11a presents the average registration times of the methods in both experiments with normal initial transformation errors. The proposed method was not as fast as the single standard ICP or M-ICP method. This is because of the heuristic escape scheme, which combines potential direction estimation and G-ICP to escape from the local minimum. On average, our proposed method reduced the registration time by 30% compared to G-ICP while guaranteeing registration accuracy at the same time. For the abrupt turn case, we only evaluated the relation between the RMS error and registration time of our proposed method. As shown in Figure 11b, the registration time for the abrupt case was closely related to the heuristic escape time, as given in Figure 8. A larger initial transformation error increased the heuristic escape time, which also increased the computation time.

Running Time
(a) (b) Figure 11. Time cost analysis: (a) in both Slender Bank test and Triangle Area test, our method could make the alignment around 2 s, faster than the generalized ICP, but slower than the standard-ICP method and multi-resolution ICP method because of the heuristic escape scheme; (b) the registration time is highly related to RMS error.

Conclusions
This paper presents the fast and accurate registration of a large-scale 3D environmental model for application to heterogeneous mobile robot cooperation with a rough initial transformation error. A hierarchical searching scheme is combined with the octree-based ICP algorithm. A novel early warning mechanism is proposed to perceive the local minimum problem, and a heuristic escape scheme is used to avoid local minima and achieve optimal registration. Experiments involving one UAV and one USV were conducted to verify the proposed technique, and the experimental results were compared with those of normal ICP registration algorithms. The results showed that the proposed algorithm is insensitive to the initial transformation error and can make effective heuristic escape decisions to resolve the local minimum problem. Figure 11. Time cost analysis: (a) in both Slender Bank test and Triangle Area test, our method could make the alignment around 2 s, faster than the generalized ICP, but slower than the standard-ICP method and multi-resolution ICP method because of the heuristic escape scheme; (b) the registration time is highly related to RMS error.

Conclusions
This paper presents the fast and accurate registration of a large-scale 3D environmental model for application to heterogeneous mobile robot cooperation with a rough initial transformation error. A hierarchical searching scheme is combined with the octree-based ICP algorithm. A novel early warning mechanism is proposed to perceive the local minimum problem, and a heuristic escape scheme is used to avoid local minima and achieve optimal registration. Experiments involving one UAV and one USV were conducted to verify the proposed technique, and the experimental results were compared with those of normal ICP registration algorithms. The results showed that the proposed algorithm is insensitive to the initial transformation error and can make effective heuristic escape decisions to resolve the local minimum problem.