GLOBALLY OPTIMAL POINT CLOUD REGISTRATION FOR ROBUST MOBILE MAPPING

Point cloud registration algorithms have been studied for several decades. In the SLAM domain, dense local convergence based methods are typically used to register consecutive scans. Since these procedures are not globally optimal, it happens that they converge to a wrong local minimum. This can lead to gross errors during mapping and can make entire datasets unusable. We introduce a new branch and bound based point cloud registration method that is globally optimal. The method is able to reliably determine the global optimum within a given parameter search space. We show how this method can be used in a mapping system as a fallback function to correct gross errors. Using various public datasets, we demonstrate the capabilities of the method.


INTRODUCTION
Point cloud registration is a problem that has been researched for a long time. Various algorithms have been developed for a wide range of applications. In this paper we restrict to the field of mobile mapping and SLAM (Simultaneous Localization and Mapping). We address methods that use point clouds and scan matching to create maps of the environment as SLAM systems.

Motivation
Current SLAM systems such as (Zhang and Singh, 2014), (Behley and Stachniss, 2018) or (Koide et al., 2021) usually work very reliably. Impressive accuracies are achieved in public benchmark datasets like (Geiger et al., 2012). While in most cases very good results can be achieved with such methods, gross errors occur from time to time. Corresponding errors can lead to strange behavior in the mobile robot domain or make whole datasets unusable in the mapping domain. In our opinion, there are two main reasons for gross errors: 1. The movement of the sensor is not observable from the point clouds. This means that an objective functional used in point cloud registration does not have a global extremum at the true correct pose.
2. The global extremum of an objective functional corresponds to the true correct pose but could not be discovered during the point cloud registration.
In this work, we present a solution to correct errors that occur as a result of the second reason.

Related Work
Here we first give a brief overview of algorithms for point cloud registration and then we go into more detail about globally optimal methods. Last we present previous works in the field of robust mapping. * Corresponding author In the SLAM area, local methods are usually used for the registration of consecutive scans. Prominent dense local methods are ICP (Besl and McKay, 1992), NDT (Biber and Straßer, 2003), CPD (Myronenko and Song, 2010) and GICP (Segal et al., 2009). We consider the latter to be a state of the art method and use it in the following for comparisons as a representative of convergence-based methods. While dense methods use a continuous representation of the environment, feature-based methods only match features. An example of a feature based local method is the one presented in LOAM (Zhang and Singh, 2014). All of the above methods have in common that they are local. That means they work convergence-based or make assumptions that nearby points or features in the separate point clouds correspond and therefore determine a solution very quickly. The disadvantage is that finding the global optimum is not guaranteed what can lead to gross errors from time to time. In addition to the methods mentioned above, global methods for point cloud registration also exist. Here, there are also methods that rely on keypoints (see (Tombari et al., 2013) for a comparison of different keypoints types) and feature correspondences such as (Yang et al., 2020). Since we do not want to limit our applications to the presence of special keypoints or edges, we only consider dense methods. Known dense globally optimal methods are the one used in Google Cartographer for closing loops (Hess et al., 2016), the Go-ICP (Yang et al., 2015) and GOGMA (Campbell and Petersson, 2016). All of these methods use a branch and bound based approach. Since (Hess et al., 2016) was developed for two-dimensional grid maps, it is not applicable to 3D point clouds. Due to the point-to-point metric, the Go-ICP algorithm is not ideally suited for sparse point clouds with highly varying point density, such as those generated by mobile laser scanners. The GOGMA method is partially suitable, but has limited accuracy due to the approximation of the point cloud by Gaussian distributions. In addition, the quality of the approximation of the point cloud by gaussian distributions can be influenced by strongly varying point densities.
In this paper a globally optimal method for point cloud registration is presented. The method is inspired by the Go-ICP (Yang et al., 2015) algorithm. In contrast to Go-ICP, the proposed method runs in 6D while using unnested optimal search. By taking into account normal vectors (point-to-plane distance), the new method is suitable for sparse point clouds, such as those generated by mobile laser scanners. To minimize the influence of outliers, a different metric is used than in the original Go-ICP. The metric chosen is related to that of NDT (Biber and Straßer, 2003). Our approach specifically addresses the application with point clouds from mobile laser scanners. Robust LiDAR-based odometry or mapping systems are becoming increasingly important. In order to increase robustness, approaches exist that apply several point cloud registration methods in parallel and use the most plausible result based on a criterion (Reinke et al., 2021). Another approach combines data from multi sensors and monitors the health (health monitoring) of the odometry result (Palieri et al., 2020). We present a robust mapping system that can detect gross errors in the results of local point cloud registration based on model assumptions without additional sensor information. In case of an error, the transform is corrected using the presented global registration method.

