http://www.diva-portal.org

Postprint

This is the accepted version of a paper presented at 2018 IEEE OES AutonomousUnderwater Vehicle Symposium.

Citation for the original published paper:

Torroba, I., Bore, N., Folkesson, J. (2018)A Comparison of Submaps Registration Methods for Multibeam Bathymetric MappingIn:

N.B. When citing this work, cite the original published paper.

Permanent link to this version:http://urn.kb.se/resolve?urn=urn:nbn:se:kth:diva-250894

A Comparison of Submaps RegistrationMethods for Multibeam Bathymetric Mapping

Ignacio Torroba, Nils Bore & John FolkessonRobotics, Perception and Learning Lab, KTH, Stockholm

{torroba, nbore, johnf}@kth.se

Abstract—On-the-fly registration of overlapping multi-beam images is important for path planning by AUVs per-forming underwater surveys. In order to meet specificationon such things as survey accuracy, coverage and density,precise corrections to the AUV trajectory while underwayare required. There are fast methods for aligning pointclouds that have been developed for robots. We compareseveral state of the art methods to align point clouds oflarge, unstructured, sub-aquatic areas to build a globalmap. We first collect the multibeam point clouds intosmaller submaps that are then aligned using variationsof the ICP algorithm. This alignment step can be appliedif the error in AUV pose is small. It would be the finalstep in correcting a larger error on loop closing where aplace recognition and a rough alignment would precedeit. In the case of a lawn mower pattern survey it would bemaking more continuous corrections to small errors in theoverlap between parallel lines. In this work we comparedifferent methods for registration in order to determinethe most suitable one for underwater terrain mapping. Todo so, we benchmark the current state of the art solutionsaccording to an error metrics and show the results.

I. INTRODUCTION & BACKGROUND

Autonomously mapping the sea bottom promisesto allow a substantial increase in the speed and effi-ciency of bathymetric surveys. Simultaneous mappingand localization by land robots is now an establishedtechnology. However, this success cannot be directlyapplied in the underwater domain, despite the recentadvancements in the area. This is mainly due to theturbid, often featureless surroundings a robot navigatesthrough and the more limited sensing and communica-tion technology available for Autonomous UnderwaterVehicles (AUVs).

The multibeam sonar, MBES, has become one ofthe main tools to construct high-resolution bathymetricmaps of the seabed, with early examples in [1] and [2].These are typically done either with surface ships ifthe depths permit or from ROVs and AUVs in deeperwaters. However, the lack of a global reference duringthe AUV navigation means that the accuracy of theconstructed maps often depends on that of the vehicle’spositioning, which is greatly affected by the descent todepth and worsens with the distance traveled. Possiblesolutions to this problem entail the use of beaconswhose position is known. However, these methods re-

quire extra infrastructure and substantially complicatethe survey mission planning. The beacon signal is alsosubject to the sound propagation in water which addsuncertainty to these estimates.

Survey missions will have specifications on accu-racy, coverage and density of soundings. In order tohave AUVs follow optimal plans to meet those thataccount for all the disturbances that occur during themission, the AUV system should use the collected datato correct its trajectory while underway. By makingthese corrections on sections of the swath that overlapprevious passes the accumulation of pose error canbe substantially reduced. There will remain an initialoffset to the map due to the initial descent which canhave allowances for in the planning. The quality of thealignment is critical as even small errors will build upover long missions lasting days.

There have been proposed solutions, methods tomap large areas combining smaller, more consistentsubmaps, as in [3] over the past years. This approachrelies on an intermediate step in which overlappingsubmaps of adjacent seabed areas have to be registeredand realigned so as to correct misalignments arisingfrom errors in the AUV dead reckoning. This procedurehas proved to yield more consistent global maps whileimproving the trajectory estimate of the vehicle asin [3], [4] or [5]. A similar approach to mapping,based on rigidly transforming the sub-scans resultingfrom partitioning the vehicle trajectory and the MBESinput, has been applied to reconstructing the surface ofsubmerged mines in [6].

