3D Scene Reconstruction Using Omnidirectional Vision and LiDAR: A Hybrid Approach

Abstract

In this paper, we propose a novel approach to obtain accurate 3D reconstructions of large-scale environments by means of a mobile acquisition platform. The system incorporates a Velodyne LiDAR scanner, as well as a Point Grey Ladybug panoramic camera system. It was designed with genericity in mind, and hence, it does not make any assumption about the scene or about the sensor set-up. The main novelty of this work is that the proposed LiDAR mapping approach deals explicitly with the inhomogeneous density of point clouds produced by LiDAR scanners. To this end, we keep track of a global 3D map of the environment, which is continuously improved and refined by means of a surface reconstruction technique. Moreover, we perform surface analysis on consecutive generated point clouds in order to assure a perfect alignment with the global 3D map. In order to cope with drift, the system incorporates loop closure by determining the pose error and propagating it back in the pose graph. Our algorithm was exhaustively tested on data captured at a conference building, a university campus and an industrial site of a chemical company. Experiments demonstrate that it is capable of generating highly accurate 3D maps in very challenging environments. We can state that the average distance of corresponding point pairs between the ground truth and estimated point cloud approximates one centimeter for an area covering approximately 4000 m${}^2$. To prove the genericity of the system, it was tested on the well-known Kitti vision benchmark. The results show that our approach competes with state of the art methods without making any additional assumptions.

1. Introduction

Reconstructing the 3D scene using a mobile observer, also referred to as mobile mapping, is used in a variety of applications ranging from navigational tasks for autonomous vehicles to facility condition assessment or cartography. The former application imposes a real-time constraint on the processing time of the used algorithms, whereas the latter allows the system to take more computation time. The application that we have in mind aims at construction site monitoring. The goal there is to obtain a 3D model of the environment in a reasonable amount of time, which spans the duration of the intervention. To this end, we propose a system that combines both a LiDAR sensor (Velodyne HDL32-e) and a panoramic camera system (Ladybug 3) to perceive the environment. Some systems exist that rely on the output of a regular camera to obtain 3D reconstructions [1,2], known in the literature as (visual) structure from motion (SfM). In the case of sequential (or incremental) SfM, the problem is also referred to as simultaneous localization and mapping (SLAM). However, visual SfM produces rather sparse 3D reconstructions, which require much post-processing to make them more dense. Moreover, they are prone to drift and tend to fail when the images are overexposed because of the abundance of sunlight. For this reason, the core of our mobile mapping system is based on the output of a LiDAR sensor, aided by images of a regular camera to perform loop detection. Often times, mobile mapping systems are also relying on GPS to estimate the trajectory of the observer. However, in the environments we have in mind, i.e., construction sites with many overhanging or protruding structures (e.g., pipelines), the GPS signal is too weak. In closed indoor environments, it is even completely lacking. Therefore, we developed a system that is fully GPS-independent. In summary, we propose an entire 3D reconstruction system that covers both the odometry and mapping problem. It incorporates the alignment of consecutive point clouds, the fusion with a global 3D map and a loop closure mechanism to cope with drift. The main goal of our approach is to make the system as generic as possible and, hence, to make as little assumptions as possible about the environment and about the sensor set-up. Doing so, we are able to use our system in combination with a drone-mounted LiDAR sensor and camera in the future. The main contribution of this work is that our solution deals explicitly with the inhomogeneous density of point clouds produced by LiDAR scanners. This latter refers to the fact that some parts of the point cloud have a higher point density than others due to the inclination of the laser beams and the proximity of the objects present in the scene. Generally, regions close to the sensor origin will be densely sampled, whereas more distant objects will be sparsely sampled. As has been shown in numerous studies [3,4,5], traditional point-to-point registration techniques (e.g., Iterative Closest Point (ICP)) fail in these situations. For that reason, we propose a number of improvements over standard point-to-point strategies and incorporate them in one framework. The main contributions can be summarized as follows:

• We propose a surface analysis technique that is used to build a topological space on top of the point cloud, providing for each point its ideal neighborhood and taking into account the underlying surface.
• We keep track of a global 3D map that is continuously updated by means of a surface reconstruction technique that allows us to resample the point cloud. This way, the global 3D map is continuously improved, i.e., the noise is reduced, and the alignment of the following point clouds can be conducted more accurately.
• The topological space is used to compute low-level features, which will be incorporated in an adapted version of the ICP algorithm to make this latter more robust. The alignment of consecutive local point clouds, as well as the alignment of a local point cloud with the global 3D map will be conducted using this improved ICP algorithm.
• We incorporate the residual of the ICP process in the loop closure process. Based on this residual, we can predict the share of each pose estimation in the final pose error when a loop has been detected.

2. Related Work

Regarding LiDAR odometry and mapping, existing solutions can roughly be divided in four main classes: based on ICP, normal distribution transform (NDT), features and planar surfaces. The majority of the approaches presented in the literature are based on the iterative closest point (ICP) algorithm in order to perform scan matching and to identify the transformation between sensor poses [6,7,8]. However, the standard ICP algorithm has many limitations, and therefore, many improvements have been presented in literature. One of the main drawbacks lies in the fact that it is often difficult to find good point to point correspondences between two point clouds, especially in the case of a sparse point density or when the speed of the moving platform is too high. The authors of velocity ICP [9] therefore propose to perform velocity estimation over the ICP iterations. Doing so, the distortion of a scan due to motion can be compensated by re-projecting all points acquired during one sweep to the position of the sensor at the beginning of the sweep. In [10], the authors tackle the correspondence problem by defining low-level features that are used to guide the ICP process. The authors focus on pair-wise registration of datasets that do not exhibit large changes. They do not investigate a full sequential 3D reconstruction approach that incorporates a global 3D map or loop closure. The authors of Velodyne SLAM [11], on the other hand, incrementally build a map of the environment by aligning each newly arrived Velodyne scan with the whole map using ICP. Their global map is improved over time by adapting incoming measurements according to already existing adapted neighbors. However, the refinement of the global map is limited as they do not perform a surface analysis or re-sampling of the point cloud. Moreover, they do not incorporate a loop closure mechanism to cope with drift. Finally, some studies focus on combining the ICP-based scan matching with (stereo) visual odometry. For example, in [12], the authors propose a solution that uses the output of visual odometry to obtain a rough estimate of the ego motion upon which consecutive point clouds are registered. The generalized ICP algorithm [13] is then used to refine the motion estimation, and finally, the output is combined with reduced IMU outputs. Although these latter solutions seem effective, these studies, in contrast to ours, do not investigate how LiDAR odometry itself can be improved.