GLOBALLY OPTIMAL POINT CLOUD REGISTRATION
Globally optimal means that the global optimum of an objective functional is determined. In our case, we are looking for a set of parameters that describes a rigid transformation. The rigid transformation represents how a point cloud must be shifted and rotated in order to be correctly placed relative to a second point cloud. The objective functional evaluates how accurately the current set of parameters aligns the point clouds. To determine the global optimum, we use a branch and bound framework. In the following chapters, our approach for global point cloud registration is presented. We call it GO-P2Plane (Globally Optimal Point-2-Plane Registration). First, the used data representation is described in detail. Subsequently, the objective functional is explained. The choice of the objective functional is elementary to enable a reliable point cloud registration.

Data Representation
In the registration we distinguish between the base point cloud and the point cloud that we want to register relative to the base point cloud. While the latter is still represented as a point cloud, the base point cloud is converted into a model consisting of planes at the beginning of the registration. First, for each point of the base point cloud, a local normal vector is estimated based on its k nearest neighbor points. The base point cloud is then spherically projected. Similar to the range image representation of point clouds, the space is then divided into discrete quadrangles. We refer to the quadrangles below as patches. The center of each patch is defined by an elevation angle and an azimuth. The width and height of a planar patch is given by the desired angular resolution δ. For each quadrant, the point whose elevation angle and azimuth are closest to the center is determined. The plane defined by the point and its normal vector is then considered as a representation of the space, which is valid within a planar patch. The entirety of planes then serves as a continuous environment representation.
In Fig. 1 the planar patches representation is outlined. Each planar patch contains a center point − → m r,c and a normal vector − → N r,c . r and c are the respective row and column indices that represent the azimuth and elevation angle. The planar patches are stored in a data structure similar to a range image representation to quickly determine projective correspondences. The division of the environment into discrete planar patches is elementary in the subsequent search for a global optimum. In Fig.  2 the planar patches representation is illustrated in 3D for an exemplary indoor scene.

Objective Functional
The objective functional is the function whose global maximum will be determined. The chosen objective functional consists of a weighted sum of the distances to the respective planar patches, normalized by the number of points (see equation (1)). The probability density function of the normal distribution is applied to weight the distances. The objective functional is related to the score presented in NDT (Biber and Straßer, 2003) but differs in that a constant value is assumed for the standard deviation and the point-to-plane error is used. The resulting score is a value in the range [0, 1]. Let P be a point cloud and n p the number of points. Let − → p i be a point from P and − → m c the center point and − → N c the normal vector of the corresponding planar patch for − → p i of the base point cloud. For a rigid transformation defined by a translation − → t and a rotation R that is applied to P , the value of the objective functional score(t, R), which is the mean of the probability density values of a normal distribution for all point-to-plane errors, is obtained as follows: e i represents the point-to-plane error: σ can be considered as a design parameter. The weight function is displayed for an exemplary σ = 0.17 in Fig. 3. Points whose distance to the neighboring point is small get a high weight and points far away have no influence on the objective functional.
The functional thus behaves similar as measuring points within a consensus set, but is still derivable for local refinement. With a suitable choice of σ, rough outliers have no direct influence on the result. A great advantage of the selected objective function is that no inlier ratio has to be selected in advance. During the optimization, the transformation is to be determined for which the score is maximum.