All these works have in common that they relyon the standard ICP algorithm for the registration ofthe point clouds resulting from creating the MBESsubmaps. This underlines the relevance of the appro-priate selection of the registration algorithm on anunderwater Simultaneous Localization And Mapping(SLAM) framework. This is where our contribution hereis relevant as we compare several of the variations ofICP on actual MBES data.

II. RELATED WORK

A comprehensive review of existing registrationmethods for point clouds based on Iterative Closest

Point (ICP) is compiled in [7]. The conclusions fromthis review highlight the importance of the constraintsimposed by the application domain in the search fora registration solution. Hence, given the wide range ofalgorithms and disciplines in which they are used, theauthors decide to focus on mobile, ground robots. [8]and [9] present surveys on point cloud registration meth-ods for both robotics and health care applications butagain, disregards the specifics of the underwater terrainand mobile robots. However, point clouds constructedfrom seabed submaps are often characterized by large,flat and featureless areas with few landmarks which aregenerally hard to disambiguate. The work introducedhere addresses the question of which registration proce-dure is best suited to the specifics of mobile robots inunderwater environments.

The Iterative Closest Point (ICP) is the seminal algo-rithm upon which most of the more recent and success-ful methods for point cloud registration have been de-rived. Two different versions of ICP exists depending onthe metric used for the matching. Point-to-point distanceto find correspondences between the point clouds wasintroduced in [10], while point-to-plane distance wasoriginally presented in [11]. Generalized-ICP [12] is anextension of these methods. It describes a probabilisticvariation of ICP in which the original distributionsof the points are constructed to model the points asextracted from planar local surfaces. Thus, plane-to-plane metrics can be used. In [13], the Trimmed ICP(TrICP) proposes an alternative approach to ICP. Theleast trimmed squares (LTS) is used instead of a least-squares method to find a closed form solution to extractthe optimal transformation between point clouds. Otherworks evolving the original ICP have focused on includ-ing additional information into the metric used in theregistration process. Color ICP, presented in [14], wasone of the first steps in this direction. In color ICP, boththe 3D information and color of the model are recoveredfrom panoramic images and used during point matching.As the amount of sensory input available increased onrecent years, methods such as Adaptive Iterative ClosestKeypoint (AICK) [15] and 4D ICP [16] have continuedto develop the same principle.

Approaches similar to GICP, in the sense that infor-mation about the original distributions from which thepoints are sampled is estimated and then used duringthe registration, have been introduced recently. Amongothers, the Normal Distribution Transform (NDT) [17]fits Gaussian models to the grid-divided clouds and thencompares them through standard iterative optimizationmethods in order to find a robust matching. [18] buildsthe probabilistic model of the points from 2D rangescans including information from all the sources ofuncertainty involved in the data acquisition process. Thework in [19] adapts this approach to MBES data for theregistration of seabed submaps.

A global alternative to local ICP-based registration

methods is proposed in [20]. The Minimally UncertainMaximal Consensus (MUMC) algorithm is suggestedto match patches extracted from point clouds. Planarsurfaces are fitted to the patches. They are then used forthe search of correspondences through the maximizationof the geometric consistency within a defined searchspace. This approach is later applied by the authors tounderwater sonar data in [21].

III. SUBMAPS REGISTRATION METHODS

The five state of the art algorithms selected for thiscomparison are briefly introduced hereafter.

A. ICP and Generalized ICP

ICP is a general purpose algorithm for registrationof geometrical primitives widely used both in academiaand industry. It consists of iterating over two steps tofind the relative transform that best aligns two givengeometries:

1) Matching step: find the closest pairs of pointsbetween the two point clouds.

2) Minimization step: Compute the relative trans-formation between point clouds by minimizingthe sum of the association distances betweenthe paired points.