Aside from ICP, the normal distribution transform (NDT) [14] is another common technique that is often used for laser scan matching [15,16,17]. It is based on an alternative representation of a point cloud similar to an occupancy grid. In each 3D cell, a normal distribution is stored, and both scans are matched using Newton’s method. Compared to ICP-based methods, the main advantage of this method is that there is no need for correspondence estimation. The main limitation is however that the accuracy of NDT is strongly related to the cell size, which is difficult to define in the case of inhomogeneous point clouds.

Another way to cope with correspondence estimation is to introduce the use of features. Feature-based methods have significant advantages as they concentrate on strong cues, such as corners and lines and filter out irregular points, such as vegetation or tree leaves. One very interesting feature-based system has been developed by Zhang et al. and described in [18,19]. Their technique is based on the extraction of only two different shape features, representing sharp edges and planar surface patches. Edge points of the source cloud are associated with edge lines, while planar patches are matched with other planar patches in the target cloud. In [19], the same authors elaborated on their method by incorporating data from a regular camera to perform visual odometry.

Finally, in [20,21,22], the authors use yet another approach based on the registration of planar surfaces. These planes can lead to very accurate alignments as they serve as strong cues and can be estimated by an accumulation of data, hence reducing the noise. However, although these methods are very effective, the lack of planes in the scene can cause them to fail and, hence, make them less generic.

In this work, we will adopt a hybrid approach in the sense that we combine ICP-based registration with feature extraction and surface reconstruction. To this end, we adopt the strategy of [10] by incorporating low-level features that describe the underlying surface in a simple way, i.e., using three main classes, linear, planar and volumetric. We exploit these properties to identify for each point an ideal neighborhood and, hence, a topological space representing the captured scene. The low-level features will further be used to guide the ICP process in such a way that the alignment concentrates itself near the most reliable regions, i.e., lines and corners instead of irregular points or clutter. Our approach can benefit from large planar surfaces while at the same time remaining generic. It is thus more widely applicable. In addition, we keep track of a global map, which will continuously be improved and refined with newly-captured points. More specifically, we use a surface reconstruction technique after which we resample the point cloud and improve its topological space. Finally, we incorporate loop closure to cope with the remaining drift.

3. System

3.1. Acquisition Platform

Our acquisition platform consists of a Velodyne High Definition LiDAR (HDL-32e) scanner combined with a Ladybug panoramic camera system. Because of this combination, we named it the Vellady platform. As can be seen in Figure 1, the Ladybug camera is mounted perpendicular to the ground plane, whereas the Velodyne is tilted on its head, making an angle of approximately 66${}^{\circ }$ with the ground plane. The Velodyne LiDAR scanner is equipped with 32 lasers mounted collinear and covering a vertical FOV of 41.3${}^{\circ }$, hence resulting in a vertical resolution of 1.29${}^{\circ }$. The vertical FOV covers 30.67${}^{\circ }$ below the middle point of the Velodyne and 10.67${}^{\circ }$ above it. The head is continuously spinning at approximately 10 Hz, resulting in a horizontal FOV of 360${}^{\circ }$. Figure 2 shows an example of a point cloud obtained by this Velodyne sensor. The Ladybug on the other hand is a fixed system that comes in the form of a pentagonal prism consisting of five vertical-oriented sides, each one incorporating a camera. A sixth camera is mounted on top pointing upwards. It is shooting images at one frame per second. A stitched image obtained by this Ladybug camera system, corresponding with the point cloud of Figure 2, is depicted in Figure 3. We mounted this Vellady platform on a moving vehicle, i.e., a kitchen cart or four-wheel trolley (see Figure 1). Our platform is hence not able to rotate around its roll angle. A rotation about its pitch angle is only possible in the case that the ground plane has a slope, but these situations were not encountered during the experiments. However, we did not exploit these facts as we want to retain the option to mount the platform on a drone in the future. Our system thus operates as a full six degrees of freedom SLAM algorithm incorporating three unknown position coordinates x, y, z and three rotation angles ${\theta }_x$, ${\theta }_y$, ${\theta }_z$. The main features of our platform are summarized in

Table 1. A summary containing the main features of the acquisition platform.

	Sensor	Feature
	Ladybug system	- 6 camera’s in pentagonal prism, one pointing upwards
		- resolution of 1600 × 1200 per image
		- 1 frame (6 images) per second
		- mounted perpendicular w.r.t. the ground plane
	Velodyne HDL-32e	- 360${}^{\circ }$ horizontal FOV, 41.3${}^{\circ }$ vertical FOV
		- 32 lasers spinning at 10 sweeps per second
		- ±700,000 points per sweep
		- mounted with an angle of 66${}^{\circ }$ w.r.t. the ground plane

.

3.2. Terminology

As mentioned in the previous section, the Velodyne is spinning its head, thereby producing 360${}^{\circ }$ point clouds. A full rotation of the head is also referred to as a sweep. Throughout this paper, we use right subscript k, $k\in {\mathbb{Z}}^{+}$ to indicate the sweep number and ${\mathcal{P}}_k$ to indicate the point cloud perceived during sweep k. This point cloud is expressed using the local coordinate system of the Velodyne ${\mathrm{V}}_k$ at the time that sweep k has started. We will use right superscript i to denote a single point ${\mathbf{p}}_k^i$. Finally, we define ${\mathcal{I}}_j$ to denote the j-th set of images. Recall that the Ladybug only outputs six images simultaneously each second, and hence, some point clouds will have no accompanying visual information. However, both sensors were synchronized, and as a result, the following condition is always warranted: $\forall j\in {\mathbb{Z}}^{+},\exists k\in {\mathbb{Z}}^{+}:t\left({\mathcal{I}}_j\right)=t\left({\mathcal{P}}_k\right)$. In this expression, t is a function that returns the timestamp at which respectively the j-th set of images was captured and sweep k was started. As the Velodyne is spinning at a frequency of 10 Hz, there will only be visual information available for every tenth point cloud ${\mathcal{P}}_k$.

3.3. Reference Coordinate System