Point Correspondence
Within the approach points are projectively associated to a correspondent planar patch. This means that for each point for which a corresponding planar patch is required, the points angle of inclination and azimuth are first calculated. For a point − → p = [x, y, z] T the angles can be calculated as follows: The angles θ and ψ are then used to find the corresponding planar patch. This is done by converting the θ and ψ into rows and columns indices (r, c). With these indices the corresponding element can be selected in the planar patches representation. Figuratively, this type of correspondence means that it is determined which planar patch intersects the straight line defined by θ and ψ. The intersected planar patch is the correspondence searched for. This kind of correspondence was introduced by (Blais and Levine, 1995) and is also used in (Behley and Stachniss, 2018) to find corresponding surface elements during point cloud registration.

Branch and Bound
Branch and Bound (BnB) is a algorithm paradigm that is used for solving combinatorial optimization problems and mathematical optimization problems. In the BnB method, the solution space is searched successively by dividing it into subspaces. The evaluated subspaces result in a graph in the form of a root tree. For each subspace it is predicted which maximum value (in case of a maximization problem) the objective functional could have within the subspace. This predicted maximum value is called upper bound. The most promising subspace is then explored subsequently. In each exploration step the objective functional is evaluated in at least one location. In comparison to the predicted upper bound, this so called lower bound serves as a guess of the value range in the subspace. The lower bounds of the evaluated subspaces are used to determine the highest value score identified so far. The core idea of BnB is now to use the previous maximum of the lower bounds (score) to remove explored subspaces whose upper bound is lower. This can be done since we know that the predicted maximum of the respective subspaces is smaller than our previous identified maximum value score. More information and various examples of BnB can be found in (Clausen, 1999). When designing a BnB based algorithm, a suitable approach that predicts the maximum value that the objective functional can assume within a subspace is required. This approach is called bounding function.

Domain Parametrization
The search space consists of six dimensions. The first three dimensions correspond to the translation in x, y and z direction. The fourth to sixth dimension corresponds to the rotation. The rotation is parameterized by the axis-angle representation. The axis angle representation is a very compact way to describe a rotation. Rotations are therefore represented by a three elements vector − → r = [r 1 , r 2 , r 3 ] T . The angle of rotation is given by || − → r || and the normalized axis of a rotation by − → r /|| − → r ||. Any rotation can be described with the elements r 1 , r 2 , r 3 ∈ [−π, π]. Rotations defined in the axisangle representation can be converted in a rotation matrix using Rodrigues' rotation formula. A rotation matrix, which is parameterized by an axis-angle representation vector − → r , we write down with R( − → r ). In total, a vector with the following elements results: In our 6 dimensional case, the exploration of a subspace results into 2 6 = 64 subspaces. The extent of the resulting subspaces in each dimension is half of the original space. During recursive exploration, the search space is divided into subspaces in which the rotation components and the translation components respectively have identical edge lengths. Let a t and a r denote the translational and rotational edge lengths, we have the following definition: a r = r 1,max −r 1,min = r 2,max −r 2,min = r 3,max −r 3,min (6) If the rotation space and the translation space are considered separately, cube-shaped search areas result in each case. A subspace can thus be completely described by a 6 dimensional position vector − → V pointing to the center of the subspace and by two edge lengths a t and a r . To accelerate the search for a global optimum, the search space can be limited by a maximum rotation angle α max and a maximum distance d max between the origins of the point clouds. In this case, the search space is parametrized with

Bounding Function Derivation
In branch and bound, an upper bound and a lower bound are used for optimization. The lower bound is the function value of an evaluation of the objective functional (usually in the middle of the subspace). When a new subspace is explored, a lower bound of it is calculated and the highest evaluated value score of all subspaces is updated. The upper bound function predicts the maximum value that the objective functional can assume within a subspace. In the following we call the upper bound B and the lower bound B. As described before, a subspace is defined by a position vector − → V to the center of the subspace and by the edge lengths a t and a r . Given a base point cloud in the planar patches representation as a set of planes defined by − → N r,c , − → m r,c and a second point cloud P that is placed relative to it we calculate the upper bound as follows: Our approach to determine the upper bound is done in two steps: 1. First, for each point in P all reachable corresponding planar patches are determined. Reachable means that for each point, the freedom of movement allowed by the extent of the subspace a t and a r is used to identify the set of planar patches that could potentially become a correspondence. 2. In a second step, for each possible planar patches correspondence, it is checked how small the point-to-plane error can become considering the degrees of freedom of the search space. For each point, the correspondence with the smallest error is selected and the score (see formula (1)) is calculated. This optimistic score represents the upper bound.