The GICP algorithm is described as a plane to planefitting algorithm. Instead of trying to minimize thedistance between two sampled points from a surfaceas in ICP, it approximates the surface by planes formedby nearby points. These planes are computed by takingthe mean and covariances of the set of nearby points.

B. Normal Distribution Transform

3D NDT is a more recent method. In the NormalDistribution Transform algorithm, the registration iscarried out comparing the probability density functionscreated for each point cloud. These pdfs are computedattaching to each grid-based division of the originalpoint clouds a normal distribution that models theprobability of measuring those points. This is similarto GICP except that the measured point cloud is firstused to estimate a Gaussian for each grid cell. By thenusing these distributions a maximum likelihood solutioncan be found. The distributions are solely based on thevariability of the measured points assigned to the cells.This variability includes the sensor noise and the finiteextent of the surface. In practice the distributions givea similar representation to the GICP model.

C. Probabilistic ICP

The PICP algorithm with MBES is a fully proba-bilistic method that models and propagates the sourcesof uncertainty in the surveying system to be used inthe registration step according to carefully constructedsensor and motion models. A probabilistic model of

(a) Initial misalignment from dead reckoning. (b) Alignment resulting from semi-PICP registration

Fig. 1: Depth map of a seabed section constructed from two submaps from the pipeline dataset, before and afterthe registration.

(a) Initial misalignment error from the navigation system. (b) Misalignment error resulting from semi-PICP regis-tration

Fig. 2: Consistency map from the example in 1, using the misalignment error metric of [22].

the distribution of the points in the submaps is derivedfrom the uncertainty propagation. First, the model isused to compute the point-to-point correspondences forcoarse alignment, followed by a point-to-plane matchingprocedure. As a second step, the minimization of thesum of the Mahalanobis distance of the association erroris solved through weighted least squares. The algorithmdescribed iterates through the pre-processed submapsuntil convergence.

The original PICP applies a feature segmentationstep prior to the registration which has been eliminatedfor this work. On our experiments, this step led tocreate more ambiguous point clouds after eliminatingthe spatial information contained in the featureless areasof the submaps.

D. Semi-probabilistic ICP

This is a hybrid method derived from the PICP.We propose a variant in which the probabilistic modelsof the point clouds are only used for point matchingin a point-to-plane fashion. In the original algorithmthey were also applied during the minimization of theMahalanobis distance of the association error, requiringcomputation of the relative transform between pointclouds. However, this would lead to the construction and

manipulation of large matrices, whose size is directlyproportional to that of the point cloud. Instead, semi-PICP computes the minimization step at the end of eachassociation stage applying a closed-form solution to theassociation step, as in the standard ICP.

IV. METHODS COMPARISON

In order to evaluate the performance of the fivemethods selected, an error metric has been defined, theyhave been tested in two different datasets of bathymetricmaps and their efforts to minimize the error, comparedto the initial error estimate.

A. The Datasets

Two bathymetric surveys carried out with a multi-beam have been used as benchmarks, the pockmarksdataset from [23] and the pipeline dataset.

The pipeline dataset was collected with a MBESattached to a ship and postprocessed with off-the-selfsoftware before its use for this work. It has been selectedto be used as a benchmark since it consists of twoclear underwater structures, a hill and a segment ofa pipeline, which can be easily checked visually forregistration errors. This alleviates the fact that ground

truth is not available. As an example, the sub-figurea) in 1, shows clearly a wrong initial alignment ofthe submaps, which causes artifacts on the hill and adistinct misalignment on the pipeline. The other datasetused for benchmarking, the pockmarks, has been used inseveral works in UW SLAM. However, a correct resultof the registration in the final map is harder to assessvisually since the real topography of the area is highlyunstructured.