As both sensor outputs ${\mathcal{P}}_k$ and ${\mathcal{I}}_k$ are expressed in their own coordinate system, we need to define a reference coordinate system to harmonize both. To ease this task, the platform was designed in a way that the origin of both sensors is collinear. Since the Ladybug was put perpendicular to the ground plane, it is sufficient to determine the orientation of the Velodyne sensor with respect to the ground plane and nullify the offset of the origins of both sensors. As the Velodyne scanner incorporates three gyroscopes and three associated two-axis accelerometers, the transformation can be derived from one of its accelerometers. Specifically, given the accelerometer output $\mathbf{a}=(a_y,a_z)$ (cf. Figure 4), the angle about the x-axis, i.e., the pitch angle, can be computed as $\alpha =atan(a_z/a_y)$. The final transformation $T^{\left(vel2lady\right)}\in {\mathbb{R}}^{4\times 4}$ from the Velodyne coordinate system $\mathrm{V}$ to the Ladybug coordinate system $\mathrm{L}$ is then given by Equation (1):

$${\mathrm{T}}^{\left(vel2lady\right)}=\left[\begin{array}{cc}\mathrm{R}& \mathbf{t}\\ {\mathbf{0}}_3^{\top }& 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& t_x\\ 0& cos\alpha & sin\alpha & t_y\\ 0& -sin\alpha & cos\alpha & t_z\\ 0& 0& 0& 1\end{array}\right].$$

(1)

Herein, the values of $\mathbf{t}=(t_x,t_y,t_z)$ were determined by subtracting the two vectors that project both origins to the ground plane. In the case of the Velodyne coordinate system $\mathrm{V}$, the projection of the origin is performed after applying $\mathrm{R}$. Next, the coordinate system $\mathrm{W}$ denotes the global coordinate system in which the final 3D point cloud is expressed. We decided to set the coordinate system of the Ladybug at the start of the first sweep ${\mathrm{L}}_0$ as the global world coordinate system $\mathrm{W}$.

3.4. Problem Statement

The problem can be formulated as finding the trajectory, i.e., the sequence of sensor poses $\mathbf{S}=\{{\mathrm{S}}_1,\dots ,{\mathrm{S}}_k,\dots ,{\mathrm{S}}_N\}$, that transforms each point cloud ${\mathcal{P}}_k$ in the world coordinate system $\mathrm{W}$. The poses ${\mathrm{S}}_{\mathrm{k}}$ are defined as in Equation (2):

$$\begin{array}{cc}\hfill {\mathrm{S}}_{\mathrm{k}}& =\left[\begin{array}{cc}{\mathrm{R}}_k& {\mathbf{t}}_k\\ {\mathbf{0}}_3^{\top }& 1\end{array}\right]={\mathrm{T}}_{\mathrm{k},\mathrm{k}-1}{\mathrm{S}}_{k-1},\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill {\mathrm{S}}_0& =\left[\begin{array}{cc}{\mathrm{R}}_0& {\mathbf{t}}_0\\ {\mathbf{0}}_3^{\top }& 1\end{array}\right]={\mathrm{T}}^{\left(\mathrm{vel}2\mathrm{lady}\right)}.\hfill \end{array}$$

(3)

Herein, ${\mathrm{T}}_{\mathrm{k},\mathrm{k}-1}$ denotes the transformation of the point clouds ${\mathcal{P}}_k$ and ${\mathcal{P}}_{k-1}$ acquired in two consecutive sweeps. The reconstructed 3D point cloud ${\mathcal{W}}_k$ after k sweeps have been processed is given by ${\mathcal{W}}_k=\{{\mathcal{P}}_0,\dots ,{\mathrm{S}}_k{\mathcal{P}}_k\}$. The reconstructed point cloud of the entire sequence containing N sweeps is finally given by $\mathcal{W}=\{{\mathcal{P}}_0,\dots ,{\mathrm{S}}_k{\mathcal{P}}_k,\dots ,{\mathrm{S}}_N{\mathcal{P}}_N\}$. Figure 5 depicts a schematic drawing of this problem statement.

4. Approach

A schematic overview of our approach is depicted in Figure 6. As can be seen, it is implemented as a sequential, i.e., incremental, process in which every newly-arrived point cloud ${\mathcal{P}}_k$ is first processed and subsequently added to the current world model ${\mathcal{W}}_k$. The process repeated for every point cloud is conducted in five steps. First, we project the generated point clouds on a 2D grid (Step 1). Subsequently, we conduct a surface analysis (Step 2), after which we perform pairwise alignment of two consecutive point clouds (Step 3), which serves as an initial guess for the current pose. Next, we register the aligned point cloud with a global 3D map (Step 4) and fuse the new points with this 3D map (Step 5). This fusion consists of re-sampling the point cloud by means of a surface reconstruction technique. Optionally, when a loop has been detected, we adopt loop closure to preserve global consistency (Step 6). This is done by means of pose graph optimization, which propagates the estimated error back in the pose graph. In the following sections, we will further clarify each of these steps.

4.1. 2D Projection

The 3D points captured by the Velodyne are organized since the 32 lasers are placed collinear in the vertical direction. Because of that, we can project the 3D points onto a two-dimensional spherical grid; cf. Figure 7. Figure 8 shows an example of a 360${}^{\circ }$ range image corresponding to the point cloud of Figure 2. Using this 2D projection, we can exploit the adjacency in the pixel domain, e.g., to quickly find neighboring points in 3D. We will exploit this knowledge to perform a surface analysis as explained in Section 4.2.

4.2. Surface Analysis

The surface analysis step is performed twice each iteration, once on the point cloud ${\mathcal{P}}_k$ perceived during sweep k and once on the point cloud map ${\mathcal{W}}_{k-1}$ after the fusion of the previous point cloud ${\mathcal{P}}_{k-1}$. Our proposed algorithm is an extension of the one presented in [10]. It aims to find for each 3D point the optimal neighborhood size or the most suitable local point set that describes the underlying geometry. This is an interdependence problem, as geometrical features largely depend on the choice of the neighborhood, whereas a good neighborhood definition should rely on the local geometry and, thus, on geometrical features. Let us first define a neighborhood function n as $n^r\left(\mathbf{p}\right):(x,y,z)\mapsto {(x,y,z)}^N$. In this equation, r denotes the search radius for neighboring points, and N denotes the number of points belonging to the neighborhood. Using this neighborhood, we determine the principal components of the covariance matrix of the points belonging to it. These principal components are used to decide whether the underlying surface is linear (1D), planar (2D) or volumetric (3D), cfr. Figure 9. Hence, we first compute the three eigenvalues ${\lambda }_1,{\lambda }_2$ and ${\lambda }_3$ and their corresponding eigenvectors ${\mathbf{v}}_1,{\mathbf{v}}_2$ and ${\mathbf{v}}_3$. Next, we define the standard deviation along an eigenvector as ${\sigma }_i=\sqrt{{\lambda }_i}$ for $i\in 1,2,3$. Using these three values, we can obtain a measure on how linear, planar or scattered the underlying surface is. To this end, we define the three dimensionality values as follows:

$$\begin{array}{c}\hfill {\psi }_1=\frac{{\sigma }_1-{\sigma }_2}{{\sigma }_1},{\psi }_2=\frac{{\sigma }_2-{\sigma }_3}{{\sigma }_1},{\psi }_3=\frac{{\sigma }_3}{{\sigma }_1}.\end{array}$$

(4)

These three values are normalized such that ${\psi }_1+{\psi }_2+{\psi }_3=1$; hence, they represent a partition of unity Ψ. Finally, the dimensionality label l of each point is given by Equation (5):

$$\begin{array}{c}\hfill l=\underset{i\in [1,3]}{argmax}\left({\psi }_i\right).\end{array}$$

(5)

When ${\sigma }_1\gg {\sigma }_2,{\sigma }_3$, then ${\psi }_1$ will be larger than the two other features. This corresponds to lines between planar surfaces or thin structures, such as pipelines. On the other hand, when ${\sigma }_1,{\sigma }_2\gg {\sigma }_3$, then ${\psi }_2$ will be larger, corresponding to planar surfaces. Finally, when ${\sigma }_1\approx {\sigma }_2\approx {\sigma }_3$, then ${\psi }_3$ will be larger often times representing a scatter, such as bushes or tree leaves. To determine the optimal radius $r^{*}$, we use the concept of Shannon entropy. To this end, we compute the geometrical features for a growing radius size $r\in [r_{min},r_{max}]$. The Shannon entropy for the partition ${\mathrm{\Psi }}^r={\{{\psi }_1,{\psi }_2,{\psi }_3\}}_r$ is given by $E\left({\mathrm{\Psi }}^r\right)=-{\psi }_{1}ln\left({\psi }_1\right)-{\psi }_{2}ln\left({\psi }_2\right)-{\psi }_{3}ln\left({\psi }_3\right)$. This value gives a measure for the uncertainty about the dimensionality label. The optimal neighborhood radius $r^{*}$ is then defined as the minimum of the entropy function E:

$$\begin{array}{c}\hfill r^{*}=\underset{r\in [r_{min},r_{max}]}{argmin}E\left({\mathrm{\Psi }}^r\right).\end{array}$$

(6)

This radius $r^{*}$ leads to an initial guess of the optimal neighborhood ${\mathcal{V}}_{\mathbf{p}}^{*}$ of a point $\mathbf{p}$. As points within the optimal radius $r^{*}$ can still belong to different surfaces and can have different dimensionality labels, we define a similarity measure S denoting the homogeneity within the neighborhood of each point:

$$\begin{array}{c}\hfill S\left(n^r\left(\mathbf{p}\right)\right)=\frac{1}{N}\sum _{{\mathbf{p}}_i\in n^r\left(\mathbf{p}\right)}{\mathbf{1}}_{l\left(\mathbf{p}\right)=l\left({\mathbf{p}}_i\right)}.\end{array}$$

(7)

In this equation, $\mathbf{1}$ represents the indicator function, and N is the cardinality of $n^r\left(\mathbf{p}\right)$. If this value S is smaller than $0.5$, we reconsider the optimal radius $r^{*}$ using Equation (8):

$$\begin{array}{c}\hfill r^{*}=\underset{r\in [r_{min},r_{max}]}{argmax}S\left(n^r\left(\mathbf{p}\right)\right).\end{array}$$

(8)

In the other case, we remove the points belonging to the neighborhood of $\mathbf{p}$ that have another dimensionality label. Hence, we define the optimal neighborhood ${\mathcal{V}}_{\mathbf{p}}^{*}$ of a point $\mathbf{p}$ as in Equation (9):

$${\mathcal{V}}_{\mathbf{p}}^{*}=\{{\mathbf{p}}_i:{\mathbf{p}}_i\in n^{r^{*}}\left(\mathbf{p}\right)\wedge l\left(\mathbf{p}\right)=l\left({\mathbf{p}}_i\right)\}.$$

(9)

In summary, the proposed method tries to find the ideal topological space ${\mathcal{T}}^{*}$ representing the underlying surface of the point cloud. For each node (or point) in ${\mathcal{T}}^{*}$, we store a 19-dimensional feature vector, which is the combination of the eigen values $\lambda =\{{\lambda }_1,{\lambda }_2,{\lambda }_3\}$, the eigen vectors $V=\{{\mathbf{v}}_1$, ${\mathbf{v}}_2,{\mathbf{v}}_3\}$, the dimensionality values $\mathrm{\Psi }=\{{\psi }_1,{\psi }_2,{\psi }_3\}$, the dimensionality label l and the values $r^{*}$ and $E^{*}$. The last feature we define represents the omnivariance and is given by $O={\prod }_{i\in [1,3]}{\sigma }_i$. One of the most important features in this vector is the local surface normal ${\mathbf{n}}_i$ of a point ${\mathbf{p}}_i$, which can be approximated by the eigenvector ${\mathbf{v}}_3$ corresponding to the smallest eigenvalue ${\lambda }_3$ of the principal components, which was computed using the optimal neighborhood.

4.3. Local Registration

The majority of the solutions presented in the literature to find the alignment of consecutive scans or point clouds are based on the iterative closest point (ICP) algorithm. As the name suggests, it is an iterative method that consists of four main steps in each iteration: (1) point selection; (2) correspondence estimation; (3) weighting; and (4) transformation estimation. In every iteration, the transformation is updated until convergence has been reached. In Figure 10, a graphical representation of the algorithm is depicted. However, the standard ICP approach has some severe limitations. One of the main issues originates from the fact that the point clouds generated by the Velodyne scanner have a sparse and inhomogeneous density. As a result, it is hard to find good point correspondences between two consecutive point clouds (referred to as source and target point cloud), as most of the time, the determined corresponding pairs will not represent the same physical point in space. This leads to point clouds that are not aligned accurately, even after a large number of iterations and, hence, to wrongly estimated sensor poses. In addition, the convergence process will be very slow. Motivated by this shortcoming, we propose to incorporate the low-level features that we derived in the surface analysis step to guide the ICP process. Doing so, we will not use the closest points in 3D as target points directly as is done in traditional ICP. Instead, we present an approach in which we use the local surface normals that were computed using the optimal neighborhood (cf. Section 4.1). We then minimize the distance from each source point to the tangent plane of its corresponding surface patch in the target point cloud. In the following, we will briefly discuss the four main sub-parts of the process.