Identification of Correspondences
Let us first focus on the identification of possible planar patch correspondences. The identification of the reachable correspondences is performed sequentially for all points in P . − → S r is the set of valid axis-angle representations and − → S t the set of valid translation vectors within our subspace defined by − → V , a t and a r . We define the different sets as follows: Let − → p i ∈ P be a point of the movable point cloud. Its possible movement resulting as a set of vectors − → p m can be described by the following equation for a rigid transformation: As (Yang et al., 2015) and (Campbell and Petersson, 2016) we resort to the theorem that for any vector − → x and two arbitrary axis angle respresentations − → r 1 and − → r 2 and their respective rotation matrix representations R r1 and R r2 holds: represents the intersection angle of the two vectors − → a and − → b . Next, we replace the rotation matrix with two separate rotation matrices using theorem (12): And we replace also the translation vector with two separate vectors: If we substitute R( − → S r ) and − → S t in equation (11) with (13) and (14) we obtain: Here R( − → V (4 : 6)) is a rotation matrix defined by the angle axis representation in − → V at the center of the search space. − → V (1 : 3) is the translation defined at the center point − → V of a subspace.
R( − → S ar ) and − → S at describe the remaining rotational and translational movement uncertainty quantified by the edge lengths a r and a t . A correspondence of a point − → p i to a planar patch exists if the elevation angle and the azimuth of the point are within the range of a planar patch. Thus, in order to determine the correspondences, it is necessary to determine what ranges of values the elevation angle and the azimuth of the point can assume considering the ability of movement enabled by the subspace. For this, − → p i is first transformed using the center parameters − → V of the search space: If equation (15) is reshaped and (16) is inserted, the remaining range of movement around − → p V can be described with: Let θ V and ψ V be the elevation angle and the azimuth of − → p V . As can be seen in equation (17), there are two dependencies R( − → S ar ) and − → S at that can influence θ V and ψ V . To determine the possible range of values for θ and ψ of − → p m , we first determine the maximum possible intersection angle between − → p V and − → p m . The maximum distance by which − → p V can be moved by R has no influence on the length and can therefore be neglected here.
The maximum angular change between − → p V and − → p m using l t can be approximated by considering l t as part of the circumference of the circle with radius || − → p v || around the point clouds origin. This gives a maximum angular change of: The relationship is outlined in Fig. 4. If (18) is substituted into (19), the relation is obtained which describes the angle by which the direction of vector − → p V can be influenced depending on the translation uncertainty a t : The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B2-2022 XXIV ISPRS Congress (2022 edition), 6-11 June 2022, Nice, France In addition to the translation term, the direction of − → p V can also be changed by a rotation R( − → S ar ). As described in (12), the rotation angle of a point in space is ≤ as the vector norm of the axis angle representation. In our case, the maximum angle of rotation is: The total possible rotation of a point − → p m relative to − → p V is thus: To efficiently determine the possible range of values for θ and ψ of a point − → p i within a subspace using the elevation angle θ V and the azimuth ψ V of the center point as well as ∆ϕ, we make the following approximation: Let − → p 1 and − → p 2 be two arbitrary vectors defined by their elevation angles θ 1 , θ 2 and their azimuths ψ 1 , ψ 2 with respective distances from the origin greater than zero. Then it is assumed that the following holds: This simplification is approximately true as long as we are away from the poles |θ 1 |, |θ 2 | << π/2. Thus we get the set C θ,ψ of all legal parameters for θ and ψ: This set C θ,ψ will now be converted to the corresponding row and column indices (r, c) of our planar patches representation using the angular resolution δ of the planar patches: Figure 5. Identification of corresponding planar patches using ∆ϕ. Grey cells mark corresponding planar patches.