The resulting maps from both datasets are composedof several smaller local maps or submaps constructedcombining consecutive multibeam readings using thevehicle’s dead reckoning as in [3]. Both datasets presenterrors in the submaps alignment as a consequence of theinaccuracy on the vehicle’s navigation. The metric usedto compute this initial error is the one presented in [22],which has been used to measure the performance ofthe registration methods evaluated. Due to the absenceof ground truth in these datasets, the initial consistencyerror has been used as the one to minimize. Even thoughthe error measure is point to point, it does not penalizepoint to plane and plane to plane registration methodsduring the comparison since the density of the pointclouds is very high enough.

The pockmarks dataset consists of 17 submaps, outof which 34 pairs with enough overlapping area havebeen used. The pipeline contains 7 larger submaps witha total of 12 overlapping pairs. None of the datasetscontains information on the navigation uncertainty.

B. Methodology

For the comparisons, the Point Cloud Library (PCL)versions of ICP, GICP and NDT have been used, whileboth PICP and semi-PICP have been implemented. Thefive methods have been applied to all the overlappingpairs of submaps for both datasets until convergenceor until a maximum number of iterations was reached.Each algorithm has been manually tuned to achieve bestperformance in both datasets before the experiments.

The processing times have not been compared in thiswork since the PICP and semi-PICP implementationshave not been optimized for performance, as opposed tothe PCL library algorithms. Among the PCL registrationmethods, however, significant differences in processingtimes have not been recorded, hence removing thisvariable from the comparison.

Prior to the alignment process, all submaps havebeen subjected to the same initial filtering and sub-sampling steps, eliminating outliers from noisy mea-surements and speeding up the alignment process. Bothdatasets have been sub-sampled uniformly to reduce re-dundant information. The percentage of points removedwas bigger in the pipeline dataset since the sub-mapscontained 500k points on average. Finally, an outlierfilter based on the statistics of nearest neighbor pointswas applied to remove noisy data.

V. RESULTS

In Table I the RMS errors in the relative alignmentbetween two point clouds from all the pairs of sub-maps in both datasets has been summarized. The resultsfrom each registration method have been compared tothe original error, considered the ground truth.

RMS Error (m) Pockmarks PipelineInitial (GT) 0.6526 0.1389

ICP 0.4884 0.1159GICP 0.4739 0.1083PICP 0.5787 0.1344

semi-PICP 0.5330 0.0821NDT 0.6437 0.1355

TABLE I: Results of the registration methods appliedto pairs of overlapping submaps

Given that a local optimization method is beingused to align a non-convex function, convergence isnot always guaranteed. Thus, in order to reduce theeffects of failed registrations, which distorted greatly thefinal values, the worst alignment result of each methodhas been removed and the means recomputed. GICPhas yielded the best results for the pockmarks and thesemi-PICP for the pipeline dataset. However the semi-PICP method provides a better averaged outcome acrossbenchmarks than the other methods. On the other hand,the NDT algorithm used does not seem to contributeany meaningful difference in the RMS error before andafter the registration process.

Figure 1, depicts the depth map from two submapsin the pipeline dataset before and after the registrationprocedure carried out with the semi-PICP. The improve-ment in the alignment can be easily seen in the reductionof the artifacts on the hill and the correct positioningof the segments of the pipeline on the right side image.The estimated tracks followed by the surveying vehiclehave been superimposed on the depth map in black.One can see the correction in the estimate of the robottrajectory derived from a more accurate alignment ofthe submaps.

In Figure 2, the consistency error maps for theoverlapping area of the submaps in 1 are shown. Thetwo sections with greater concentration of RMS errorcorrespond to the hill and a segment of a pipeline, therest being featureless seabed. The reduction of the errorafter aligning both submaps with the semi-PICP methodcan be easily appreciated on the right image. Morechallenging configurations of overlapping submaps havebeen used in this comparison work to present the resultson Table I. In Figure 3 the output of a registration pro-cess with GICP applied to two perpendicular submapsof the pockmarks dataset is presented. In this case, thereduction of the initial RMS misalignment error wasof 9.4%, comparatively smaller than that in the cases