(1) Point Selection

The goal of the point selection step is to focus on the most reliable areas for accurate registration. An area is considered as unreliable if it is located near the border between several objects or surfaces or if it belongs to a geometrically complex object. As the points corresponding with a volumetric label ($l=3$) are mainly corresponding to scattered points, e.g., bushes or tree leaves, we will exclude them from the estimation process, as we consider them less reliable. This is mainly due to the fact that it is hard to estimate accurate surface normals for these points, as they represent complex structures and are too sparsely sampled. In addition, we will exclude planar points for which the uncertainty, i.e., the entropy, is too high. In the experiments, the threshold for the entropy was set to 0.8.

(2) Correspondence Estimation

In contrast with existing systems, we will select the corresponding target point ${\mathbf{p}}_{k-1}^j$ as the point having the most similar neighborhood as the source point ${\mathbf{p}}_k^i$ in terms of the geometry of the underlying surface. To this end, we compute the distance from the source point to the target point in feature space. However, as it is too infeasible to compare every point in the target point cloud with the source point, we first determine corresponding candidates by selecting the closest points in Euclidean space using the 2D projection of both point clouds. More specifically, we will look for the points that are only a few pixels away in the 2D domain. Finally, the target point that has the smallest distance in feature space is chosen as the corresponding point.

(3) Weighting

In order to make the procedure more robust against correspondence outliers computed in the previous section, we incorporate robust M-estimators in the iteration process. Thus, instead of using fixed weights, we adapt them through the iterations resulting in an iteratively reweighted least squares (IRLS) ICP optimization. Compared to traditional solutions, this weighting is carried out using the distance of the corresponding pairs in feature space instead of Euclidean 3D space. This is important as the point clouds are inhomogeneous, and the distance between correct correspondence pairs, i.e., representing the same physical point, will vary greatly based on the distance of the points to the origin of the sensor. We want to emphasize that the feature vector of a point itself is not changing during consecutive ICP iterations, but its corresponding point can be chosen differently, as the matching process also depends on their Euclidean distance (cf. Section 4.3). Thus, the distance in feature space can be varying during the iterations.

(4) Transformation Estimation

In traditional ICP approaches, the transformation between point clouds is estimated by minimizing the sum of the Euclidean distances between corresponding points, known as the point-to-point distance. In our approach, we will not match points from the source point cloud ${\mathcal{P}}_k$ with points in the target point cloud ${\mathcal{P}}_{k-1}$, but rather match points of ${\mathcal{P}}_k$ with surface patches of ${\mathcal{P}}_{k-1}$. The goal is to minimize the distance between the points in ${\mathcal{P}}_k$ with the tangent plane of the corresponding surface patch in ${\mathcal{P}}_{k-1}$, known as the point-to-plane distance and given by the following error metric:

$$E({\mathcal{P}}_k,{\mathcal{P}}_{k-1};{\mathrm{T}}_{\mathrm{k},\mathrm{k}-1})=\sum _{i=1}^N{\mathbf{w}}^i{(({\mathrm{T}}_{\mathrm{k},\mathrm{k}-1}{\mathbf{p}}_k^i-{\mathbf{p}}_{k-1}^{\mathbf{c}\left(i\right)})·{\mathbf{n}}_{k-1}^{\mathbf{c}\left(i\right)})}^2.$$

(10)

Herein, ${\mathrm{T}}_{\mathrm{k},\mathrm{k}-1}$ is the estimated transformation matrix; ${\mathcal{P}}_k$ is the source point cloud; ${\mathcal{P}}_{k-1}$ is the target point cloud; ${\mathbf{n}}_{k-1}^i$ is the surface normal according to target point; ${\mathbf{p}}_{k-1}^i$, ${\mathbf{w}}^i$ is the weight vector; and $\mathbf{c}$ is the vector containing the indices of the N corresponding points. Equation (11) gives the expression to derive the final transformation matrix ${\mathrm{T}}_{k,k-1}$:

$${\mathrm{T}}_{\mathrm{k},\mathrm{k}-1}=\left[\begin{array}{cc}{\mathrm{R}}_{k,k-1}& {\mathbf{t}}_{k,k-1}\\ {\mathbf{0}}_3^{\top }& 1\end{array}\right]=\underset{{\mathrm{T}}_{k,k-1}}{argmin}E({\mathcal{P}}_k,{\mathcal{P}}_{k-1};{\mathrm{T}}_{k,k-1}).$$

(11)

In order to solve this optimization problem, we adopt the method proposed by Low et al. in [23]. In that paper, a method is derived to approximate the nonlinear optimization problem with a linear least squares one in the case that the relative orientation between the two input point clouds is small. Since we consider point clouds acquired in two consecutive sweeps, this assumption is guaranteed in our case.

4.4. Global Registration

Once we have found the initial transformation ${\mathrm{T}}_{k,k-1}$ between point clouds ${\mathcal{P}}_k$ and ${\mathcal{P}}_{k-1}$, the next step consists of fusing ${\mathcal{P}}_k$ with the current registered point cloud ${\mathcal{W}}_{k-1}$. To this end, we first transform ${\mathcal{P}}_k$ in the world coordinate system using the estimate of the current pose, which is given by ${\widehat{\mathrm{S}}}_k={\mathrm{T}}_{k,k-1}{\mathrm{S}}_{k-1}$ resulting in ${\widehat{\mathcal{P}}}_k={\widehat{\mathrm{S}}}_k{\mathcal{P}}_k$. As the point cloud ${\widehat{\mathcal{P}}}_k$ is still not accurately aligned with ${\mathcal{W}}_{k-1}$, we perform a second registration step based on ICP as explained in the previous section. However, regarding the transformation estimation, the cost function that we want to minimize is now given by Equation (12):

$${\mathrm{T}}_{\mathrm{k},\mathrm{w}}=\left[\begin{array}{cc}{\mathrm{R}}_{k,w}& {\mathbf{t}}_{k,w}\\ {\mathbf{0}}_3^{\top }& 1\end{array}\right]=\underset{{\mathrm{T}}_{k,w}}{argmin}E({\widehat{\mathcal{P}}}_k,{\mathcal{W}}_{k-1};{\mathrm{T}}_{k,w}).$$

(12)