Determination of the Minimum Error
The smallest possible error of a point taking into account the degrees of freedom of the current subspace is determined by checking all correspondences of a point step by step. When checking correspondences, we now assume that the point was rotated in the direction of the planar patch. We can do this because the correspondence is enabled through the rotation of a point. To avoid the parametrization of a rotation matrix for the point rotation, we use the distance of the point − → p V from the origin and compare it with the distance of the planar patches center point − → m r,c from the origin. Using a unit vector − → m r,c || − → m r,c || in the direction of the planar patch we then calculate the pointto-plane error of the point: The error calculated in (26) is a prediction of the error defined in equation (2) for one of the possible correspondence. A geometrical derivation of ϵ r,c is shown in Fig. 6. The error ϵ r,c Figure 6. Derivation of the predicted error ϵ r,c for a corresponding planar patch. The planar patch is defined by its center point − → m r,c and its normal vector − → N r,c .
can be further reduced by a rotational component l δ compensating the coarse angular resolution of the planar patches and by the translational component l t (see equation (18)). The rotation compensates for the circumstance that the rotation parameters only allow discrete steps, namely the selection of the planar patches. Angle changes within a planar patch would otherwise have no effect. The maximum possible reduction of the error ϵ r,c within a planar patch is calculated by using the same relation as in (19): The smallest possible error between a point − → p i and its current correspondence is therefore: The minimum error ϵ i,min of a point − → p i results from the minimum of the values for epsilon ϵ r,c,min of all its correspondences C r,c :

Calculation of the Upper Bound
The upper bound is a prediction of the maximum possible value of the score as it is defined in equation (1) within a subspace. To calculate this prediction, we now determine the minimum error based on the subspace defined by − → V , a t , a r for each point − → p i in the point cloud P . The minimum error is then used to calculate the upper bound score. The procedure how to calculate the upper bound is defined in detail in algorithm 1.

Calculation of the Lower Bound
The lower bound is calculated by evaluating the value of the objective functional at the center point − → V of a subspace: As mentioned before, this value serves as a measure of the range of values within the subspace.

Local Alignment
When searching for a global maximum, a local alignment is performed to speed up the procedure. Higher lower bound values usually allow more areas to be removed during global optimization by Branch and Bound. A local alignment within a subspace is performed when the lower bound B, i.e. the evaluation of the score in the center of a subspace, exceeds a threshold λ local = 0.5·score. Where score is the highest identified score so far. The local alignment follows a ICP (Besl and McKay, 1992) related scheme. Instead of minimizing the point-to-point distance, the score is maximized based on the corresponding planar patches in each step. The Newton method is used for this purpose. To use the same metric as in our branch-and-bound optimization, planar patches are projectively associated to each point. This also speeds up the determination of the correspondences.

Algorithm Overview
Next, we present a pseudo code describing the sequence of the GO-P2Plane algorithm. As already mentioned, a subspace S is defined by a vector to the center point − → V and the edge lengths a r and a t . B is the upper bound of a subspace and B the lower bound. The list L is a data structure containing all subsequent subspaces S i of our search space S 0 with their respective upper bounds B i . R is a rotation matrix and − → t a translation vector with three elements. The sequence of the algorithm is defined in algorithm 2.

INTEGRATION INTO MAPPING
We used the presented GO-P2Plane algorithm to develop a robust mapping system. Current mapping methods usually use convergence-based methods to register scans. If the convergence-based registration converges to a wrong local minimum, gross errors occur. Our system is characterized by the fact that errors can be detected and corrected. A globally optimal registration of all scans would be too time-consuming. That's why we combine local convergence-based registration with global registration. Based on various criteria, we decide scan by scan whether the result of the local registration is plausible.