(a) Initial alignment of two perpendicular submaps. (b) Alignment correction of 9.4% of the RMS error fromGICP registration.

Fig. 3: Depth map of two perpendicular submaps from the pockmarks dataset.

where the overlap between maps is proportionally muchlarger.

VI. CONCLUSIONS

The relevance of the point cloud registration pro-cedure as a key component of several state-of-the-art underwater SLAM frameworks has been presentedin this paper. However, the lack of a comprehensivestudy of the existing methods available in the literaturefor subsea vehicles has motivated an analysis of theirperformance with underwater test benches. Four state ofthe art registration algorithms for point clouds, togetherwith a new one derived from PICP, have been comparedand evaluated aligning MBES bathymetric submaps.From the results obtained, GICP seems to be the mostrobust method across datasets. Even though semi-PICPhas provided better results on average, this has beenonly after eliminating failed rounds of registration inwhich the algorithm did not converge. This, togetherwith the fact that GICP needs little parameter tunningand it can be found in several open-source librariesmakes it the optimal solution among the ones tested.

The NDT algorithm has not yielded any significantimprovement in the reduction of the registration errorfor both benchmarks.

A mention to the semi-PICP as a promising solutionto further develop is in place. The main advantageof both PICP and semi-PICP is that they exploit theinformation contained in the navigation system’s uncer-tainty. Thus, with a better knowledge of the sourcesof uncertainty in the vehicle and sensors, usually notcontained in the datasets, these methods might be ableto yield better results at the expense of having tocompute and store the covariances of the noise models

and their propagation through the system. However,these methods need a more careful parameter tunning inorder to achieve a good performance, making them lessgeneral than ICP-based solutions. Furthermore, bothsemi-PICP and PICP require in average more processingtime, since the points correspondences are computedin the Mahalanobis distance. This also increases thecomplexity of the implementation.

VII. ACKNOWLEDGEMENT

The authors want to thank Stefan Williams, Os-car Pizarro and colleagues for allowing us to usethe pockmarks dataset for our evaluation. This workwas supported by Stiftelsen for StrategiskForskning(SSF) through the Swedish Maritime Robotics Centre(SMaRC) (IRC15-0046).

REFERENCES

[1] S. B. Williams, P. Newman, G. Dissanayake, and H. Durrant-Whyte, “Autonomous underwater simultaneous localisation andmap building,” in Robotics and Automation, 2000. Proceedings.ICRA’00. IEEE International Conference on, vol. 2, pp. 1793–1798, IEEE, 2000.

[2] J. L. Leonard, R. N. Carpenter, and H. J. S. Feder, “Stochasticmapping using forward look sonar,” Robotica, vol. 19, no. 5,pp. 467–480, 2001.

[3] C. Roman and H. Singh, “Improved vehicle based multibeambathymetry using sub-maps and slam,” in Intelligent Robotsand Systems, 2005.(IROS 2005). 2005 IEEE/RSJ InternationalConference on, pp. 3662–3669, IEEE, 2005.

[4] M. VanMiddlesworth, M. Kaess, F. Hover, and J. J. Leonard,“Mapping 3d underwater environments with smoothedsubmaps,” in Field and Service Robotics, pp. 17–30, Springer,2015.

[5] C. Roman and H. Singh, “A self-consistent bathymetric map-ping algorithm,” Journal of Field Robotics, vol. 24, no. 1-2,pp. 23–50, 2007.

[6] M. Bleier, A. Dias, A. Ferreira, J. Pidgeon, J. Almeida, E. Silva,K. Schilling, and A. Nuchter, “Signed distance function basedsurface reconstruction of a submerged inland mine usingcontinuous-time slam,” IFAC-PapersOnLine, vol. 50, no. 1,pp. 1139–1144, 2017.