To find the correspondence points of ${\widehat{\mathcal{P}}}_k$ in ${\mathcal{W}}_{k-1}$, we can still use the criterion explained in Section 4.3. However, as the world map is growing, it is practically infeasible to look in the entire map for correspondences. Fortunately, this is not necessary as corresponding points in the map will be located close to the newly-added point cloud ${\widehat{\mathcal{P}}}_k$, as this latter is already transformed with an estimate of the current pose ${\widehat{\mathrm{S}}}_k$. Therefore, we can first filter the map by means of frustum culling using the oriented bounding box of the current sweep as a box filter extended with a small offset. The principal axes of this oriented bounding box are already known as these are previously determined by the accelerometer output. This box filtering will lower the processing speed without harming the guarantee to find good point correspondences. As we cannot use the organized structure of the separated point clouds any more after they have been fused with the point cloud map, we first create a kd-tree of the points remaining after the box filtering. After the registration of ${\widehat{\mathcal{P}}}_k$ with ${\mathcal{W}}_{k-1}$, we define ${\mathrm{S}}_k={\mathrm{T}}_{\mathrm{k},\mathrm{w}}{\widehat{\mathrm{S}}}_k$.

4.5. Map Fusion

After adding ${\mathcal{P}}_k$ to the former world model ${\mathcal{W}}_{k-1}$, the point density in this world model will increase. We can now refine the ideal topological space ${\mathcal{T}}^{*}$ and hence the local surface normals in the global 3D map by re-estimating them using their new neighborhood. However, as this point cloud contains noise and has an inhomogeneous point density, we will resample it using the moving least squares (MLS) surface reconstruction algorithm [24]. In summary, this method will try to locally approximate the underlying surface by higher order polynomial interpolations between surrounding data points. Using these polynomials one can resample the point cloud and obtain more accurate estimates for the surface normals. The procedure can be described as follows. Using the ideal topological space ${\mathcal{T}}^{*}$, we have for each point $\mathbf{p}$ an optimal neighborhood ${\mathcal{V}}_{\mathbf{p}}^{*}$, as well as the tangent plane to $\mathbf{p}$ defined as $H_{\mathbf{p}}\triangleq [\mathbf{n},d]$. For all points lying within this neighborhood, we can compute the distance to this tangent plane. Subsequently, we fit a polynomial in the set of distances from these points to the surface. To this end, we define a local approximation of degree m by a polynomial $\tilde{p}\in {\mathrm{\Pi }}_m$ minimizing, among all $p\in {\mathrm{\Pi }}_m$, the weighted least-squares error of Equation (13):

$$\sum _{i\in \mathit{I}}{(p\left({\mathbf{x}}_i\right)-f_i)}^2\theta \left(\right||{\mathbf{p}}_i-\mathbf{p}\left|\right|).$$

(13)

In this equation, $\mathit{I}$ is the vector of indices representing the points in ${\mathcal{V}}_{\mathbf{p}}^{*}$; ${\left\{{\mathbf{x}}_i\right\}}_{i\in \mathit{I}}$ are the orthogonal projections of the points ${\left\{{\mathbf{p}}_i\right\}}_{i\in \mathit{I}}$ onto $H_{\mathbf{p}}$; and $f_i\triangleq \langle {\mathbf{p}}_i,\mathbf{n}\rangle -d$ is the distance of ${\mathbf{p}}_i$ to the tangent plane $H_{\mathbf{p}}$. Finally, $\theta \left(x\right)=e^{-{\left(\frac{x}{{\sigma }_r}\right)}^2}$ represents the weighting function that is based on the distances to the tangent plane and the average separation ${\sigma }_r$ of the 3D points.

Once the parameters of the polynomials are known, we finally project the data points back on the moving least squares surface. This procedure will hence manipulate the sampled data points in such a way that they represent the underlying surface in a better way. In addition, we upsample the point cloud using voxel grid dilation in order to fill small gaps. This latter procedure will first dilate a voxel grid representation of the world model built using a predefined voxel size. After that, the resulting new points are projected to the MLS surface of the closest point in the world point cloud. With time, ${\mathcal{W}}_{k-1}$ is becoming larger and larger, and as a result, the search for nearest neighbors becomes quite intractable. For that reason, we store the map as an octree data structure. Using this octree, we can work on a downsampled version of the point cloud map ${\mathcal{W}}_{k-1}$. The main benefit of this octree representation is that we can keep all points, but at the same time, we can freely choose the level of density we want to use.

4.6. Loop Closure

Detecting loops using the acquired point clouds would be too cumbersome, as it would take ages to compare every newly-generated point cloud with the entire 3D map. For this reason, we perform loop detection on the images of the Ladybug. We adopt the same strategy presented in [25] (DBOW2) and improved by [1] (ORB-SLAM). More specifically, we adopt a bag of words (BOW) implemented as a hierarchical tree that uses a visual vocabulary. This vocabulary is built offline using ORB descriptors and converts an image into a sparse numerical vector. Each image is thus converted into a bag of word vector that is used to compare it with other images and to measure the similarity. This latter is conducted in the same way as described in [25]. In addition, an index is maintained that stores for each word in the vocabulary the list of images where it is present. Doing so, we are able to perform comparisons only against images that have some word in common with the query image.