Overview
In Fig. 7, our embedding of the globally optimal algorithm in a mapping system is sketched. For each new point cloud R t , the for all S j , j ∈ {1, ..., 64} do 7: Calculate B j and B j for S j using algorithm 1 and equation (30) 8: if B j > score then 9: if score score < 1 then [score, R, end for 16: end while 17: return score, R and t processing steps are executed consecutively. t is a time index that is incremented by one for each new point cloud. A new point cloud R t is preprocessed first using a voxel grid filter for downsampling. Then, the point cloud P i is registered locally relative to last keyframe K i using the Generalized-ICP (Segal et al., 2009). The result of the registration donated by transformation T t is then checked for plausibility by comparing it with the results of the last time step. The plausibility check used is described in section 3.2. If the results are plausible, a keyframe update is performed with it. If they are not, a globally optimal registration is performed using GO-P2Plane. The selector is a placeholder that symbolizes the passing of the respective transformation parameters. If the distance to the last keyframe exceeds a threshold the keyframe is updated. This means the current point cloud P t is saved and provided as K i for further processing. The index i indicates the keyframe number. Figure 7. Overview of the mapping process. The numbers in the diamond-shaped boxes represent the order.

Plausibility Check
We use 3 criteria to check the plausibility of local registration results. Let T t be the transformation of the current scanner pose relative to the map origin that we want to evaluate. It consists of the translation vector − → t t and a rotation matrix R t . T t−t is the corresponding transformation of the previous time step. ∆τ is the time difference between the two points in time τ t−1 and τ t of the transformations. Our plausibility check is mainly based on model assumptions for the movement of the LiDAR sensor during data recording. For this we define a maximum velocity v max of the sensor during recording, a maximum acceleration a max and a maximum rotation rate ω max . Our model assumption is that the sensor moves continuously during recording and that these values are not exceeded. The sensor velocity is considered constant between two scans and we calculate it as follows: The absolute rotation rate between two scans can be calculated with: The result of the registration is considered plausible if the following conditions are met:

Global Registration
For global registration we use our proposed GO-P2Plane approach. The procedure serves as a fallback function if the result determined with the use of local registration does not seem plausible. In order to use the globally optimal method as efficiently as possible, we limit the search range using the model parameters. This can significantly reduce the computation time of the method. For the translation range, the search distance is limited to d max = ∆τ v max , where ∆τ is the time difference between two consecutive LiDAR scans and v max the maximum assumed velocity. For the rotational search range a maximum rotation of α max = ∆τ ω max is assumed between the point clouds. Here, ω max is the maximum assumed angular velocity of the scanner.

Keyframe Update
To determine the global pose relative to the very first point cloud, the point clouds are always registered relative to the last so-called keyframe. The keyframes are a subset of the total set of point clouds. If the distance to the last keyframe exceeds a limit, a new keyframe is saved.

RESULTS
Since we are aiming at mapping in our application, we have limited our choice of experiments to scans with partial overlap. We implemented the GO-P2Plane algorithm in C++ and during the tests the software runs on an Intel i7 4. generation.
Our results consists of two parts. First, we applied the presented algorithm in isolation to point clouds of the public Stairs dataset (Pomerleau et al., 2012) containing laser scans obtained with custom rotating setup using a Hokuyo UTM-30LX laser range finder. The contained scans are related to terrestrial laser scans in terms of point density and field-of-view. Furthermore, we tested the mapping system presented in chapter 3 using the first sequence of the KITTI dataset (Geiger et al., 2012).

Stairs Dataset
The Stairs dataset (Pomerleau et al., 2012) consists of 31 laser scans with a mean of 191,000 points per scan. The scans were recorded indoors in a staircase and the recording points of the sequence are approx. 0.4 m away from each other. The challenges of the dataset are large volume changes and large variations in the overlaps. Global positions with mm-range precision are available as ground truth for all scans. In this experiment we registered all the individual scans one after the other relative to each other using our method. The methods are evaluated by comparing the errors of the relative transformations between each two consecutive point clouds with the ground truth. The average error of the translation and the average rotation error are used as error metrics. The mean overlap of the consecutive point clouds is 91 % and the minimum overlap 62 %. The Go-ICP (Yang et al., 2015) and GOGMA (Campbell and Petersson, 2016) registration results for this dataset were taken from (Campbell and Petersson, 2016). For the Go-ICP we display the results for N=500 points. For the GOGMA algorithm there are also presented results with a subsequent refinement in (Campbell and Petersson, 2016), but we have not included these in the table, since methods for refinement are not the focus of this work. Our GO-P2Plane algorithm used also N=500 points for its model point cloud. As search space of the algorithm we allowed any kind of rotation in these experiments α max = 180°and set a maximum distance of d max = 1 m.
As a state-of-the-art representative of convergence-based registration algorithms, the Generalized-ICP (GICP) (Segal et al., 2009) was tested. For this purpose, the publicly available implementation in the Point Cloud Library (Rusu and Cousins, 2011) was used with the default settings. Using this example, it can  ) shows that at least in one case the GICP has converged into a false local minimum. This happened when processing the scans with IDs 24 and 25 in the dataset. The scene with the respective solutions of GICP and our method are shown in Fig. 8. The low maximum translation and rotation errors of our presented method show that the approach could register all point clouds correctly. The results obtained are comparable to those of the GOGMA algorithm.