[7] F. Pomerleau, F. Colas, R. Siegwart, et al., “A review of pointcloud registration algorithms for mobile robotics,” Foundationsand Trends R© in Robotics, vol. 4, no. 1, pp. 1–104, 2015.

[8] B. Bellekens, V. Spruyt, R. Berkvens, and M. Weyn, “A surveyof rigid 3d pointcloud registration algorithms,” in AMBIENT2014: the Fourth International Conference on Ambient Com-puting, Applications, Services and Technologies, August 24-28,2014, Rome, Italy, pp. 8–13, 2014.

[9] M. Magnusson, N. Vaskevicius, T. Stoyanov, K. Pathak, andA. Birk, “Beyond points: Evaluating recent 3d scan-matchingalgorithms,” in Robotics and Automation (ICRA), 2015 IEEEInternational Conference on, pp. 3631–3637, IEEE, 2015.

[10] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” in Sensor Fusion IV: Control Paradigms and DataStructures, vol. 1611, pp. 586–607, International Society forOptics and Photonics, 1992.

[11] Y. Chen and G. Medioni, “Object modelling by registration ofmultiple range images,” Image and vision computing, vol. 10,no. 3, pp. 145–155, 1992.

[12] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.,” inRobotics: science and systems, vol. 2, p. 435, 2009.

[13] D. Chetverikov, D. Svirko, D. Stepanov, and P. Krsek, “Thetrimmed iterative closest point algorithm,” in Pattern Recog-nition, 2002. Proceedings. 16th International Conference on,vol. 3, pp. 545–548, IEEE, 2002.

[14] A. E. Johnson and S. B. Kang, “Registration and integration oftextured 3d data1,” Image and vision computing, vol. 17, no. 2,pp. 135–147, 1999.

[15] J. Ekekrantz, A. Pronobis, J. Folkesson, and P. Jensfelt, “Adap-tive iterative closest keypoint,” in Mobile Robots (ECMR), 2013European Conference on, pp. 80–87, IEEE, 2013.

[16] H. Men, B. Gebre, and K. Pochiraju, “Color point cloudregistration with 4d icp algorithm,” in Robotics and Automation(ICRA), 2011 IEEE International Conference on, pp. 1511–1516, IEEE, 2011.

[17] P. Biber and W. Straßer, “The normal distributions transform:A new approach to laser scan matching,” in IROS, vol. 3,pp. 2743–2748, 2003.

[18] L. Montesano, J. Minguez, and L. Montano, “Probabilistic scanmatching for motion estimation in unstructured environments,”in Intelligent Robots and Systems, 2005.(IROS 2005). 2005IEEE/RSJ International Conference on, pp. 3499–3504, IEEE,2005.

[19] A. Palomer, P. Ridao, and D. Ribas, “Multibeam 3d underwaterslam with probabilistic registration,” Sensors, vol. 16, no. 4,p. 560, 2016.

[20] K. Pathak, A. Birk, N. Vaskevicius, and J. Poppinga, “Fast reg-istration based on noisy planes with unknown correspondencesfor 3-d mapping,” IEEE Transactions on Robotics, vol. 26,no. 3, pp. 424–441, 2010.

[21] K. Pathak, A. Birk, and N. Vaskevicius, “Plane-based registra-tion of sonar data for underwater 3d mapping,” in IntelligentRobots and Systems (IROS), 2010 IEEE/RSJ InternationalConference on, pp. 4880–4885, IEEE, 2010.

[22] C. Roman and H. Singh, “Consistency based error evaluationfor deep sea bathymetric mapping with robotic vehicles,” inRobotics and automation, 2006. ICRA 2006. Proceedings 2006IEEE international conference on, pp. 3568–3574, Ieee, 2006.

[23] S. Barkby, S. Williams, O. Pizarro, and M. Jakuba, “An effi-cient approach to bathymetric slam,” in Intelligent Robots andSystems, 2009. IROS 2009. IEEE/RSJ International Conferenceon, pp. 219–224, IEEE, 2009.