Let us assume that a loop has been detected between image set ${\mathcal{I}}_j$ and ${\mathcal{I}}_i$. We now consider all point clouds ${\mathcal{P}}_l$ captured in the interval $[t\left({\mathcal{I}}_j\right),t\left({\mathcal{I}}_{j+1}\right)[$ (recall that there are ten sweeps in the interval) and try to find the most similar point cloud ${\mathcal{P}}_k$ captured in the interval $[t\left({\mathcal{I}}_i\right),t\left({\mathcal{I}}_{i+1}\right)[$. As the sensor could have visited the same location from different directions, it can happen that both ends of the loop have different orientations. Therefore, we first determine the matrix ${\mathrm{R}}_{k,l}$ that describes the rotation between ${\mathcal{P}}_l$ and ${\mathcal{P}}_k$. It is important to note that we compute the transformation on the local point clouds, which are expressed in their own coordinate system. The rotation estimation is a two-step process, consisting of an initial rough alignment and a refinement step using the ICP algorithm. The two point clouds ${\mathcal{P}}_l$ and ${\mathcal{P}}_k$ that have the lowest residual after the ICP step are considered the most similar point clouds and are denoted by ${\mathcal{P}}_e$ and ${\mathcal{P}}_s$, representing both ends of the loop. Their poses are respectively ${\mathrm{S}}_e$ and ${\mathrm{S}}_s$. Next, the difference in pose, i.e., the loop transform, is given by $\mathrm{\Delta }={\left({\mathrm{R}}_{s,e}{\mathrm{S}}_e\right)}^{-1}{\mathrm{S}}_s$. This loop transform is considered as an error as both poses ${\mathrm{R}}_{s,e}{\mathrm{S}}_e$ and ${\mathrm{S}}_s$ should be equal. The next step consists of propagating the error Δ back in the pose graph. To this end, we use the residual of the minimization step (cf. Section 4.3) to assign a weight $c_{i,j}$ to each link in the pose graph. We assign a higher weight for those transformations that had a high residual in the previous minimization step. The idea is that a high residual indicates that two consecutive point clouds were potentially inaccurately aligned. On the other hand, the ICP process will have already been converging in the right direction. Next, we define the distance between two poses ${\mathrm{S}}_k$ and ${\mathrm{S}}_l$ as $d({\mathrm{S}}_k,{\mathrm{S}}_l)={\sum }_{i,j}{c}_{i,j}$. Herein, $\{i,j\}$ denotes the set of all edges in the path from ${\mathrm{S}}_k$ to ${\mathrm{S}}_l$. Finally, we define a weight $w_i=\frac{d({\mathrm{S}}_s,{\mathrm{S}}_i)}{d({\mathrm{S}}_s,{\mathrm{S}}_e)}$ for each pose in the graph that specifies the fraction of the matrix Δ by which the pose has to be transformed. The poses ${\mathrm{S}}_k$ are than updated replacing ${\mathbf{t}}_k$ by ${\mathbf{t}}_k{w}_k\mathrm{\Delta }$ and ${\mathrm{R}}_k$ by $slerp({\mathrm{R}}_k,w_k\mathrm{\Delta })$. In this latter assignment, slerp denotes the spherical linear interpolation function as described in [26]. A graphical representation of this loop closure procedure is depicted in Figure 11.

5. Evaluation

To evaluate the system, we considered two different datasets. The first dataset we have captured ourselves. It covers different environments, including a university campus and an industrial site. The latter was captured at a chemical site of the Dow company in Terneuzen in the Netherlands. In order to be able to compare our system with the state of the art, we also performed some experiments on the well-known Kitti vision benchmark that was presented in [27].

5.1. Our Dataset

The first set of sequences was captured at a chemical site of the Dow company in Terneuzen. This environment is part of a disused area that is planned to be demolished. It consists of many pipelines that were formerly used to carry liquids or gases, as can be seen in Figure 3. Although this environment seems outdoors, the GPS signal is far too unreliable due to the abundance of pipelines. The acquired data consists of video sequences recorded with our Vellady platform. The speed of the platform approximated walking speed, i.e., 4 km/h. Furthermore, accurate ground truth information by means of terrestrial laser scanning was captured using a Leica system.(www.leica-geosystems.be) An image of this ground truth point cloud is depicted in Figure 12. As we do not have ground truth of the actual trajectory, we have to compare the reconstructed point cloud with the ground truth point cloud. For this comparison, it is necessary that both point clouds are aligned. Hence, we will first perform a rough alignment based on key points that we manually selected from both point clouds. Subsequently, we perform ICP to align them in a fine way. The final residual of the ICP process, i.e., the Euclidean distance between all closest point pairs, can be considered as a measure for the accuracy of the reconstruction. The experiments demonstrated that the final residual of this ICP process is approximately 2.1 cm for our reconstruction. However, as the closest point pairs do not necessarily represent the same physical points in space, the ICP residual is not a perfect measure for the evaluation of the accuracy. For that reason, we conducted another experiment in which we use the dominant planes in the scene to make the comparison. More specifically, in both point clouds, we estimate the parameters of the most dominant planes after which we determine the corresponding planes. Subsequently, we compute for all inliers of the planes from the source point cloud the distance to its corresponding plane in the target point cloud. We hence define the average distance of two corresponding planes $H_s\triangleq [{\mathbf{n}}_s,d_s]$ and $H_t\triangleq [{\mathbf{n}}_t,d_t]$ as $d(H_s,H_t)\triangleq \frac{1}{|H_t|}{\sum }_{{\mathbf{p}}_t\in H_t}\langle {\mathbf{p}}_t,{\mathbf{n}}_s\rangle -d_s$. In addition, we also define the difference in angle of the two corresponding planes as $\varphi (H_s,H_t)\triangleq \mathrm{acos}({\mathbf{n}}_s·{\mathbf{n}}_t)$. The results are summarized in

Table 2. The difference in angle between two corresponding planes $\varphi (H_s,H_t)\triangleq \mathrm{acos}({\mathbf{n}}_s·{\mathbf{n}}_t)$ (in degrees) and the average distance of two corresponding planes $d(H_s,H_t)\triangleq \frac{1}{|H_t|}{\sum }_{{\mathbf{p}}_t\in H_t}\langle {\mathbf{p}}_t,{\mathbf{n}}_s\rangle -d_s$ (in meter) for the eight most dominant planes in the scene.

Plane	$\mathit{d}({\mathit{\varphi }}_{\mathit{g}},{\mathit{\varphi }}_{\mathit{r}})$	${\mathit{d}}_{\mathit{\mu }}({\mathit{H}}_{\mathit{g}},{\mathit{H}}_{\mathit{r}})$
1	0.759761	0.00818437
2	0.276246	0.00724192
3	1.12969	0.008299
4	0.286674	0.0115731
5	0.292083	0.0164202
6	0.492578	0.0106246
7	1.80667	0.0118678
8	1.68458	0.00993411
	$\mathbf{0}.\mathbf{841036}$	$\mathbf{0}.\mathbf{0105181}$

for the eight most dominant planes in the scene. The table shows that on average, the deviation in angle of two corresponding planes is approximately $0.{84}^{\circ }$, whereas the average distance between two planes is approximately 1 cm. In Figure 13, an image is depicted in which both the ground truth and estimated point cloud are shown after alignment seen from a bird’s eye view. In Figure 14, the same models are depicted, but this time after they have been projected onto the ground plane to make it easier to evaluate the accuracy. Visually, there are little to no discrepancies noticeable between the two 3D point cloud models.

By means of comparison, the trajectory of the mobile observer is plotted in Figure 15. Herein, six plots are depicted representing the trajectories obtained by the visual structure from motion (SfM) approach using SIFT features, presented in [28] (in red) and the trajectory obtained by our proposed LiDAR mapping system (in blue). Each red graph is the result of the visual SfM method run on the output of one camera of the Ladybug system. As can be clearly seen, many poses are missing for this approach. This is due to the fact that sometimes too few good matches could be found between one image and the others. As a consequence, the resulting point cloud of the visual SfM is far more sparse than the one obtained by the LiDAR system. In addition, many outlier poses are present. The graphs show that the performance of visual SfM is less compared to odometry and mapping using LiDAR data, and hence, the former is less suited to obtain an accurate 3D model of the environment using a mobile observer. Finally, Figure 16 shows an image of the estimated trajectory, as well as the obtained 3D reconstruction using our proposed approach using the LiDAR data acquired by our Vellady platform.

By means of further evaluation, a second set of sequences was captured at a campus of Ghent University. Some example images of 3D reconstructions are shown in Figure 17. Unfortunately, there is no ground truth available for these environments. Visually, we can see that the reconstructions are adequate. The reconstructions of the indoor environment (images at the bottom) however have a mirror effect due to the reflectance of the windows.

5.2. Kitti Vision Benchmark

To further evaluate our system, we performed experiments on the Kitti vision benchmark presented by A. Geiger in [27], currently one of the main benchmarks related to (visual) odometry. For that benchmark, a van was driving in the streets of the German city Karlsruhe, thereby recording data from different modalities, among them a Velodyne HDL-64e LiDAR scanner and a stereo camera rig. To evaluate our odometry system, we ran our algorithm on the eleven test sequences that were recorded. Important to note is that the sensor set-up by which the sequences were captured differs from our set-up in the sense that the LiDAR scanner was placed perpendicularly. In order to evaluate our results quantitatively, we projected the ground truth and estimated poses on the ground plane as is suggested by the Kitti vision benchmark. The plots of these trajectories are depicted in Figure 18. Visually, the graphs show that our method is capable of approximating the ground truth in most cases. For the very long sequences, the system suffers from drift as is inherent to the SLAM problem. These results were also obtained without the loop detection and loop closure as described in Section 4.6. Next, we derived the translational error $e_t$ and rotational error $e_r$ from the trajectories. In order to evaluate both errors, the trajectories were quantized in intervals of length Δ. The sensor pose ${\mathrm{S}}_f$ corresponding with sweep f represents the first sensor pose of the interval, whereas ${\mathrm{S}}_l$ represents the last sensor pose of the interval. For ${\mathrm{S}}_f$ and ${\mathrm{S}}_l$, the following condition is valid: $\left|\right|{\mathbf{t}}_f-{\mathbf{t}}_l{\left|\right|}_2\approx \mathrm{\Delta }$. We now define the difference of the two ground truth poses ${\mathrm{S}}_f$ and ${\mathrm{S}}_l$ and estimated poses ${\widehat{\mathrm{S}}}_f$ and ${\widehat{\mathrm{S}}}_l$ as $S_{\mathrm{\Delta }}=S_f^{-1}{S}_l$ and ${\widehat{S}}_{\mathrm{\Delta }}={\widehat{S}}_f^{-1}{\widehat{S}}_l$, respectively. The final pose error ${\mathrm{E}}_{\mathrm{\Delta }}$ is then given by Equation (14):

$$\begin{array}{cc}\hfill {\mathrm{E}}_{\mathrm{\Delta }}& =\left[\begin{array}{cc}{\mathrm{R}}_{\mathrm{e}}& {\mathbf{t}}_e\\ {\mathbf{0}}_3^{\top }& 1\end{array}\right]={\widehat{S}}_{\mathrm{\Delta }}^{-1}{S}_{\mathrm{\Delta }}.\hfill \end{array}$$

(14)

Finally, the rotational error $e_r$ and translational error $e_t$ are given by Equations (16) and (17):

$$\begin{array}{cc}\hfill \mathrm{d}& =\frac{1}{2}(\mathrm{tr}\left({\mathrm{R}}_e\right)-1),\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill e_r& =\frac{1}{\mathrm{\Delta }}\mathrm{acos}\left(\max (\min (d,1),-1)\right),\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill e_t& =\frac{1}{\mathrm{\Delta }}\left|\right|{\mathbf{t}}_e{\left|\right|}_2.\hfill \end{array}$$

(17)

The graphs that summarize the translational and rotational errors for $\mathrm{\Delta }\in \{100,\dots ,800\}$ are depicted in Figure 19 and Figure 20. These graphs are the average numbers for the eleven different sequences of the Kitti dataset. As can be seen, the rotational error decreases from $0.{016}^{\circ }$ per meter to $0.{006}^{\circ }$ per meter when the path length Δ increases from 100 to 800 meter. The translational error on the other hand increases from $1\%$ to $1.75\%$ per meter for the path length increasing from 100 to 500. After that, it stabilizes more or less. Regarding the translational error, our algorithm competes with the methods ranked seventh to 20th in the Kitti vision benchmark. Figure 20 also shows the translational and rotational errors for an increasing speed. The rotational error varies only slightly when the speed increases. Regarding the translational error, we can see that it increases for higher speeds, which is expected. For speeds higher than 70 km/h, the error increases drastically. The reason for this failure is clear, as for higher speeds, consecutive point clouds begin to differ greatly from each other and no good correspondences can be found. However, we want to point up that our 3D mapping system is meant to operate in industrial plants or indoor environments where the speed of a mobile observer or a drone is always limited. Our system can perfectly cope with speeds up to 30 km/h.

Moreover, our method is more generic, as it does not pose any restriction on the sensor set-up nor any prior knowledge of the type of the scene. Furthermore, it does not rely on the detection of the ground plane, and as a result, it does not need to be present in the LiDAR image. Although the latter assumption could improve the accuracy of the system, we want it to be generic and applicable in any environment and in combination with any sensor set-up. Finally, some example images of 3D reconstructions using the Kitti benchmark are presented in Figure 21. These reconstructions are part of the validation sequences for which no ground truth is provided.

6. Conclusions

In this paper, a novel mobile mapping system using a multi-modal sensor set-up was presented. The platform is unique in the sense that it copes with the inhomogeneity of the point clouds produced by LiDAR scanners. To this end, it integrates both an intensive surface analysis, as well as a global map that is continuously updated and improved. Moreover, loop detection is conduction using the output of the Ladybug images, and a loop closure technique was proposed based on the output of the LiDAR odometry. Experiments demonstrated that our system is able to reconstruct challenging environments, such as chemical plants, and can compete with state of the art methods concerning LiDAR mapping in the field of autonomous vehicles; cf. the Kitti vision benchmark.