KITTI dataset
Here we want to present the results of how we tested our mapping system using the KITTI dataset (Geiger et al., 2012). First of all, it is important to clarify that our goal with this system is not to achieve a particularly high accuracy. We are also aware that the procedures of the authors leading the ranking with their odometry results have run reliably within the KITTI dataset and that there were most likely no gross errors in the registration. Situations in which gross errors occur during scan matching differ depending on the environment setup, the methods and may also behave differently depending on the parameter settings. Our goal with this experiment is to demonstrate the functionality of a robust mapping system In our experiments, we applied our system to the first sequence of the KITTI dataset. To enforce difficult conditions, we reduced the data set to one-third of the scans by using only every 3rd scan. This reduces the overlap and increases the distance between scans.

Validation of Plausibility Check
First, we tested our plausibility check conditions which are described in chapter 3.2. For this purpose, we processed the modified dataset as described without global registration with our pipeline. Each time the result of the local registration violated the plausibility conditions, we saved this information. In Fig. 9 translation and rotation errors for each pose are shown. We marked identified implausible poses red, while plausible poses are green. With this experiment, we can show that we can successfully identify poses with large translation error using our plausibility check. On further analysis, we found that in this dataset it was mainly the maximum assumed acceleration (we use a max = 10 m s 2 ) that allowed the identification of the poses with faulty translations. In Fig. 11 we share the resulting trajectory in den x-y plane with outlined locations of implausible states.

Application of our System
Here we present our mapping results applying our complete mapping pipeline as introduced in chapter 3 to the modified first sequence of the KITTI dataset (Geiger et al., 2012). For the presented GO-P2Plane algorithm we used an angular resolution of δ = 3°for the planar patches. We reduced the second point cloud to 2,000 points by choosing random points. To further reduce the computation time of our global optimal registration algorithm, we added an additional stop condition. After N = 300, 000 iterations, the search for the global optimum is terminated. In our configuration, this corresponds to 20 s of computation time. It should be noted that the algorithm is no longer globally optimal as a result. Nevertheless, the number of iterations was in our experiments sufficient to find suitable transformations.  The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B2-2022 XXIV ISPRS Congress (2022 edition), 6-11 June 2022, Nice, France

CONCLUSION
We have presented a new globally optimal point cloud registration algorithm (chapter 2). Like some other registration algorithms, it works according to the branch and bound paradigm. However, it is the first branch and bound that optimizes according to the point-to-plane metric. Like the point-to-plane metric, the newly developed upper bound function is particularly well suited for sparse point clouds from mobile laser scanners. Using the Stairs dataset, we were able to show that our method achieves similar results to related methods. Furthermore, we have developed a mapping system for point clouds that can detect gross errors and also correct them with the help of the presented global registration algorithm (chapter 3). We were able to test the system successfully using a public data set.

Limitations
As long as the correct pose has a global maximum in our objective functional at registration, it can be identified with our algorithm. However, if the movement of the sensor in the point clouds is not observable, because the sensor is moving on a free surface or in a tunnel with the same shape, the presented algorithm cannot find a reliable solution. Another problem that can occur with a small overlap of two point clouds is that points that do not actually belong together can be staged by a wrong pose in such a way that a maximum in the objective functional is generated. Such a maximum can be obtained, for example, by superimposing two observations of the ground that do not actually represent the same location. Another challenge is the computing time of the presented method. By reducing the parameter search space on the basis of assumptions about motion limitations, it can be reduced, but the method is still far away from real-time. The results presented were not calculated in real time. One possibility would be to integrate the procedure into a real-time mapping system and, if necessary, to calculate error corrections parallely in a separate thread.

Future Work
In order to enable a broader applicability of the method, a drastic reduction of the computational costs is necessary. This can be achieved, for example, by more compact representations of the environment than point clouds or planar patches. Another way to reduce the computational costs is a more exact upper bound function for maximum value predictions. Integration into real-time mapping systems is also being considered.