- Email: [email protected]
Signed Distance Function Based Surface Reconstruction of a Submerged Inland Mine Using Continuous-time SLAM
Proceedings of the 20th World Congress Proceedings of 20th The International Federation of Congress Automatic Control Proceedings of the the 20th World World Congress Proceedings of the 20th9-14, World The Federation of Automatic Control Toulouse, France, July 2017 The International International Federation of Congress Automatic Control Available online at www.sciencedirect.com The International of Automatic Control Toulouse, France, July Toulouse, France,Federation July 9-14, 9-14, 2017 2017 Toulouse, France, July 9-14, 2017
ScienceDirect
IFAC PapersOnLine 50-1 (2017) 1139–1144
Signed Distance Function Based Surface Signed Distance Function Based Surface Signed Distance of Function Based Surface Reconstruction a Submerged Inland Reconstruction of a Submerged Inland Reconstruction of a SubmergedSLAM Inland Mine Using Continuous-time Mine Using Continuous-time SLAM Mine Using† Continuous-time SLAM∗ ∗,∗∗
Michael Bleier † Andr´ e Dias ∗,∗∗ Ant´ onio Ferreira ∗ ∗ ∗∗∗† Andr´ ∗,∗∗ Michael Bleier e Ant´ o Michael Bleier Andr´ e Dias Dias ∗,∗∗ Ant´ onio nio Ferreira Ferreira John Pidgeon Jos´ e Almeida Eduardo Silva ∗,∗∗ † ∗,∗∗ ∗ ∗∗∗ ∗,∗∗ Michael Bleier Andr´ e Dias ∗,∗∗ Ant´ onio Ferreira ∗∗∗ Jos´ ∗,∗∗ ∗,∗∗ +,† +,† John e Almeida Eduardo Silva John Pidgeon Pidgeon Jos´ e Almeida Eduardo Silva Klaus Schilling Andreas N¨ u chter ∗∗∗ ∗,∗∗ ∗,∗∗ +,† John Pidgeon Jos´ e +,† Almeida Eduardo Silva Klaus u Klaus Schilling Schilling +,† Andreas Andreas N¨ N¨ uchter chter +,† Klaus Schilling +,† Andreas N¨ uchter +,† ∗ INESC Technology and Science, Porto, Portugal ∗ ∗ INESC Technology and Science, Porto, Portugal ∗∗ INESC Technology and Science, Porto, School of Engineering, Polytechnic Institute of Portugal Porto, Portugal ∗ ∗∗ Technology and Science, Porto, Portugal ∗∗ School INESC of∗∗∗Engineering, Polytechnic Institute of Engineering, Polytechnic InstituteAustralia of Porto, Porto, Portugal Portugal BMT WBM Pty Ltd, Brisbane, ∗∗ School of ∗∗∗ Engineering, Polytechnic InstituteAustralia of Porto, Portugal ∗∗∗ + School of BMT WBM Pty Ltd, Brisbane, BMT WBM Pty Ltd, Brisbane, Australia Informatics VII – Robotics and Telematics, Julius Maximilian ∗∗∗ + WBM Pty and Ltd,Telematics, Brisbane, Australia + InformaticsBMT VII – Julius – Robotics Robotics and Telematics, Julius Maximilian Maximilian University of W¨ urzburg, Germany + Informatics VII Informatics VII – Robotics and Telematics, Julius Maximilian † University of W¨ u rzburg, Germany University of W¨ ue.V., rzburg, Germany Zentrum f¨ u r Telematik W¨ u rzburg, Germany, † University of W¨ urzburg, GermanyGermany, † Zentrum u u f¨ urr Telematik Telematik e.V., e.V., W¨ W¨ urzburg, rzburg, Germany, (e-mail: f¨ [email protected]) † Zentrum Zentrum f¨ u r Telematik e.V., W¨ u rzburg, Germany, (e-mail: (e-mail: [email protected]) [email protected]) (e-mail: [email protected]) Abstract: The planning of mining operations in water filled open-pit mines requires detailed Abstract: planning of filled open-pit requires detailed Abstract: The The planning of mining mining operations in water filled risks. open-pit mines requires detailed bathymetry to create a mine plan operations and assess in thewater involved Thismines paper presents postAbstract: The planning of mining operations in water filled risks. open-pit mines requires detailed bathymetry to plan and assess involved paper presents postbathymetrytechniques to create create a aformine mine plan an andimproved assess the the involved risks.aThis This paper presents postprocessing creating 3D model from survey carried out using bathymetry to create aformine plan an andimproved assess the involved risks.aThis paper presents postprocessing techniques creating 3D model from survey carried out using processing techniques for creating ana improved 3Dsonar model from a surveynavigation carried outsystem. using an autonomous surface vehicle with multibeam and a GPS/INS processing techniques for creating an improved 3D model from a survey carried out using an vehicle with sonar and system. an autonomous autonomousofsurface surface vehiclepoint with aa multibeam multibeam sonar and aa GPS/INS GPS/INS navigation system. Inconsistencies the created as a result of calibration errors navigation or GPS signal loss an autonomousofsurface vehicle with cloud a multibeam sonar and a GPS/INS navigation system. Inconsistencies created as of errors GPS loss Inconsistencies of the the created point point cloud cloud as aa result result of calibration calibration errors or or(SLAM) GPS signal signal loss are corrected using a continuous-time simultaneous localization and mapping solution. Inconsistencies of the created point cloud as a result of calibration errors or(SLAM) GPS signal loss are using a simultaneous localization and mapping are corrected corrected using a continuous-time continuous-time simultaneous localization and mapping (SLAM) solution. solution. Signed distance function (SDF) based mapping is employed to fuse the measurements from are corrected using a continuous-time simultaneousis localization and mapping (SLAM) solution. Signed (SDF) based employed fuse the from Signed distance distance function (SDF) based mapping mapping employed tonoise. fuse From the measurements measurements from multiple runs intofunction a consistent representation and is reduce sensorto the signed distance Signed distance function (SDF) based mapping is employed to fuse the measurements from multiple runs into a consistent representation and reduce sensor noise. From the signed distance multiple runs into a consistent representation and reduce sensor noise. From the signed distance function model we reconstruct a 3D surface mesh. We use this terrain model to establish a multiple runs into a consistent representation and reduce sensor noise. From the signed distance function model we a surface mesh. use this model to establish aa functionreality modelscene we reconstruct reconstruct a 3D 3D surface mesh. We We use mining this terrain terrain modelfor to testing establish virtual for immersive data visualization of the operations and function modelscene we reconstruct a 3D surface mesh. We use this terrain model to testing establish a virtual for data visualization of operations and virtual reality reality scene for immersive immersive data visualization of the the mining mining operations for foron testing and planing during development. Results of the proposed approach are demonstrated a dataset virtual reality scene for immersive data visualization of the mining operations foron testing and planing during development. Results of the proposed approach are demonstrated a dataset planing during development. Results ofinland the proposed are demonstrated on a dataset captured in an abandoned submerged mine. approach planing during development. Results of the proposed approach are demonstrated on a dataset captured captured in in an an abandoned abandoned submerged submerged inland inland mine. mine. captured in an abandonedFederation submerged inland mine. © 2017, IFAC (International of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: marine systems, underwater mapping, 3D reconstruction Keywords: marine marine systems, systems, underwater underwater mapping, mapping, 3D 3D reconstruction reconstruction Keywords: Keywords: marine systems, underwater mapping, 3D reconstruction 1. INTRODUCTION mining vehicle is lowered to the bottom of the mine pit. 1. INTRODUCTION INTRODUCTION mining vehiclecontrolled is lowered lowered to to the the bottomcenter of the the mine mine pit. pit. 1. mining vehicle is bottom of It is remotely a control on 1. INTRODUCTION mining vehiclecontrolled is loweredfrom to the bottomcenter of thelocated mine pit. It is remotely from a control located on It is surface remotely controlled fromand a control centerpowered located by on via optical fiber electrically The presented work was carried out within the Horizon the It is surface remotely controlled fromand a control centerpowered located by on the via optical fiber electrically The presented work was carried out within the Horizon the surface via optical fiber and electrically powered by umbilical cord. The mining vehicle cuts thepowered ore usingby a The presented work was carried out within Horizon an 2020 research project Viable Alternative Minethe Operating the surface via optical fiber and electrically The presented work was carried out within the Horizon an umbilical umbilical cord. The Thetype mining vehicle cuts the ore using usingby 2020 research research projectThe Viable Alternative Mine Operating an cord. mining vehicle ore aa hydraulic roadheader cutter. Thecuts ore the is collected 2020 project Viable Alternative Mine Operating System (¡VAMOS!). objective of this project is the an umbilical cord. The mining vehicle cuts the ore using a 2020 research projectThe Viable Alternative Operating hydraulic roadheader typebelow cutter. The ore and is collected collected by System (¡VAMOS!). objective of this thisMine project is raw the ahydraulic roadheader type cutter. ore is by suction mouth located theThe cutter the slurry System (¡VAMOS!). The objective of project is the development of a prototype mining system to extract hydraulic roadheader type cutter. The ore is collected by System (¡VAMOS!). The objective of this to project is raw the aa suction suction mouth located below thea vertical cutter and and the slurry development of an prototype mining system extract the cutter slurry pumpedmouth to the located surface below through riserthe hose. development of aa prototype mining system to extract raw is materials from abandoned water-filled ais suction mouth located below thea vertical cutter and the slurry development of an a prototype mining system open-pit to extractmine. raw is pumped to the surface through riser hose. materials from abandoned water-filled open-pit mine. pumped to the surface through a vertical riser hose. materials frommines an abandoned open-pit in mine. These inland have been water-filled considered depleted the For is pumped to these the surface through a vertical riser hose.the planning types of operations and assessing materials from an abandoned water-filled open-pit mine. Thesebecause inland with minesprevious have been been considered depleted in not the For planning these types of operations and assessing the These inland mines have considered depleted in the past mining techniques it was For planning these types of operations and assessing the of performing mining trials detailed bathymetry These inland with minesprevious have been considered depleted in not the viability past because mining techniques it was For planning these types of operations and assessing the past because with previous mining techniques it was not economically viableprevious anymoremining to continue operations. viability of performing performing mining trials detailed detailed bathymetry viability of mining trials bathymetry potential mining sites are necessary. However, many past because with techniques it was Tonot of economically viable anymore to continue operations. Toviability of performing mining trials detailed bathymetry economically viable anymore to rare continue operations. To- of day, with rising prices of certain ores it might become of potential mining sites are necessary. However, many potential mining sites are necessary. However, many mines do not have detailed historic records available. economically viable anymore to rare continue To- older day, with with rising rising prices of certain certain oresinit itoperations. mighttobecome become of potential mining sites are necessary. However, many day, of ores might interesting againprices to re-open theserare mines order access Moreover, older minesthe do not have detailed historic records available. older mines do not have detailed historic records available. records might not represent the current day, with rising prices of certain rare ores it might become interesting again to re-open these mines in order to access older mines do not have detailed historic records available. interesting again to re-open these mines in order to access deeper seated minerals. However, once in theorder mining oper- state Moreover, the records or might not represent represent the updated current records might not the current due tothe backfilling instabilities. Therefore, interesting again to re-open these mines to access deeper stop seated minerals. However, once the mining mining oper- Moreover, Moreover, the records or might not represent the updated current deeper seated minerals. However, once the operations and no water control is carried out anymore state due to backfilling instabilities. Therefore, state due to backfilling or instabilities. Therefore, updated map data is necessary to asses the mine site. deeper seated the mining oper- state due to backfilling or instabilities. Therefore, updated ations stop stop andminerals. no water waterHowever, control isonce carried out ations and no control is carried outoranymore anymore open-pit mines eventually fill up with ground surface map data is necessary to asses the mine site. map data is necessary to asses the mine site. ations stop and eventually no water control is carried outoranymore open-pit mines fill up with with ground surface map presented data is necessary to asses the mine site. out at the bathymetric survey was carried open-pit mines eventually up ground or surface water. Conventional miningfill techniques require high treat- The open-pit mines eventually fill up with ground or surface The presented bathymetric survey was carried outMining at the the water. Conventional mining techniques require high treatThe presented bathymetric survey was carried out at Bejanca mine site near Queir˜ a village in Portugal. water. Conventional mining techniques require high treatment and dewateringmining costs. This is especially The presented bathymetric survey wasincarried outMining at the water. Conventional techniques requireproblematic high treat- Bejanca Bejanca mine site near Queir˜ Queir˜ village Portugal. ment and dewatering costs. This is especially especially problematic mine site near aa village in Portugal. Mining operations were conducted there between 1919 and 1942. ment and dewatering costs. This is problematic in the presence of high pressure aquifers. Moreover, from Bejanca mine siteconducted near Queir˜ a village in Portugal. Mining ment and dewatering costs. This aquifers. is especially problematic operations were there between 1919are and 1942. in the the presence of perspective high pressure Moreover, from operations were between 1919 and minerals of conducted interest in there the open-pit mine tin1942. and in presence of high pressure aquifers. Moreover, an environmental it is desirable that thefrom wa- The operations were conducted there between 1919 and 1942. in the presence of high pressure aquifers. Moreover, from The minerals of interest in the open-pit mine are tin and an environmental perspective it is desirable that the waThe minerals of interest in the open-pit mine are tin and After the mine was closed in 1945 the pit filled an environmental perspective it is mines desirable thatchanged. the wa- tungsten. ter table of these flooded inland is not The minerals of the interest inwas theclosed open-pit minethe arepit tinfilled and an environmental perspective it is mines desirable thatchanged. the wa- tungsten. tungsten. After mine in 1945 ter table of these flooded inland is not After the mine was closed in 1945 the pit filled with water from the winter rains. The survey showed that ter table of these flooded inland mines is not changed. Therefore, the ¡VAMOS! project aims toisdevelop a new tungsten. After the mine was closed in 1945 the pit filled ter table of these flooded inland mines not changed. with water from the winter rains. The survey showed that Therefore,controlled the ¡VAMOS! ¡VAMOS! projectmining aims to to develop aawhich new there with water from significant the winter backfilling, rains. The survey has been which showed results that in a Therefore, the project aims develop new remotely underwater technique, with from significant the winter backfilling, rains. The survey showed that Therefore, the ¡VAMOS! projectmining aims to develop awhich new there therewater has been whichmine results in aa remotely controlled underwater technique, has been significant backfilling, which results in rather shallow waterbody. The submerged exhibits remotely controlled underwater mining technique, which is environmentally and economically more viable than the there has been waterbody. significant backfilling, whichmine results in a remotely controlledand underwater mining technique, which rather shallow The submerged exhibits is environmentally economically more viable than the rather shallow waterbody. The submerged mine exhibits depths ofwaterbody. up to 27 m The and asubmerged size of 125mine m × 90 m. is environmentally and economically more viable than the water state-of-the-art. rather shallow exhibits is environmentally and economically more viable than the water depths depths of of up up to to 27 27 m m and and aa size size of of 125 125 m m× × 90 90 m. m. state-of-the-art. water state-of-the-art. waterdataset depths was of uprecorded to 27 m and sizeautonomous of 125 m × 90 m. state-of-the-art. witha an surface A virtual reality scene of the envisioned mining system is The The dataset was recorded with an autonomous surface A virtual reality scene of the envisioned mining system is The dataset was recorded with an autonomous surface (ASV)was equipped with a multibeam sonar, surface Global A virtual in reality of the envisioned mining system depicted Fig. scene 1. From a launch and recovery vessel is a vehicle The dataset recorded with an autonomous A virtual in reality of the envisioned mining system vehicle (ASV) equipped equipped with multibeam sonar, Global Global depicted Fig. scene 1. From From launch and recovery recovery vessel is (ASV) with aa multibeam sonar, depicted in Fig. 1. aa launch and vessel aa vehicle vehicle (ASV) equipped with a multibeam sonar, Global depicted in Fig. 1. From a launch and recovery vessel a
Copyright © 2017, 2017 IFAC 1162Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2017 IFAC 1162 Copyright ©under 2017 responsibility IFAC 1162Control. Peer review of International Federation of Automatic Copyright © 2017 IFAC 1162 10.1016/j.ifacol.2017.08.397
Proceedings of the 20th IFAC World Congress 1140 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144 Toulouse, France, July 9-14, 2017
Fig. 1. Virtual reality scene of the ¡VAMOS! underwater mining system with the created terrain surface model. Positioning System (GPS) and inertial navigation system (INS). Some errors are introduced in the multibeam survey due to inaccuracies of the vehicle motion data and calibration errors. In this paper we propose post-processing techniques for creating a more consistent 3D mine model. The ASV took multiple passes of the mine with varying amount of overlap between individual laps, which can be exploited to compute an improved solution using a simultaneous localization and mapping (SLAM) algorithm. In this work we look at the problem of SLAM as a global trajectory optimization problem. We find an improved trajectory, such that the global consistency of point measurements is optimized. Moreover, we show how signed distance function (SDF) based mapping can be applied to fuse multiple observations into a single consistent surface representation. From the established mine model we create a virtual reality scene for immersive data visualization to find suitable landing positions for the mining vehicle, create a mine plan, or perform simulations. 2. STATE OF THE ART Typically bathymetric maps are created with multibeam echosounders. The vehicle motion is compensated using an attitude reference system and Global Navigation Satellite System (GNSS). Nowadays, different SLAM techniques have been proposed to improve the underwater surveys. In man-made structured environments, such as harbors, feature-based SLAM approaches have been proven to be effective. Typically employed features include planar patches (Pathak et al., 2010; Ozog and Eustice, 2013) and line features (Ribas et al., 2008). An example of a feature-less approach is the algorithm described by Barkby et al. (2012). They use Particle Filter based SLAM to create a 2.5D point cloud of the
seafloor. Individual particles are weighted based on how well the multibeam measurements agree with the global elevation map. Loop closures are detected using a Gaussian process regression of previous sonar beam observations. This allows to detect loop closures with minimal overlap and enforces consistency of neighboring map borders even if there is no overlap. Roman and Singh (2005) divide the terrain map into smaller sub-maps that are assumed to be error free. Overlapping sub-maps are first coarsely aligned using cross correlation and then the Iterative Closest Point (ICP) algorithm is used for fine registration. The relative pose measurements are then used to further constrain an Extended Kalman Filter (EKF) based mapping procedure. Bichucher et al. (2015) extend this approach with a Graph SLAM based framework to improve the full trajectory. Palomer et al. (2016) propose a coarse-to-fine scan matching technique using ICP, which takes point measurement uncertainties into account during submap registration. The proposed approach in this paper differs from these methods in the sense that it does not partition the trajectory into submaps, which are matched using rigid registration, but employs a continuous-time SLAM algorithm. For many bathymetry application 2.5D digital elevation maps (DEM) are created. More recently creating dense surface models from sonar imaging has become of interest. However, the significant amount of noise in acoustic measurements makes it challenging to extract surface meshes directly from the point cloud. Therefore, for reconstructing meshes from noisy data often the point measurements are integrated into an implicit surface description (Hornung and Kobbelt, 2006) or robust local surface descriptors are fitted to the 3D point cloud (Campos et al., 2014). Our work follows this direction and uses an SDF voxel map as an intermediate implicit surface model to create a more noise free representation.
1163
Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144
1141
3. MULTIBEAM SONAR SURVEY OF A SUBMERGED INLAND MINE
4. 3D MINE MAPPING AND MULTIPLE-VIEW DATA INTEGRATION
The robotic boat used for the mine mapping is the ASV ROAZ (Ferreira et al., 2009). It is a 4m long twin hull robotic vehicle with electric propulsion and autonomous navigation and control, see Fig. 2. For bathymetric mapping it is equipped with an Imagenex Delta T multibeam profiling sonar, which has a fan angle of 120 deg and a maximum range of 100 m. The experiments were conducted with a resolution of 480 beams and a beam width of 1.5 deg. Sonar data was recorded at 10 Hz.
For integrating measurements from multiple passes with the multibeam sonar we choose to employ SDF-based mapping. SDF voxel maps represent the surfaces implicitly by storing in each voxel cell the signed distance to the closest surface. Typically, the signed distance is only stored in a narrow band around the surfaces, which is referred to as a truncated signed distance function (TSDF). This representation became popular in the robotic mapping community with the work of Newcombe et al. (2011) on KinectFusion, which demonstrated excellent real time 3D reconstruction and tracking results.
For positioning and localization of the vehicle a L1/L2 precision GPS unit with Real Time Kinematic (RTK) differential corrections and a fiber optic based INS were installed on the robotic boat. The employed fiber optic gyro features a very low drift rating of only 0.05 deg /h. A high precision localization solution is later obtained by post-processing the raw INS data in combination with the raw GPS data. The post-processing step is performed using the Inertial Explorer software (NovAtel, 2016), where all raw GPS observations are processed in RTK and integrated with raw inertial measurements in a tightly coupled manner. The multibeam sonar, GPS antenna and INS were mounted rigidly to the same sensor bar. This was done to ensure that the relative positions and orientations stay consistent even during transport, which requires some disassembly. The sensor bar was mounted to the front of the vehicle with the multibeam sonar only a few centimeters below the water surface. The translation offsets between the individual sensors were measured manually. Rotational offsets between the INS and multibeam sonar are later estimated using a calibration routine as described in Sec 4.1. All sensor measurements are recorded with GPS timestamps for correct data association. The use of an ASV with autonomous navigation and synchronized on board logging capabilities allowed for an efficient data acquisition process. A trajectory was chosen, such that there is about 30 - 50% overlap between individual laps of the surface vehicle and all parts of the mine are covered multiple times.
A SDF map is a beneficial surface representation because noisy measurements are smoothed over multiple observations. However, overlap errors between multiple laps of the multibeam survey due to inaccuracies of the vehicle trajectory measurement or calibration errors can result in errors of the SDF model. In this case individual scan segments might not line up very well, which creates artifacts in the resulting surface model, such as additional surfaces or gaps in the 3D reconstruction. Therefore, we first apply a continuous-time SLAM technique to compute an improved trajectory of the robotic vehicle, which optimizes point cloud consistency. The examples shown in this paper are specific to a multibeam sonar, and the surface vehicle does not perform fully unconstrained 6-DOF motion. Please note that the proposed approach generalizes well to other types of sensors and unrestricted motion. 4.1 Calibration Although great care was taken to mount the INS and multibeam sonar aligned to the sensor bar, we need to calibrate the rotational offset between the two reference systems. Even very small alignment errors introduce large inconsistencies in the resulting point cloud. To do this we manually select a short trajectory segment, which we can assume to have minimal drift. Then the rotational offset is optimized based on an error measurement which determines point cloud quality similar to Sheehan et al. (2012). The error measurement is computed by splitting the trajectory into overlapping parts and calculating a point distance error based on closest point correspondences. We find the rotational offset parameters that minimize the error and verify the result on different trajectory segments. 4.2 Continous-time SLAM For processing the multibeam sonar data, we employ a continuous-time SLAM solution, also called semi-rigid SLAM. To understand the basic idea, we summarize its basis, 6D SLAM, which was initially designed as a GraphSLAM solution for point clouds. 6D SLAM works similarly to the the well-known ICP algorithm, which minimizes the following error function E(R, t) =
Fig. 2. ROAZ surface vehicle at the Bejanca mine site.
N 1 mi − (Rdi + t) 2 N i=1
(1)
to solve iteratively for an optimal transformation T = (R, t) with rotation R and translation t, where the tuples 1164
Proceedings of the 20th IFAC World Congress 1142 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144 Toulouse, France, July 9-14, 2017
(mi , di ) of corresponding model M and data points D are given by minimal distance, i.e., mi is the closest point to di within a close limit (Besl and McKay, 1992). Instead of the two-scan-Eq. (1), we look at the n-scan case: 2 E= |Rj mi + tj − (Rk di + tk )| , (2) j→k
i
where j and k refer to scans of the SLAM graph, i.e., to the graph modelling the pose constraints in SLAM. If they overlap, i.e., closest points are available, then the point pairs for the link are included in the minimization. We solve for all poses at the same time and iterate like in the original ICP. The derivation of a GraphSLAM method using a Mahalanobis distance that describes the global error of all the poses ¯ j,k − E′ )T C−1 (E ¯ ′ − E′ ) W = (E (3) j,k
j,k
j,k
j,k
j→k
where E′j,k is the linearized error metric and the Gaus¯ j,k , Cj,k ) with computed covariances sian distribution is (E from scan matching as given in Borrmann et al. (2008) does not lead to different results. Please note, while there are four closed-form solutions for the original ICP Eq. (1), linearization of the rotation in Eq. (2) or (3) is always required. Semi-rigid SLAM. The algorithm is adopted from Elseberg et al. (2013), where it was used in different mobile mapping contexts. We make no rigidity assumptions, except for the computation of the point correspondences. We also require no explicit motion model. This means the specific dynamics of the employed vehicle do not need to be known. The continuous-time SLAM for trajectory optimization works in full 6 DoF. The algorithm requires no computation of high-level visual or geometric feature descriptors, i.e., we require only the points themselves. In case of multibeam sonar mapping, we do not have separate 3D point clouds, just slices. In the current state of the art developed by Bosse and Zlot (2009) for improving overall map quality of mobile mappers in the robotics community the time is coarsely discretized. This results in a partition of the trajectory into sub-scans that are treated rigidly. Then rigid registration algorithms like the ICP and other solutions to the SLAM problem are employed. Obviously, trajectory errors within a sub-scan cannot be improved in this fashion. Applying rigid pose estimation to this non-rigid problem directly is also problematic since rigid transformations can only approximate the underlying ground truth. When a finer discretization is used, single 2D scan slices or single points result that do not constrain a 6 DoF pose sufficiently for rigid algorithms. Mathematical details of our algorithm are given in Elseberg et al. (2013). Essentially, we first split the trajectory into sections, and match these sections using the automatic high-precise registration of terrestrial 3D scans, i.e., globally consistent scan matching (Borrmann et al., 2008). Here, the graph is estimated using a heuristics that measures the overlap of sections using the number of closest point pairs. After applying globally consistent scan matching on the sections the actual semi-rigid matching as described in Elseberg et al. (2013) is applied, using the results of the rigid optimization as starting values to compute the numerical minimum of the underlying least
square problem. To speed up the calculations, we make use of the sparse Cholesky decomposition. A key issue in continuous-time SLAM is the search for closest point pairs. We use an octree and a multi-core implementation using OpenMP to solve this task efficiently. A time-threshold for the point pairs is used, i.e., we match only to points, if they were recorded at least td time steps away. For the presented experiments we choose a duration of 30 s which corresponds to 300 sonar scan slices. In addition, we use a maximal allowed point-to-point-distance which has been set to 50 cm. 4.3 Signed Distance Function Based Mapping We integrate all sonar scans into a SDF voxel model based on the optimized trajectory computed by the continuoustime SLAM solution. The signed distance measurement d(v) for a voxel with center v is computed as follows d(v) = m − p − v , (4) where p is the sensor position and m is the distance measurement of the sensor. Multiple measurements of the same voxel cell are integrated based on a weighting function f . This way noise cancels out over multiple observations. We store in each voxel cell the signed distance s(v) and the weight w(v). To integrate a new measurement d(v) at iteration k + 1 we compute the weighted average wk (v)sk (v) + f dk+1 (v) sk+1 (v) = , (5) wk (v) + f where f is a weight assigned to the new measurement. The signed distance is truncated to the interval [smin ; smax ]. Since we do not have an accurate noise model of the sonar sensor uniform weights (f = 1) are employed. The weight is updated by wk+1 (v) = min(wk (v) + f, wmax ) , (6) where wmax is the maximum weight. For the experiments in this paper we choose wmax = 20. SDF-based mapping is not completely robust to coarse outliers. Noisy surfaces are only smoothed if the individual measurements lie within a certain band, which is determined by the penetration depths Dmin and Dmax of the TSDF. Underwater sonar sensors typically exhibit some amount of coarse outliers. Measurement points that lie outside the truncation thresholds are integrated as additional surfaces. To address this problem we choose a large truncation threshold of 1.5 m. This limits the minimum thickness of objects that can be represented by the SDF model. However, in the particular case of the submerged inland mine this is not an issue because we only want to represent a single surface of the mine floor. To remove erroneous integrated surfaces we filter the SDF voxels based on the weight. This is based on the assumption that voxels representing real surfaces carry a higher weight, i.e., are observed more often, compared to voxels filled from measurement outliers. For modeling the mine we choose a voxel resolution of 10 cm. This means the TSDF space of the entire mine has a size in the order of a billion voxels. In order to store large maps with low memory consumption we need to encode free space efficiently. Different techniques to do this have been proposed: Whelan et al. (2012) store a
1165
Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144
1143
5
120 100
0 z in m
y in m
80 60
(c)
-5
40 20 0
-50
0 x in m
50
-10
-50
0 x in m
(a)
50
(d)
(b)
Fig. 3. Initial (black dashed line) and optimized (red line) trajectory (a,b), and cross sections of the point clouds created using the initial GPS/INS (c) and optimized trajectory (d). dense representation of the TSDF dynamically in a small predefined volume around the sensor in motion. The parts of the map that move outside this volume are converted to a triangular mesh and only the mesh is stored. Methods for storing the entire TSDF are voxel hashing (Nießner et al., 2013) or octree data structures (Steinbr¨ ucker et al., 2014). For this work we use a B-tree based data structure (Museth, 2013) to store the complete sparse TSDF grid. The tree has constant depth, which allows constant time local and random traversals. We use a three-level tree with branching factors decreasing closer to the leaves.
as depicted in Fig. 4. Fig. 4(a) shows the resulting point cloud using the GPS/INS trajectory while Fig. 4(b) shows the result using the optimized trajectory from continuoustime SLAM. The color encodes the depth. This dataset consists of 12786 multibeam scans captured at 10 Hz. It was captured in 22 min and the trajectory is 1567 m long (result of the SLAM solution). Especially at the bottom of the mine it is visible that the multibeam measurements are more consistent in the optimized results. This can be seen more clearly in the cross sections of the point clouds presented in Fig. 4(e) and Fig. 4(f).
To integrate the multibeam data in the TSDF we follow the generalized sensor fusion approach proposed by May et al. (2014). We model the multibeam as a polar line sensor with a beam width of 1.5 deg. Individual voxel cells within measurement range are then updated based on back projection using this sensor model. From the scalar TSDF grid we finally extract a surface mesh for visualization using the marching cubes algorithm.
Consequently, the extracted mesh from the SDF representation using the optimized continuous-time SLAM solution, depicted in Fig. 4(d), exhibits smoother surfaces than the initial result depicted in Fig. 4(c). Despite the noise of the measurements a smooth surface can be extracted if a sufficient amount of repeated observations are available.
4.4 Results To demonstrate the continuous-time SLAM algorithm Fig. 3 shows results on a trajectory with significant drift of multiple meters. In this specific case the GPS signal was lost temporarily during data acquisition, which explains the large trajectory errors. The dataset consists of 7291 multibeam scans captured at 10 Hz. It was captured in 13 min and the trajectory is 757 m long (result of the SLAM solution). Fig. 3 (a,b) depict the initial GPS/INS trajectory as a black dashed line and the optimized trajectory as a red continuous line. The x/y-plane is aligned parallel to the water surface. We can see that SLAM converges to a solution that puts the sensor poses closer to a planar motion as expected for a surface vehicle. Please note that the algorithm does not impose any movement constraints or rely on a vehicle motion model. Cross sections of the resulting point cloud are displayed in Fig.3 (c,d). We can clearly see misalignment between multiple passes of the multibeam sonar in the initial result shown in Fig.3 (c). Point measurements line up well using the improved trajectory estimate based on continuous-time SLAM visualized in Fig.3 (d). Moreover, also data captured with a good GPS/INS result can be further improved using the proposed techniques
The borders of the mine show holes in the mesh. This is a result of the irregular and low point density of the sonar measurements due to limited coverage close to the borders of the mine. Since this is undesirable, we later interpolate the holes for display in the virtual reality system. 5. CONCLUSION In this paper we showed first results on creating a detailed terrain model of a submerged inland mine from a multibeam sonar survey. We demonstrate that SLAM techniques are effective to remove inconsistencies due to inaccuracies of the motion data. Moreover, we presented a signed distance function based approach for data fusion. Rendering a surface mesh for visualization compared to a point cloud is advantageous because it allows the human operator to see the surfaces and structure more clearly. We integrate the terrain model into a virtual reality scene together with prototype models of the envisioned mining system, which is used for pre-planning, visualization and development. ACKNOWLEDGEMENTS This work was supported by the European Union’s Horizon 2020 research and innovation programme under grant agreement No 642477.
1166
Depth in m
Proceedings of the 20th IFAC World Congress 1144 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144 Toulouse, France, July 9-14, 2017
(c)
(e)
(b)
(d)
(f)
Depth in m
(a)
Fig. 4. Initial (a) and optimized (b) 3D point cloud, surface mesh extracted from SDF model using initial GPS/INS (c) and continuous-time SLAM solution (d), and a cross section of initial (e) and optimized (f) point cloud. REFERENCES Barkby, S., Williams, S.B., Pizarro, O., and Jakuba, M.V. (2012). Bathymetric particle filter SLAM using trajectory maps. The International Journal of Robotics Research, 31(12), 1409–1430. Besl, P.J. and McKay, N.D. (1992). Method for registration of 3-D shapes. In IEEE Trans. Pattern Analysis and Machine Intelligenc, volume 14, 239–256. Bichucher, V., Walls, J.M., Ozog, P., Skinner, K.A., and Eustice, R.M. (2015). Bathymetric factor graph SLAM with sparse point cloud alignment. In Proc. OCEANS 2015 - MTS/IEEE Washington, 1–7. Borrmann, D., Elseberg, J., Lingemann, K., N¨ uchter, A., and Hertzberg, J. (2008). Globally consistent 3D mapping with scan matching. Robotics and Autonomous Systems, 56(2), 130–142. Bosse, M. and Zlot, R. (2009). Continuous 3D scanmatching with a spinning 2D laser. In Proc. IEEE Int. Conf. Robotics and Automation, 4312–4319. Campos, R., Garcia, R., Alliez, P., and Yvinec, M. (2014). A surface reconstruction method for in-detail underwater 3D optical mapping. The International Journal of Robotics Research, 64 – 89. Elseberg, J., Borrmann, D., and N¨ uchter, A. (2013). Algorithmic solutions for computing precise maximum likelihood 3D point clouds from mobile laser scanning platforms. Remote Sensing, 5(11), 5871–5906. Ferreira, H., Almeida, C., Martins, A., Almeida, J., Dias, N., Dias, A., and Silva, E. (2009). Autonomous bathymetry for risk assessment with ROAZ robotic surface vehicle. In Proc. OCEANS 2009 - Europe, 1–6. Hornung, A. and Kobbelt, L. (2006). Robust reconstruction of watertight 3D models from non-uniformly sampled point clouds without normal information. In Fourth Eurographics Symp. on Geometry Processing, 41–50. May, S., Koch, P., Koch, R., Merkl, C., Pfitzner, C., and N¨ uchter, A. (2014). A generalized 2D and 3D multisensor data integration approach based on signed distance functions for multi-modal robotic mapping. In 19th Int. Workshop on Vision, Modeling and Visualization, 95–102.
Museth, K. (2013). VDB: High-resolution sparse volumes with dynamic topology. ACM Transactions on Graphics, 32(3), 27. Newcombe, R.A., Izadi, S., Hilliges, O., Molyneaux, D., Kim, D., Davison, A.J., Kohi, P., Shotton, J., Hodges, S., and Fitzgibbon, A. (2011). KinectFusion: Real-time dense surface mapping and tracking. In Proc. 10th IEEE Int. Mixed and Augmented Reality Symp., 127–136. Nießner, M., Zollh¨ofer, M., Izadi, S., and Stamminger, M. (2013). Real-time 3D reconstruction at scale using voxel hashing. ACM Transactions on Graphics, 32(6), 169. NovAtel (2016). Inertial Explorer. http://www.novatel. com/products/software/inertial-explorer/. Ozog, P. and Eustice, R.M. (2013). Real-time SLAM with piecewise-planar surface models and sparse 3D point clouds. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 1042–1049. Palomer, A., Ridao, P., and Ribas, D. (2016). Multibeam 3D underwater slam with probabilistic registration. Sensors, 16(4), 560. Pathak, K., Birk, A., and Vaskevicius, N. (2010). Planebased registration of sonar data for underwater 3D mapping. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 4880–4885. Ribas, D., Ridao, P., Tard´os, J.D., and Neira, J. (2008). Underwater SLAM in man-made structured environments. Journal of Field Robotics, 25(11-12), 898–921. Roman, C. and Singh, H. (2005). Improved vehicle based multibeam bathymetry using sub-maps and SLAM. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 3662–3669. Sheehan, M., Harrison, A., and Newman, P. (2012). Selfcalibration for a 3D laser. The International Journal of Robotics Research, 31(5), 675–687. Steinbr¨ ucker, F., Sturm, J., and Cremers, D. (2014). Volumetric 3D mapping in real-time on a CPU. In Proc. IEEE Int. Conf. Robotics and Automation, 2021–2028. Whelan, T., Kaess, M., Fallon, M., Johannsson, H., Leonard, J., and McDonald, J. (2012). Kintinuous: Spatially extended KinectFusion. Technical report, Massachusetts Institute of Technology.
1167
ScienceDirect
IFAC PapersOnLine 50-1 (2017) 1139–1144
Signed Distance Function Based Surface Signed Distance Function Based Surface Signed Distance of Function Based Surface Reconstruction a Submerged Inland Reconstruction of a Submerged Inland Reconstruction of a SubmergedSLAM Inland Mine Using Continuous-time Mine Using Continuous-time SLAM Mine Using† Continuous-time SLAM∗ ∗,∗∗
Michael Bleier † Andr´ e Dias ∗,∗∗ Ant´ onio Ferreira ∗ ∗ ∗∗∗† Andr´ ∗,∗∗ Michael Bleier e Ant´ o Michael Bleier Andr´ e Dias Dias ∗,∗∗ Ant´ onio nio Ferreira Ferreira John Pidgeon Jos´ e Almeida Eduardo Silva ∗,∗∗ † ∗,∗∗ ∗ ∗∗∗ ∗,∗∗ Michael Bleier Andr´ e Dias ∗,∗∗ Ant´ onio Ferreira ∗∗∗ Jos´ ∗,∗∗ ∗,∗∗ +,† +,† John e Almeida Eduardo Silva John Pidgeon Pidgeon Jos´ e Almeida Eduardo Silva Klaus Schilling Andreas N¨ u chter ∗∗∗ ∗,∗∗ ∗,∗∗ +,† John Pidgeon Jos´ e +,† Almeida Eduardo Silva Klaus u Klaus Schilling Schilling +,† Andreas Andreas N¨ N¨ uchter chter +,† Klaus Schilling +,† Andreas N¨ uchter +,† ∗ INESC Technology and Science, Porto, Portugal ∗ ∗ INESC Technology and Science, Porto, Portugal ∗∗ INESC Technology and Science, Porto, School of Engineering, Polytechnic Institute of Portugal Porto, Portugal ∗ ∗∗ Technology and Science, Porto, Portugal ∗∗ School INESC of∗∗∗Engineering, Polytechnic Institute of Engineering, Polytechnic InstituteAustralia of Porto, Porto, Portugal Portugal BMT WBM Pty Ltd, Brisbane, ∗∗ School of ∗∗∗ Engineering, Polytechnic InstituteAustralia of Porto, Portugal ∗∗∗ + School of BMT WBM Pty Ltd, Brisbane, BMT WBM Pty Ltd, Brisbane, Australia Informatics VII – Robotics and Telematics, Julius Maximilian ∗∗∗ + WBM Pty and Ltd,Telematics, Brisbane, Australia + InformaticsBMT VII – Julius – Robotics Robotics and Telematics, Julius Maximilian Maximilian University of W¨ urzburg, Germany + Informatics VII Informatics VII – Robotics and Telematics, Julius Maximilian † University of W¨ u rzburg, Germany University of W¨ ue.V., rzburg, Germany Zentrum f¨ u r Telematik W¨ u rzburg, Germany, † University of W¨ urzburg, GermanyGermany, † Zentrum u u f¨ urr Telematik Telematik e.V., e.V., W¨ W¨ urzburg, rzburg, Germany, (e-mail: f¨ [email protected]) † Zentrum Zentrum f¨ u r Telematik e.V., W¨ u rzburg, Germany, (e-mail: (e-mail: [email protected]) [email protected]) (e-mail: [email protected]) Abstract: The planning of mining operations in water filled open-pit mines requires detailed Abstract: planning of filled open-pit requires detailed Abstract: The The planning of mining mining operations in water filled risks. open-pit mines requires detailed bathymetry to create a mine plan operations and assess in thewater involved Thismines paper presents postAbstract: The planning of mining operations in water filled risks. open-pit mines requires detailed bathymetry to plan and assess involved paper presents postbathymetrytechniques to create create a aformine mine plan an andimproved assess the the involved risks.aThis This paper presents postprocessing creating 3D model from survey carried out using bathymetry to create aformine plan an andimproved assess the involved risks.aThis paper presents postprocessing techniques creating 3D model from survey carried out using processing techniques for creating ana improved 3Dsonar model from a surveynavigation carried outsystem. using an autonomous surface vehicle with multibeam and a GPS/INS processing techniques for creating an improved 3D model from a survey carried out using an vehicle with sonar and system. an autonomous autonomousofsurface surface vehiclepoint with aa multibeam multibeam sonar and aa GPS/INS GPS/INS navigation system. Inconsistencies the created as a result of calibration errors navigation or GPS signal loss an autonomousofsurface vehicle with cloud a multibeam sonar and a GPS/INS navigation system. Inconsistencies created as of errors GPS loss Inconsistencies of the the created point point cloud cloud as aa result result of calibration calibration errors or or(SLAM) GPS signal signal loss are corrected using a continuous-time simultaneous localization and mapping solution. Inconsistencies of the created point cloud as a result of calibration errors or(SLAM) GPS signal loss are using a simultaneous localization and mapping are corrected corrected using a continuous-time continuous-time simultaneous localization and mapping (SLAM) solution. solution. Signed distance function (SDF) based mapping is employed to fuse the measurements from are corrected using a continuous-time simultaneousis localization and mapping (SLAM) solution. Signed (SDF) based employed fuse the from Signed distance distance function (SDF) based mapping mapping employed tonoise. fuse From the measurements measurements from multiple runs intofunction a consistent representation and is reduce sensorto the signed distance Signed distance function (SDF) based mapping is employed to fuse the measurements from multiple runs into a consistent representation and reduce sensor noise. From the signed distance multiple runs into a consistent representation and reduce sensor noise. From the signed distance function model we reconstruct a 3D surface mesh. We use this terrain model to establish a multiple runs into a consistent representation and reduce sensor noise. From the signed distance function model we a surface mesh. use this model to establish aa functionreality modelscene we reconstruct reconstruct a 3D 3D surface mesh. We We use mining this terrain terrain modelfor to testing establish virtual for immersive data visualization of the operations and function modelscene we reconstruct a 3D surface mesh. We use this terrain model to testing establish a virtual for data visualization of operations and virtual reality reality scene for immersive immersive data visualization of the the mining mining operations for foron testing and planing during development. Results of the proposed approach are demonstrated a dataset virtual reality scene for immersive data visualization of the mining operations foron testing and planing during development. Results of the proposed approach are demonstrated a dataset planing during development. Results ofinland the proposed are demonstrated on a dataset captured in an abandoned submerged mine. approach planing during development. Results of the proposed approach are demonstrated on a dataset captured captured in in an an abandoned abandoned submerged submerged inland inland mine. mine. captured in an abandonedFederation submerged inland mine. © 2017, IFAC (International of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: marine systems, underwater mapping, 3D reconstruction Keywords: marine marine systems, systems, underwater underwater mapping, mapping, 3D 3D reconstruction reconstruction Keywords: Keywords: marine systems, underwater mapping, 3D reconstruction 1. INTRODUCTION mining vehicle is lowered to the bottom of the mine pit. 1. INTRODUCTION INTRODUCTION mining vehiclecontrolled is lowered lowered to to the the bottomcenter of the the mine mine pit. pit. 1. mining vehicle is bottom of It is remotely a control on 1. INTRODUCTION mining vehiclecontrolled is loweredfrom to the bottomcenter of thelocated mine pit. It is remotely from a control located on It is surface remotely controlled fromand a control centerpowered located by on via optical fiber electrically The presented work was carried out within the Horizon the It is surface remotely controlled fromand a control centerpowered located by on the via optical fiber electrically The presented work was carried out within the Horizon the surface via optical fiber and electrically powered by umbilical cord. The mining vehicle cuts thepowered ore usingby a The presented work was carried out within Horizon an 2020 research project Viable Alternative Minethe Operating the surface via optical fiber and electrically The presented work was carried out within the Horizon an umbilical umbilical cord. The Thetype mining vehicle cuts the ore using usingby 2020 research research projectThe Viable Alternative Mine Operating an cord. mining vehicle ore aa hydraulic roadheader cutter. Thecuts ore the is collected 2020 project Viable Alternative Mine Operating System (¡VAMOS!). objective of this project is the an umbilical cord. The mining vehicle cuts the ore using a 2020 research projectThe Viable Alternative Operating hydraulic roadheader typebelow cutter. The ore and is collected collected by System (¡VAMOS!). objective of this thisMine project is raw the ahydraulic roadheader type cutter. ore is by suction mouth located theThe cutter the slurry System (¡VAMOS!). The objective of project is the development of a prototype mining system to extract hydraulic roadheader type cutter. The ore is collected by System (¡VAMOS!). The objective of this to project is raw the aa suction suction mouth located below thea vertical cutter and and the slurry development of an prototype mining system extract the cutter slurry pumpedmouth to the located surface below through riserthe hose. development of aa prototype mining system to extract raw is materials from abandoned water-filled ais suction mouth located below thea vertical cutter and the slurry development of an a prototype mining system open-pit to extractmine. raw is pumped to the surface through riser hose. materials from abandoned water-filled open-pit mine. pumped to the surface through a vertical riser hose. materials frommines an abandoned open-pit in mine. These inland have been water-filled considered depleted the For is pumped to these the surface through a vertical riser hose.the planning types of operations and assessing materials from an abandoned water-filled open-pit mine. Thesebecause inland with minesprevious have been been considered depleted in not the For planning these types of operations and assessing the These inland mines have considered depleted in the past mining techniques it was For planning these types of operations and assessing the of performing mining trials detailed bathymetry These inland with minesprevious have been considered depleted in not the viability past because mining techniques it was For planning these types of operations and assessing the past because with previous mining techniques it was not economically viableprevious anymoremining to continue operations. viability of performing performing mining trials detailed detailed bathymetry viability of mining trials bathymetry potential mining sites are necessary. However, many past because with techniques it was Tonot of economically viable anymore to continue operations. Toviability of performing mining trials detailed bathymetry economically viable anymore to rare continue operations. To- of day, with rising prices of certain ores it might become of potential mining sites are necessary. However, many potential mining sites are necessary. However, many mines do not have detailed historic records available. economically viable anymore to rare continue To- older day, with with rising rising prices of certain certain oresinit itoperations. mighttobecome become of potential mining sites are necessary. However, many day, of ores might interesting againprices to re-open theserare mines order access Moreover, older minesthe do not have detailed historic records available. older mines do not have detailed historic records available. records might not represent the current day, with rising prices of certain rare ores it might become interesting again to re-open these mines in order to access older mines do not have detailed historic records available. interesting again to re-open these mines in order to access deeper seated minerals. However, once in theorder mining oper- state Moreover, the records or might not represent represent the updated current records might not the current due tothe backfilling instabilities. Therefore, interesting again to re-open these mines to access deeper stop seated minerals. However, once the mining mining oper- Moreover, Moreover, the records or might not represent the updated current deeper seated minerals. However, once the operations and no water control is carried out anymore state due to backfilling instabilities. Therefore, state due to backfilling or instabilities. Therefore, updated map data is necessary to asses the mine site. deeper seated the mining oper- state due to backfilling or instabilities. Therefore, updated ations stop stop andminerals. no water waterHowever, control isonce carried out ations and no control is carried outoranymore anymore open-pit mines eventually fill up with ground surface map data is necessary to asses the mine site. map data is necessary to asses the mine site. ations stop and eventually no water control is carried outoranymore open-pit mines fill up with with ground surface map presented data is necessary to asses the mine site. out at the bathymetric survey was carried open-pit mines eventually up ground or surface water. Conventional miningfill techniques require high treat- The open-pit mines eventually fill up with ground or surface The presented bathymetric survey was carried outMining at the the water. Conventional mining techniques require high treatThe presented bathymetric survey was carried out at Bejanca mine site near Queir˜ a village in Portugal. water. Conventional mining techniques require high treatment and dewateringmining costs. This is especially The presented bathymetric survey wasincarried outMining at the water. Conventional techniques requireproblematic high treat- Bejanca Bejanca mine site near Queir˜ Queir˜ village Portugal. ment and dewatering costs. This is especially especially problematic mine site near aa village in Portugal. Mining operations were conducted there between 1919 and 1942. ment and dewatering costs. This is problematic in the presence of high pressure aquifers. Moreover, from Bejanca mine siteconducted near Queir˜ a village in Portugal. Mining ment and dewatering costs. This aquifers. is especially problematic operations were there between 1919are and 1942. in the the presence of perspective high pressure Moreover, from operations were between 1919 and minerals of conducted interest in there the open-pit mine tin1942. and in presence of high pressure aquifers. Moreover, an environmental it is desirable that thefrom wa- The operations were conducted there between 1919 and 1942. in the presence of high pressure aquifers. Moreover, from The minerals of interest in the open-pit mine are tin and an environmental perspective it is desirable that the waThe minerals of interest in the open-pit mine are tin and After the mine was closed in 1945 the pit filled an environmental perspective it is mines desirable thatchanged. the wa- tungsten. ter table of these flooded inland is not The minerals of the interest inwas theclosed open-pit minethe arepit tinfilled and an environmental perspective it is mines desirable thatchanged. the wa- tungsten. tungsten. After mine in 1945 ter table of these flooded inland is not After the mine was closed in 1945 the pit filled with water from the winter rains. The survey showed that ter table of these flooded inland mines is not changed. Therefore, the ¡VAMOS! project aims toisdevelop a new tungsten. After the mine was closed in 1945 the pit filled ter table of these flooded inland mines not changed. with water from the winter rains. The survey showed that Therefore,controlled the ¡VAMOS! ¡VAMOS! projectmining aims to to develop aawhich new there with water from significant the winter backfilling, rains. The survey has been which showed results that in a Therefore, the project aims develop new remotely underwater technique, with from significant the winter backfilling, rains. The survey showed that Therefore, the ¡VAMOS! projectmining aims to develop awhich new there therewater has been whichmine results in aa remotely controlled underwater technique, has been significant backfilling, which results in rather shallow waterbody. The submerged exhibits remotely controlled underwater mining technique, which is environmentally and economically more viable than the there has been waterbody. significant backfilling, whichmine results in a remotely controlledand underwater mining technique, which rather shallow The submerged exhibits is environmentally economically more viable than the rather shallow waterbody. The submerged mine exhibits depths ofwaterbody. up to 27 m The and asubmerged size of 125mine m × 90 m. is environmentally and economically more viable than the water state-of-the-art. rather shallow exhibits is environmentally and economically more viable than the water depths depths of of up up to to 27 27 m m and and aa size size of of 125 125 m m× × 90 90 m. m. state-of-the-art. water state-of-the-art. waterdataset depths was of uprecorded to 27 m and sizeautonomous of 125 m × 90 m. state-of-the-art. witha an surface A virtual reality scene of the envisioned mining system is The The dataset was recorded with an autonomous surface A virtual reality scene of the envisioned mining system is The dataset was recorded with an autonomous surface (ASV)was equipped with a multibeam sonar, surface Global A virtual in reality of the envisioned mining system depicted Fig. scene 1. From a launch and recovery vessel is a vehicle The dataset recorded with an autonomous A virtual in reality of the envisioned mining system vehicle (ASV) equipped equipped with multibeam sonar, Global Global depicted Fig. scene 1. From From launch and recovery recovery vessel is (ASV) with aa multibeam sonar, depicted in Fig. 1. aa launch and vessel aa vehicle vehicle (ASV) equipped with a multibeam sonar, Global depicted in Fig. 1. From a launch and recovery vessel a
Copyright © 2017, 2017 IFAC 1162Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2017 IFAC 1162 Copyright ©under 2017 responsibility IFAC 1162Control. Peer review of International Federation of Automatic Copyright © 2017 IFAC 1162 10.1016/j.ifacol.2017.08.397
Proceedings of the 20th IFAC World Congress 1140 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144 Toulouse, France, July 9-14, 2017
Fig. 1. Virtual reality scene of the ¡VAMOS! underwater mining system with the created terrain surface model. Positioning System (GPS) and inertial navigation system (INS). Some errors are introduced in the multibeam survey due to inaccuracies of the vehicle motion data and calibration errors. In this paper we propose post-processing techniques for creating a more consistent 3D mine model. The ASV took multiple passes of the mine with varying amount of overlap between individual laps, which can be exploited to compute an improved solution using a simultaneous localization and mapping (SLAM) algorithm. In this work we look at the problem of SLAM as a global trajectory optimization problem. We find an improved trajectory, such that the global consistency of point measurements is optimized. Moreover, we show how signed distance function (SDF) based mapping can be applied to fuse multiple observations into a single consistent surface representation. From the established mine model we create a virtual reality scene for immersive data visualization to find suitable landing positions for the mining vehicle, create a mine plan, or perform simulations. 2. STATE OF THE ART Typically bathymetric maps are created with multibeam echosounders. The vehicle motion is compensated using an attitude reference system and Global Navigation Satellite System (GNSS). Nowadays, different SLAM techniques have been proposed to improve the underwater surveys. In man-made structured environments, such as harbors, feature-based SLAM approaches have been proven to be effective. Typically employed features include planar patches (Pathak et al., 2010; Ozog and Eustice, 2013) and line features (Ribas et al., 2008). An example of a feature-less approach is the algorithm described by Barkby et al. (2012). They use Particle Filter based SLAM to create a 2.5D point cloud of the
seafloor. Individual particles are weighted based on how well the multibeam measurements agree with the global elevation map. Loop closures are detected using a Gaussian process regression of previous sonar beam observations. This allows to detect loop closures with minimal overlap and enforces consistency of neighboring map borders even if there is no overlap. Roman and Singh (2005) divide the terrain map into smaller sub-maps that are assumed to be error free. Overlapping sub-maps are first coarsely aligned using cross correlation and then the Iterative Closest Point (ICP) algorithm is used for fine registration. The relative pose measurements are then used to further constrain an Extended Kalman Filter (EKF) based mapping procedure. Bichucher et al. (2015) extend this approach with a Graph SLAM based framework to improve the full trajectory. Palomer et al. (2016) propose a coarse-to-fine scan matching technique using ICP, which takes point measurement uncertainties into account during submap registration. The proposed approach in this paper differs from these methods in the sense that it does not partition the trajectory into submaps, which are matched using rigid registration, but employs a continuous-time SLAM algorithm. For many bathymetry application 2.5D digital elevation maps (DEM) are created. More recently creating dense surface models from sonar imaging has become of interest. However, the significant amount of noise in acoustic measurements makes it challenging to extract surface meshes directly from the point cloud. Therefore, for reconstructing meshes from noisy data often the point measurements are integrated into an implicit surface description (Hornung and Kobbelt, 2006) or robust local surface descriptors are fitted to the 3D point cloud (Campos et al., 2014). Our work follows this direction and uses an SDF voxel map as an intermediate implicit surface model to create a more noise free representation.
1163
Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144
1141
3. MULTIBEAM SONAR SURVEY OF A SUBMERGED INLAND MINE
4. 3D MINE MAPPING AND MULTIPLE-VIEW DATA INTEGRATION
The robotic boat used for the mine mapping is the ASV ROAZ (Ferreira et al., 2009). It is a 4m long twin hull robotic vehicle with electric propulsion and autonomous navigation and control, see Fig. 2. For bathymetric mapping it is equipped with an Imagenex Delta T multibeam profiling sonar, which has a fan angle of 120 deg and a maximum range of 100 m. The experiments were conducted with a resolution of 480 beams and a beam width of 1.5 deg. Sonar data was recorded at 10 Hz.
For integrating measurements from multiple passes with the multibeam sonar we choose to employ SDF-based mapping. SDF voxel maps represent the surfaces implicitly by storing in each voxel cell the signed distance to the closest surface. Typically, the signed distance is only stored in a narrow band around the surfaces, which is referred to as a truncated signed distance function (TSDF). This representation became popular in the robotic mapping community with the work of Newcombe et al. (2011) on KinectFusion, which demonstrated excellent real time 3D reconstruction and tracking results.
For positioning and localization of the vehicle a L1/L2 precision GPS unit with Real Time Kinematic (RTK) differential corrections and a fiber optic based INS were installed on the robotic boat. The employed fiber optic gyro features a very low drift rating of only 0.05 deg /h. A high precision localization solution is later obtained by post-processing the raw INS data in combination with the raw GPS data. The post-processing step is performed using the Inertial Explorer software (NovAtel, 2016), where all raw GPS observations are processed in RTK and integrated with raw inertial measurements in a tightly coupled manner. The multibeam sonar, GPS antenna and INS were mounted rigidly to the same sensor bar. This was done to ensure that the relative positions and orientations stay consistent even during transport, which requires some disassembly. The sensor bar was mounted to the front of the vehicle with the multibeam sonar only a few centimeters below the water surface. The translation offsets between the individual sensors were measured manually. Rotational offsets between the INS and multibeam sonar are later estimated using a calibration routine as described in Sec 4.1. All sensor measurements are recorded with GPS timestamps for correct data association. The use of an ASV with autonomous navigation and synchronized on board logging capabilities allowed for an efficient data acquisition process. A trajectory was chosen, such that there is about 30 - 50% overlap between individual laps of the surface vehicle and all parts of the mine are covered multiple times.
A SDF map is a beneficial surface representation because noisy measurements are smoothed over multiple observations. However, overlap errors between multiple laps of the multibeam survey due to inaccuracies of the vehicle trajectory measurement or calibration errors can result in errors of the SDF model. In this case individual scan segments might not line up very well, which creates artifacts in the resulting surface model, such as additional surfaces or gaps in the 3D reconstruction. Therefore, we first apply a continuous-time SLAM technique to compute an improved trajectory of the robotic vehicle, which optimizes point cloud consistency. The examples shown in this paper are specific to a multibeam sonar, and the surface vehicle does not perform fully unconstrained 6-DOF motion. Please note that the proposed approach generalizes well to other types of sensors and unrestricted motion. 4.1 Calibration Although great care was taken to mount the INS and multibeam sonar aligned to the sensor bar, we need to calibrate the rotational offset between the two reference systems. Even very small alignment errors introduce large inconsistencies in the resulting point cloud. To do this we manually select a short trajectory segment, which we can assume to have minimal drift. Then the rotational offset is optimized based on an error measurement which determines point cloud quality similar to Sheehan et al. (2012). The error measurement is computed by splitting the trajectory into overlapping parts and calculating a point distance error based on closest point correspondences. We find the rotational offset parameters that minimize the error and verify the result on different trajectory segments. 4.2 Continous-time SLAM For processing the multibeam sonar data, we employ a continuous-time SLAM solution, also called semi-rigid SLAM. To understand the basic idea, we summarize its basis, 6D SLAM, which was initially designed as a GraphSLAM solution for point clouds. 6D SLAM works similarly to the the well-known ICP algorithm, which minimizes the following error function E(R, t) =
Fig. 2. ROAZ surface vehicle at the Bejanca mine site.
N 1 mi − (Rdi + t) 2 N i=1
(1)
to solve iteratively for an optimal transformation T = (R, t) with rotation R and translation t, where the tuples 1164
Proceedings of the 20th IFAC World Congress 1142 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144 Toulouse, France, July 9-14, 2017
(mi , di ) of corresponding model M and data points D are given by minimal distance, i.e., mi is the closest point to di within a close limit (Besl and McKay, 1992). Instead of the two-scan-Eq. (1), we look at the n-scan case: 2 E= |Rj mi + tj − (Rk di + tk )| , (2) j→k
i
where j and k refer to scans of the SLAM graph, i.e., to the graph modelling the pose constraints in SLAM. If they overlap, i.e., closest points are available, then the point pairs for the link are included in the minimization. We solve for all poses at the same time and iterate like in the original ICP. The derivation of a GraphSLAM method using a Mahalanobis distance that describes the global error of all the poses ¯ j,k − E′ )T C−1 (E ¯ ′ − E′ ) W = (E (3) j,k
j,k
j,k
j,k
j→k
where E′j,k is the linearized error metric and the Gaus¯ j,k , Cj,k ) with computed covariances sian distribution is (E from scan matching as given in Borrmann et al. (2008) does not lead to different results. Please note, while there are four closed-form solutions for the original ICP Eq. (1), linearization of the rotation in Eq. (2) or (3) is always required. Semi-rigid SLAM. The algorithm is adopted from Elseberg et al. (2013), where it was used in different mobile mapping contexts. We make no rigidity assumptions, except for the computation of the point correspondences. We also require no explicit motion model. This means the specific dynamics of the employed vehicle do not need to be known. The continuous-time SLAM for trajectory optimization works in full 6 DoF. The algorithm requires no computation of high-level visual or geometric feature descriptors, i.e., we require only the points themselves. In case of multibeam sonar mapping, we do not have separate 3D point clouds, just slices. In the current state of the art developed by Bosse and Zlot (2009) for improving overall map quality of mobile mappers in the robotics community the time is coarsely discretized. This results in a partition of the trajectory into sub-scans that are treated rigidly. Then rigid registration algorithms like the ICP and other solutions to the SLAM problem are employed. Obviously, trajectory errors within a sub-scan cannot be improved in this fashion. Applying rigid pose estimation to this non-rigid problem directly is also problematic since rigid transformations can only approximate the underlying ground truth. When a finer discretization is used, single 2D scan slices or single points result that do not constrain a 6 DoF pose sufficiently for rigid algorithms. Mathematical details of our algorithm are given in Elseberg et al. (2013). Essentially, we first split the trajectory into sections, and match these sections using the automatic high-precise registration of terrestrial 3D scans, i.e., globally consistent scan matching (Borrmann et al., 2008). Here, the graph is estimated using a heuristics that measures the overlap of sections using the number of closest point pairs. After applying globally consistent scan matching on the sections the actual semi-rigid matching as described in Elseberg et al. (2013) is applied, using the results of the rigid optimization as starting values to compute the numerical minimum of the underlying least
square problem. To speed up the calculations, we make use of the sparse Cholesky decomposition. A key issue in continuous-time SLAM is the search for closest point pairs. We use an octree and a multi-core implementation using OpenMP to solve this task efficiently. A time-threshold for the point pairs is used, i.e., we match only to points, if they were recorded at least td time steps away. For the presented experiments we choose a duration of 30 s which corresponds to 300 sonar scan slices. In addition, we use a maximal allowed point-to-point-distance which has been set to 50 cm. 4.3 Signed Distance Function Based Mapping We integrate all sonar scans into a SDF voxel model based on the optimized trajectory computed by the continuoustime SLAM solution. The signed distance measurement d(v) for a voxel with center v is computed as follows d(v) = m − p − v , (4) where p is the sensor position and m is the distance measurement of the sensor. Multiple measurements of the same voxel cell are integrated based on a weighting function f . This way noise cancels out over multiple observations. We store in each voxel cell the signed distance s(v) and the weight w(v). To integrate a new measurement d(v) at iteration k + 1 we compute the weighted average wk (v)sk (v) + f dk+1 (v) sk+1 (v) = , (5) wk (v) + f where f is a weight assigned to the new measurement. The signed distance is truncated to the interval [smin ; smax ]. Since we do not have an accurate noise model of the sonar sensor uniform weights (f = 1) are employed. The weight is updated by wk+1 (v) = min(wk (v) + f, wmax ) , (6) where wmax is the maximum weight. For the experiments in this paper we choose wmax = 20. SDF-based mapping is not completely robust to coarse outliers. Noisy surfaces are only smoothed if the individual measurements lie within a certain band, which is determined by the penetration depths Dmin and Dmax of the TSDF. Underwater sonar sensors typically exhibit some amount of coarse outliers. Measurement points that lie outside the truncation thresholds are integrated as additional surfaces. To address this problem we choose a large truncation threshold of 1.5 m. This limits the minimum thickness of objects that can be represented by the SDF model. However, in the particular case of the submerged inland mine this is not an issue because we only want to represent a single surface of the mine floor. To remove erroneous integrated surfaces we filter the SDF voxels based on the weight. This is based on the assumption that voxels representing real surfaces carry a higher weight, i.e., are observed more often, compared to voxels filled from measurement outliers. For modeling the mine we choose a voxel resolution of 10 cm. This means the TSDF space of the entire mine has a size in the order of a billion voxels. In order to store large maps with low memory consumption we need to encode free space efficiently. Different techniques to do this have been proposed: Whelan et al. (2012) store a
1165
Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144
1143
5
120 100
0 z in m
y in m
80 60
(c)
-5
40 20 0
-50
0 x in m
50
-10
-50
0 x in m
(a)
50
(d)
(b)
Fig. 3. Initial (black dashed line) and optimized (red line) trajectory (a,b), and cross sections of the point clouds created using the initial GPS/INS (c) and optimized trajectory (d). dense representation of the TSDF dynamically in a small predefined volume around the sensor in motion. The parts of the map that move outside this volume are converted to a triangular mesh and only the mesh is stored. Methods for storing the entire TSDF are voxel hashing (Nießner et al., 2013) or octree data structures (Steinbr¨ ucker et al., 2014). For this work we use a B-tree based data structure (Museth, 2013) to store the complete sparse TSDF grid. The tree has constant depth, which allows constant time local and random traversals. We use a three-level tree with branching factors decreasing closer to the leaves.
as depicted in Fig. 4. Fig. 4(a) shows the resulting point cloud using the GPS/INS trajectory while Fig. 4(b) shows the result using the optimized trajectory from continuoustime SLAM. The color encodes the depth. This dataset consists of 12786 multibeam scans captured at 10 Hz. It was captured in 22 min and the trajectory is 1567 m long (result of the SLAM solution). Especially at the bottom of the mine it is visible that the multibeam measurements are more consistent in the optimized results. This can be seen more clearly in the cross sections of the point clouds presented in Fig. 4(e) and Fig. 4(f).
To integrate the multibeam data in the TSDF we follow the generalized sensor fusion approach proposed by May et al. (2014). We model the multibeam as a polar line sensor with a beam width of 1.5 deg. Individual voxel cells within measurement range are then updated based on back projection using this sensor model. From the scalar TSDF grid we finally extract a surface mesh for visualization using the marching cubes algorithm.
Consequently, the extracted mesh from the SDF representation using the optimized continuous-time SLAM solution, depicted in Fig. 4(d), exhibits smoother surfaces than the initial result depicted in Fig. 4(c). Despite the noise of the measurements a smooth surface can be extracted if a sufficient amount of repeated observations are available.
4.4 Results To demonstrate the continuous-time SLAM algorithm Fig. 3 shows results on a trajectory with significant drift of multiple meters. In this specific case the GPS signal was lost temporarily during data acquisition, which explains the large trajectory errors. The dataset consists of 7291 multibeam scans captured at 10 Hz. It was captured in 13 min and the trajectory is 757 m long (result of the SLAM solution). Fig. 3 (a,b) depict the initial GPS/INS trajectory as a black dashed line and the optimized trajectory as a red continuous line. The x/y-plane is aligned parallel to the water surface. We can see that SLAM converges to a solution that puts the sensor poses closer to a planar motion as expected for a surface vehicle. Please note that the algorithm does not impose any movement constraints or rely on a vehicle motion model. Cross sections of the resulting point cloud are displayed in Fig.3 (c,d). We can clearly see misalignment between multiple passes of the multibeam sonar in the initial result shown in Fig.3 (c). Point measurements line up well using the improved trajectory estimate based on continuous-time SLAM visualized in Fig.3 (d). Moreover, also data captured with a good GPS/INS result can be further improved using the proposed techniques
The borders of the mine show holes in the mesh. This is a result of the irregular and low point density of the sonar measurements due to limited coverage close to the borders of the mine. Since this is undesirable, we later interpolate the holes for display in the virtual reality system. 5. CONCLUSION In this paper we showed first results on creating a detailed terrain model of a submerged inland mine from a multibeam sonar survey. We demonstrate that SLAM techniques are effective to remove inconsistencies due to inaccuracies of the motion data. Moreover, we presented a signed distance function based approach for data fusion. Rendering a surface mesh for visualization compared to a point cloud is advantageous because it allows the human operator to see the surfaces and structure more clearly. We integrate the terrain model into a virtual reality scene together with prototype models of the envisioned mining system, which is used for pre-planning, visualization and development. ACKNOWLEDGEMENTS This work was supported by the European Union’s Horizon 2020 research and innovation programme under grant agreement No 642477.
1166
Depth in m
Proceedings of the 20th IFAC World Congress 1144 Michael Bleier† et al. / IFAC PapersOnLine 50-1 (2017) 1139–1144 Toulouse, France, July 9-14, 2017
(c)
(e)
(b)
(d)
(f)
Depth in m
(a)
Fig. 4. Initial (a) and optimized (b) 3D point cloud, surface mesh extracted from SDF model using initial GPS/INS (c) and continuous-time SLAM solution (d), and a cross section of initial (e) and optimized (f) point cloud. REFERENCES Barkby, S., Williams, S.B., Pizarro, O., and Jakuba, M.V. (2012). Bathymetric particle filter SLAM using trajectory maps. The International Journal of Robotics Research, 31(12), 1409–1430. Besl, P.J. and McKay, N.D. (1992). Method for registration of 3-D shapes. In IEEE Trans. Pattern Analysis and Machine Intelligenc, volume 14, 239–256. Bichucher, V., Walls, J.M., Ozog, P., Skinner, K.A., and Eustice, R.M. (2015). Bathymetric factor graph SLAM with sparse point cloud alignment. In Proc. OCEANS 2015 - MTS/IEEE Washington, 1–7. Borrmann, D., Elseberg, J., Lingemann, K., N¨ uchter, A., and Hertzberg, J. (2008). Globally consistent 3D mapping with scan matching. Robotics and Autonomous Systems, 56(2), 130–142. Bosse, M. and Zlot, R. (2009). Continuous 3D scanmatching with a spinning 2D laser. In Proc. IEEE Int. Conf. Robotics and Automation, 4312–4319. Campos, R., Garcia, R., Alliez, P., and Yvinec, M. (2014). A surface reconstruction method for in-detail underwater 3D optical mapping. The International Journal of Robotics Research, 64 – 89. Elseberg, J., Borrmann, D., and N¨ uchter, A. (2013). Algorithmic solutions for computing precise maximum likelihood 3D point clouds from mobile laser scanning platforms. Remote Sensing, 5(11), 5871–5906. Ferreira, H., Almeida, C., Martins, A., Almeida, J., Dias, N., Dias, A., and Silva, E. (2009). Autonomous bathymetry for risk assessment with ROAZ robotic surface vehicle. In Proc. OCEANS 2009 - Europe, 1–6. Hornung, A. and Kobbelt, L. (2006). Robust reconstruction of watertight 3D models from non-uniformly sampled point clouds without normal information. In Fourth Eurographics Symp. on Geometry Processing, 41–50. May, S., Koch, P., Koch, R., Merkl, C., Pfitzner, C., and N¨ uchter, A. (2014). A generalized 2D and 3D multisensor data integration approach based on signed distance functions for multi-modal robotic mapping. In 19th Int. Workshop on Vision, Modeling and Visualization, 95–102.
Museth, K. (2013). VDB: High-resolution sparse volumes with dynamic topology. ACM Transactions on Graphics, 32(3), 27. Newcombe, R.A., Izadi, S., Hilliges, O., Molyneaux, D., Kim, D., Davison, A.J., Kohi, P., Shotton, J., Hodges, S., and Fitzgibbon, A. (2011). KinectFusion: Real-time dense surface mapping and tracking. In Proc. 10th IEEE Int. Mixed and Augmented Reality Symp., 127–136. Nießner, M., Zollh¨ofer, M., Izadi, S., and Stamminger, M. (2013). Real-time 3D reconstruction at scale using voxel hashing. ACM Transactions on Graphics, 32(6), 169. NovAtel (2016). Inertial Explorer. http://www.novatel. com/products/software/inertial-explorer/. Ozog, P. and Eustice, R.M. (2013). Real-time SLAM with piecewise-planar surface models and sparse 3D point clouds. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 1042–1049. Palomer, A., Ridao, P., and Ribas, D. (2016). Multibeam 3D underwater slam with probabilistic registration. Sensors, 16(4), 560. Pathak, K., Birk, A., and Vaskevicius, N. (2010). Planebased registration of sonar data for underwater 3D mapping. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 4880–4885. Ribas, D., Ridao, P., Tard´os, J.D., and Neira, J. (2008). Underwater SLAM in man-made structured environments. Journal of Field Robotics, 25(11-12), 898–921. Roman, C. and Singh, H. (2005). Improved vehicle based multibeam bathymetry using sub-maps and SLAM. In Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 3662–3669. Sheehan, M., Harrison, A., and Newman, P. (2012). Selfcalibration for a 3D laser. The International Journal of Robotics Research, 31(5), 675–687. Steinbr¨ ucker, F., Sturm, J., and Cremers, D. (2014). Volumetric 3D mapping in real-time on a CPU. In Proc. IEEE Int. Conf. Robotics and Automation, 2021–2028. Whelan, T., Kaess, M., Fallon, M., Johannsson, H., Leonard, J., and McDonald, J. (2012). Kintinuous: Spatially extended KinectFusion. Technical report, Massachusetts Institute of Technology.
1167