Development of Kinematic 3D Laser Scanning System for Indoor Mapping and As-Built BIM Using Constrained SLAM

Abstract

The growing interest and use of indoor mapping is driving a demand for improved data-acquisition facility, efficiency and productivity in the era of the Building Information Model (BIM). The conventional static laser scanning method suffers from some limitations on its operability in complex indoor environments, due to the presence of occlusions. Full scanning of indoor spaces without loss of information requires that surveyors change the scanner position many times, which incurs extra work for registration of each scanned point cloud. Alternatively, a kinematic 3D laser scanning system, proposed herein, uses line-feature-based Simultaneous Localization and Mapping (SLAM) technique for continuous mapping. Moreover, to reduce the uncertainty of line-feature extraction, we incorporated constrained adjustment based on an assumption made with respect to typical indoor environments: that the main structures are formed of parallel or orthogonal line features. The superiority of the proposed constrained adjustment is its reduction for uncertainties of the adjusted lines, leading to successful data association process. In the present study, kinematic scanning with and without constrained adjustment were comparatively evaluated in two test sites, and the results confirmed the effectiveness of the proposed system. The accuracy of the 3D mapping result was additionally evaluated by comparison with the reference points acquired by a total station: the Euclidean average distance error was 0.034 m for the seminar room and 0.043 m for the corridor, which satisfied the error tolerance for point cloud acquisition (0.051 m) according to the guidelines of the General Services Administration for BIM accuracy.

1. Introduction

A Building Information Model (BIM) is based on 3D models that organize and represent as-designed construction site information, whereas as-built information usually is derived from monitoring activities. Comparison of as-designed with as-built information facilitates quality control and enhances building management efficiency [1]. Recently introduced 3D laser scanners make possible rapid and accurate capturing of a huge number of point clouds, which produces very dense and elaborate coordinate data points for the surfaces of a physical object [2,3]. Integration of laser scanning with BIM can yield significant advantages over traditional approaches, specifically by facilitating fast and accurate data acquisition for existing conditions [4,5,6,7]. In the AEC (Architecture, Engineering, and Construction) domain, correspondingly, the 3D “as-built BIM” has become an essential means of accurately representing recently constructed buildings and their facilities to support maintenance process [8,9]. The major focus in this study is the development of efficient 3D data acquisition system for input of as-built BIM creation.

Conventional static laser scanners capture data from objects in their line of sight. Ensuring a complete map in the presence of occlusions necessitates scans from multiple positions, which result in a number of point cloud groups. The process of transforming multiple point clouds into a single point cloud is called registration. Registration of multiple point clouds requires that surveyors setup the laser scanner at a position with known coordinates or position artifacts (known as targets) in the overlap areas. Not surprisingly, using targets to merge multiple point clouds incurs additional cost and time in scanning-position surveying and manual post-processing. Moreover, it requires accurate instrument installation; any error at any given position renders the data collected there unusable [10,11,12]. In any case, indoor mapping applications involving very complex office environments with many occlusions certainly impose severe operational limitations on conventional static scanning systems.

Alternatively, we propose herein a kinematic 3D laser scanning system that continuously scans and registers point cloud data using feature-based Simultaneous Localization And Mapping (feature-based SLAM) technique. The feature-based SLAM has been employed for autonomously navigating mobile systems with 2D laser scanner that horizontally map the surrounding environment and use the acquired features for system-position correction. One way to acquire a 3D map is to use an additional scanner to scan the vertical profiles of the environment along the system’s trajectory. In this case, the accuracy of 3D data depends on that of the system’s position [13,14,15,16,17]. Unfortunately however, the feature-based SLAM suffers from data association errors due to incorrect extraction and matching of feature extractions [18].

In order to improve the performance of feature-based SLAM, constraint approaches are interesting solutions that modify the basic algorithm according to some environmental assumptions. This allows, for all cases that do not violate those assumptions, much improved performance [19,20]. The basic assumptions, specifically for indoor environments, are as follows: (1) the main structures (e.g., walls and doors) are formed of straight lines; (2) all such structures are parallel or perpendicular to each other. Zunino [21] used the orientation of the first-extracted line as a reference angle and corrected the other lines to fulfill the 90° geometric constraint. Nguyen et al. [22] suggested orthogonal SLAM, by which only lines that are parallel or perpendicular to each other are mapped. Using the lengths of line segments as weights, they defined the reference line segment for horizontal and vertical directions and rotated the other lines around their midpoints to align them with the reference orientation; finally, the system’s position and its surrounding map were updated according to the orientations and relative distances between the corrected lines. The main drawback of the above methods is that they are basically heuristic approaches: they do not statistically consider observation (point cloud) errors in correcting extracted lines with respect to geometric constraints. Choi et al. [23] aligned the first extracted line parallel with x-axis and compared its colinearity and geometric constraints with the other lines. If a line satisfied both conditions, it was merged with the first line; if it satisfied only the geometric constraints, it was added as a new feature. However, at the starting point, they always need to align the system’s initial direction with the main structure of the environment and update a single line segment separately without considering its geometric relationships with the other lines. Kuo et al. [24] incorporated the orthogonal assumptions into the lightweight Rao-Blackwellized Particle Filter (RBPF) SLAM. They picked up a reference line that has been observed most of the time and identified whether the other lines are orthogonal to the reference one. By filtering out the non-orthogonal lines, they could increase the accuracy and reduce the complexity when calculating the importance weight of each particle in RBPF process. However, they did not use the orthogonal constraint to adjust the line parameters. Recently, Choi, et al. [25] proposed a soft constrained SLAM system that utilizes a monocular upward-looking camera. The camera extracts line and point features on the ceiling: both are detected repeatedly and consistently for long periods of operation time. The distances between line and point measurements are calculated and applied in the constrained Extended Kalman Filter (EKF) framework. Since the constraint is not derived from a priori knowledge but rather from an observed geometric relationship, it is considered a soft constraint method. Nonetheless, further studies are necessary to exploit the soft constraint approach for laser-scanning-based SLAM.

This research formulated a new feature-based SLAM technique incorporating a constrained least squares method. The superiority of the proposed approach, compared with the previous works, lies in its direct adjustment of extracted line features according to the parallel or orthogonal conditions: the least squares method accounts for the presence of errors in point cloud observations and decreases the uncertainties of estimations of final line-feature parameters [26,27], which leads to successful data association. For the proposed approach, the Unscented Kalman Filter (UKF) algorithm was chosen, because it is a widely used means of estimation for feature-based SLAM and is easy to implement [28,29]. The performance of the proposed approach was tested both with and without the constrained adjustment. The accuracy of the constrained kinematic 3D laser scanning system’s point cloud acquisition was evaluated by comparison with the measurements acquired by a total station. Additionally, to investigate the feasibility of the point cloud acquisition in BIM perspective, further evaluation was performed in reference to the guidelines of the General Services Administration for BIM accuracy [30].

2. Methods

2.1. Overview

Features contain both semantic and metric information: semantically, they provide the feature type such as point, line or plane; and metrically they provide geometric parameters such as range and orientation [19,31]. The feature-based SLAM technique entails incrementally building a map of features in the environments and using this feature map to simultaneously localize the mobile system [20]. In case of line-feature-based SLAM, the basic assumption, particularly for building interiors, is that the physical structures can be modeled by a set of orthogonal or parallel lines, though this requires a reliable feature extraction technique [32,33].

This research proposes a constrained least squares method that adjusts the extracted line features according to the geometric conditions (orthogonality or parallelism) to effect better localization quality. Figure 1 shows the overall process of the proposed approach. The current mobile system, which has two wheels on the left and the right, initially predicts its state based on odometry information, but it is strongly influenced by the accumulation of errors, which results in considerable location errors at the end [24]. Assuming the line features taken from scans to be more reliable, they can be used to correct the system’s state through data association. Unfortunately, uncertainties arising from scan data can lead to incorrect feature extraction and failure in data association step [18,34]. The proposed constrained approach is applied to adjust the line-feature extractions, which helps to reduce uncertainties, thus leading to successful data association. Finally, based on the corrected system’s locations, 2D vertical point profiles are sequentially registered. In this way, the 3D environment is reconstructed along the system’s trajectory.

Figure 1. Study flow chart.

Figure 1. Study flow chart.

2.2. Odometry Positioning

The Kalman Filter based SLAM estimates a process state at some given time and then obtains feedback in the form of measurements. As such, its equations fall into two groups: control update equations (the state model) and measurement update equations (the measurement model). The state model is responsible for projecting forward (in time) the current state and error covariance estimates to obtain the a priori estimates for the next time step. The measurement model is responsible for the feedback; i.e., for the incorporation of a new measurement into the a priori estimate to obtain an improved a posteriori estimate [35].

In the control update, provided that the state estimation of the current kinematic scanning system (position and orientation) at time t is $\widehat{\text{X}}(t)={\left[\begin{array}{ccc}x(t)& y(t)& \mathrm{\theta }(t)\end{array}\right]}^{\text{T}}$, the next state ${\widehat{\text{X}}}^{-}(t+1)$, which is the displacement of the mobile system between two intermediate points along its trajectory, can be obtained by odometry dead-reckoning. The odometry model relies on a piecewise approximation using the displacements of the left and the right wheel as

$$O={\left[\begin{array}{cc}{s}_r& s_l\end{array}\right]}^{\text{T}}$$

(1)

Aiming to estimate the a priori state of the kinematic scanning system at time t + 1, the following transition function is used:

$${\widehat{\text{X}}}^{-}(t+1)=f(\widehat{\text{X}}(t),O(t))$$

(2)

$$\hspace{0.17em}\hspace{0.17em}=\hspace{0.17em}\left[\begin{array}{c}x(t)+s\cdot \cos (\mathrm{\theta }(t)+\mathrm{\Delta }\mathrm{\theta }/2)\\ y(t)+s\cdot \sin (\mathrm{\theta }(t)+\mathrm{\Delta }\mathrm{\theta }/2)\\ \mathrm{\theta }(t)+\mathrm{\Delta }\mathrm{\theta }\end{array}\right]$$

(3)

$$=\left[\begin{array}{c}x(t)+(s_r+s_{\ell })/2\cdot \cos (\mathrm{\theta }(t)+\left(s_r-s_{\ell }\right)/2b)\\ y(t)+(s_r+s_{\ell })/2\cdot \sin (\mathrm{\theta }(t)+\left(s_r-s_{\ell }\right)/2b)\\ \mathrm{\theta }(t)+\left(s_r-s_{\ell }\right)/b\end{array}\right]$$

(4)

where s and ∆θ represent the distance and angular displacement respectively between two consecutive time steps t and t + 1, and b is the baseline between two wheels [36].

2.3. Line-Feature Extraction

After the displacement, only the location of the mobile system changes, as estimated by odometry, while the locations of map features, being static entities, remain the same as estimated in the previous time instant. Since the odometry information is often erroneous, we cannot rely directly on it [37], but we can use the map features of the environment to estimate the a posteriori state of the kinematic scanning system because the displacement of the mobile system produces changes in the dependencies existing between the location of the mobile system and those of the map features [38]. This is accomplished by scanning the features from the surrounding environment and re-observing them while the system moves around [39]. The line segment, as represented by the Hessian model, is the commonly employed feature in SLAM [40,41]:

$$x\cos \mathrm{\varphi }+y\sin \mathrm{\varphi }-\mathrm{\rho }=0$$

(5)

where the line parameters $Y={\left[\begin{array}{cc}\mathrm{\varphi }& \mathrm{\rho }\end{array}\right]}^{\text{T}}$ are the orientation and distance from the origin, respectively.

In the present study, an incremental algorithm is used to extract line features from the laser scan data, owing to its superior speed and correctness compared to other line-extraction algorithms [42]. The incremental algorithm starts with the first two points (p₁ and p₂ in Figure 2) to construct a line. It then adds the next point to the current line model (line #1 in Figure 2), and re-computes the line parameters. If a predefined condition (e.g., the variances of line parameters) is satisfied, it continues to add new points (p₃–p₆ in Figure 2); otherwise, it puts back the last point and computes new line parameters (line #2 in Figure 2) [42,43].

Figure 2. Incremental line extraction algorithm (adapted from [43]).

Figure 2. Incremental line extraction algorithm (adapted from [43]).

2.4. Constrained Adjustment

The proposed constrained approach is based on the fact that in most indoor environments, major structures, such as walls and doors, can be represented by sets of lines that are orthogonal or parallel to each other. Assuming that the first line Y^r₁ is the reference, the conditional equations for the other orthogonal or parallel lines Y_i are defined as

$$if |{\mathrm{\varphi }}_1^r-{\mathrm{\varphi }}_i|-\mathrm{\pi }/2<\mathrm{\theta }\hspace{0.17em},\hspace{0.17em}\hspace{0.17em}{Y}_i\hspace{0.17em}\hspace{0.17em}\text{is}\hspace{0.17em}\hspace{0.17em}\text{orthogonal}\hspace{0.17em}\hspace{0.17em}(\perp )\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i=2,\cdots ,n$$

(6)

$$if |{\mathrm{\varphi }}_1^r-{\mathrm{\varphi }}_i|-\mathrm{\pi }<\mathrm{\theta }\hspace{0.17em}\hspace{0.17em}or\hspace{0.17em}\hspace{0.17em}|{\mathrm{\varphi }}_1^r-{\mathrm{\varphi }}_i|<\mathrm{\theta }\hspace{0.17em}\hspace{0.17em},\hspace{0.17em}\hspace{0.17em}{Y}_i\hspace{0.17em}\hspace{0.17em}\text{is}\hspace{0.17em}\hspace{0.17em}\text{parallel}\hspace{0.17em}\hspace{0.17em}(\parallel )\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i=2,\cdots ,n$$

(7)

where ${\mathrm{\varphi }}_1^r$ and ϕ_i are the orientation of the reference and the other line, respectively (ranging from −π to π), θ is the threshold to identify the orthogonal lines: θ = 10° was empirically determined because it effectively filtered out the arbitrarily-oriented lines while reserving the lines which are slightly off the constraints (possibly due to sensor imperfections) in this research. In Equation (7), the former condition indicates the case that the other line Y_i is located in the same side as the reference line Y^r₁, and the latter condition, vice versa. Figure 3 illustrates the conceptual idea of the orthogonal and the parallel relationships of extracted line features. As indicated in the figure, a total of five lines are detected within the range (δ) of the laser scanner. Among them, a line including the largest number of point clouds is selected as the reference line (Y^r₁). If the other lines satisfy conditional Equation (6), they are considered to be orthogonal, as $Y_2^{\perp }$ and $Y_3^{\perp }$; otherwise, if the other lines are on the opposite side of the reference line and satisfy the former condition of Equation (7), or if they are on the same side of the reference line and satisfy the latter condition of Equation (7), they are considered to be parallel, as $Y_4^{\parallel }$ and $Y_5^{\parallel }$, respectively.

Figure 3. Constrained relations of extracted line features.

Figure 3. Constrained relations of extracted line features.

To formulate the matrix expression, the normal matrix and its matching constraints matrix are formed. In this procedure, the constraint equations border the normal equations as

$$\left[\begin{array}{cc}{J}^{\text{T}}WJ& Z^{\text{T}}\\ Z& 0\end{array}\right]\left[\begin{array}{c}\widehat{\mathrm{\xi }}\\ \widehat{\mathrm{\lambda }}\end{array}\right]=\left[\begin{array}{c}{J}^{\text{T}}W\mathrm{\tau }\\ {\mathrm{\tau }}_c\end{array}\right]$$

(8)

$$f_i=x\cos {\mathrm{\varphi }}_i+y\sin {\mathrm{\varphi }}_i-{\mathrm{\rho }}_i$$

(9)

$$J\widehat{\mathrm{\xi }}=\mathrm{\tau }, J=\left[\begin{array}{cccc}{A}_1& & & \\ & A_2& & \\ & & \ddots & \\ & & & A_i\end{array}\right], A_i=\left[\begin{array}{cc}\frac{\partial f_{i1}}{\partial {\mathrm{\varphi }}_i}& \frac{\partial f_{i1}}{\partial {\mathrm{\rho }}_i}\\ \begin{array}{l}\frac{\partial f_{i2}}{\partial {\mathrm{\varphi }}_i}\\ ⋮\\ \frac{\partial f_{i{n}_i}}{\partial {\mathrm{\varphi }}_i}\end{array}& \begin{array}{l}\frac{\partial f_{i2}}{\partial {\mathrm{\rho }}_i}\\ ⋮\\ \frac{\partial f_{i{n}_i}}{\partial {\mathrm{\rho }}_i}\end{array}\end{array}\right], \widehat{\mathrm{\xi }}=\left[\begin{array}{c}\begin{array}{c}\begin{array}{c}\mathrm{\Delta }{\mathrm{\varphi }}_1\\ \mathrm{\Delta }{\mathrm{\rho }}_1\end{array}\\ \mathrm{\Delta }{\mathrm{\varphi }}_2\\ \mathrm{\Delta }{\mathrm{\rho }}_2\end{array}\\ ⋮\\ \mathrm{\Delta }{\mathrm{\varphi }}_i\\ \mathrm{\Delta }{\mathrm{\rho }}_i\end{array}\right], \mathrm{\tau }=\left[\begin{array}{c}-f_1{\left(Y_{1_0}\right)}_1\\ ⋮\\ -f_1{\left(Y_{1_0}\right)}_{n_1}\\ ⋮\\ -f_i{\left(Y_{i_0}\right)}_1\\ \begin{array}{c}⋮\\ -f_i{\left(Y_{i_0}\right)}_{n_i}\end{array}\end{array}\right]$$

(10)

The Equation (10) is a linearization of Equation (9) by Taylor series, where J is the Jacobian matrix of the Hessian line model Equation (9) with respect to ϕ and ρ, τ is the observed minus computed values, $\widehat{\mathrm{\xi }}$ is correction values of line parameters, Y_i0 is initial approximation of line parameters (ϕ_io ρ_io) of line i. A_i indicates the matrix of partial differentials, where i is number of lines and n_i is number of points which was used to extract the line. Correspondingly, the Jacobian matrix of the constraint model (Equations (6) and (7)) is formed with respect to ${\varphi }_1^r$ and ϕ_i then included in the normal matrix as additional rows Z and columns Z^T, and their constants $\pm \raisebox{1ex}{$\mathrm{\pi }$}\!\left/ \!\raisebox{-1ex}{$2$}\right.\hspace{0.17em}\hspace{0.17em}$ or ±π are added to the constants matrix as additional rows τ_c. For example, in the case of Figure 3, line $Y_2^{\perp }$ and $Y_3^{\perp }$ satisfy the orthogonal condition, and line $Y_4^{\parallel }$ and $Y_5^{\parallel }$ satisfy the parallel condition relative to the reference line (Y^r₁). Therefore the matrix form of the constraint equation is formed as shown in Equation (11) and the first term of Equation (11) will be matrix Z. The signs in front of $\raisebox{1ex}{$\mathrm{\pi }$}\!\left/ \!\raisebox{-1ex}{$2$}\right.$ and π are dependent on the angle parameter difference, positive or negative. Weight matrix (W) is based on the number of points which was used to extract a line (Equation (12)). τ is the observed minus computed values. $\widehat{\mathrm{\xi }}$ is the estimated correction value for the line-feature parameters while $\widehat{\mathrm{\lambda }}$ is the additional row for Lagrangian multipliers.

$$Z\widehat{\mathrm{\xi }}={\mathrm{\tau }}_c, Z=\left[\begin{array}{cccc}1& 0& -1& 0\\ 1& 0& 0& 0\\ 1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\begin{array}{cccc}0& 0& 0& 0\\ -1& 0& 0& 0\\ 0& 0& -1& 0\\ 0& 0& 0& 0\end{array}\begin{array}{c}0\\ 0\\ 0\\ -1\end{array}\begin{array}{c}0\\ 0\\ 0\\ 0\end{array}\right], \widehat{\mathrm{\xi }}=\left[\begin{array}{c}\begin{array}{l}\mathrm{\Delta }{\mathrm{\varphi }}_1\\ \mathrm{\Delta }{\mathrm{\rho }}_1\\ \mathrm{\Delta }{\mathrm{\varphi }}_2\end{array}\\ \begin{array}{c}\mathrm{\Delta }{\mathrm{\rho }}_2\\ \begin{array}{l}\mathrm{\Delta }{\mathrm{\varphi }}_3\\ \mathrm{\Delta }{\mathrm{\rho }}_3\end{array}\\ \begin{array}{l}\mathrm{\Delta }{\mathrm{\varphi }}_4\\ \mathrm{\Delta }{\mathrm{\rho }}_4\end{array}\end{array}\\ \begin{array}{l}\mathrm{\Delta }{\mathrm{\varphi }}_5\\ \mathrm{\Delta }{\mathrm{\rho }}_5\end{array}\end{array}\right], {\mathrm{\tau }}_c=\left[\begin{array}{c}\frac{\mathrm{\pi }}{2}\\ \frac{\mathrm{\pi }}{2}\\ \mathrm{\pi }\\ -\mathrm{\pi }\end{array}\right]$$

(11)

$$W={\left[\begin{array}{cccc}{W}_1& & & \\ & W_2& & \\ & & \ddots & \\ & & & W_i\end{array}\right]}_{(N \text{X} N)}, W_i={\left[\begin{array}{ccc}\frac{n_i}{N}& & \\ & \ddots & \\ & & \frac{n_i}{N}\end{array}\right]}_{(n_i \mathrm{x} n_i)}$$

(12)

where, n_i is number of points when extracting ith line and i is number of lines extracted. Finally, the correction $\widehat{\mathrm{\xi }}$ and the dispersion $\text{D}\left\{\widehat{\mathrm{\xi }}\right\}$ (the adjusted covariance) of the line parameters are calculated as

$$\widehat{\mathrm{\xi }}=N^{-1}c+N^{-1}{Z}^{\text{T}}{\left(Z{N}^{-1}{Z}^{\text{T}}\right)}^{-1}\left({\mathrm{\tau }}_c-Z{N}^{-1}c\right)$$

(13)

$$\text{D}\left\{\widehat{\mathrm{\xi }}\right\}={\sigma }_0^2\left\{N^{-1}-N^{-1}{Z}^{\text{T}}{\left(Z{N}^{-1}{Z}^{\text{T}}\right)}^{-1}Z{N}^{-1}\right\}$$

(14)

where ${\sigma }_0^2$ is the reference variance, N indicates the normal matrix J^TWJ, and c is J^TWτ. For nonlinear least squares adjustment, the initial value is necessary. The extracted line parameter from the point cloud data was used as initial value. The initial value is continuously adjusted by the computed values which were based on the constraint conditions. The adjustment process is repeated until the computed value ($\widehat{\mathrm{\xi }}$) become sufficiently small [26,27,44]. In this study, process is repeated until the total sum of the line parameter’s increment ($\widehat{\mathrm{\xi }}$) gets smaller than 0.001 or the number of iteration reaches 20 times. For the distance (ρ) value the threshold 0.001 means 0.001 m, and for the orientation (ϕ) it means 0.001 rad which results in 0.0017 m error per 10 m. Figure 4 shows the detail process. The number of iteration (≤20) was checked as a convergent condition. When the iteration hits 20 times, it is not considered to be convergent, and the line is eliminated from the line list. Note that both $N^{-1}$ and $Z^{\text{T}}Z$ in Equation (14) are symmetric, positive definite matrix, the constraints will correspondingly decrease the uncertainties of adjusted parameters.

Figure 4. Iterative process of nonlinear least squares adjustment with constraints.

Figure 4. Iterative process of nonlinear least squares adjustment with constraints.

The most important aspect of least squares adjustment, for which reason it is superior to the other heuristic approaches for constrained SLAM, is its permission of all point observations with corresponding Jacobian matrix (J) and constraint condition matrix (Z) to be entered into a single adjustment equation and used simultaneously in the computations. The adjusted line parameters satisfy the geometric constraints (orthogonality or parallelism) and minimize the weighted residuals under the given constraints [26], thus resulting in better consistency between line extractions at different time steps for the next process, data association.

2.5. Data Association

Once the mobile system obtains sensor readings at any position, a way of paring the newly observed features to the past observations has to be defined. It is called data association and plays an important role because the system’s pose can be well estimated only when data association is correct [45]. New features at time t + 1 are obtained from horizontal scanning in the body frame (b), whereas previous features until time t are stored in the global frame (g), thus the transformation model to the corresponding sensor is defined as

$$\begin{array}{l}{\widehat{\mathrm{\varphi }}}_b(t+1)={\mathrm{\varphi }}_g(t)-{\widehat{\mathrm{\theta }}}_g(t+1)\\ {\widehat{\mathrm{\rho }}}_b(t+1)={\mathrm{\rho }}_g(t)-{\widehat{x}}_g(t+1)\cos {\mathrm{\varphi }}_g(t)-{\widehat{y}}_g(t+1)\sin {\mathrm{\varphi }}_g(t)\end{array}$$

(15)

where ${\mathrm{\varphi }}_g(t)$ and ${\mathrm{\rho }}_g(t)$ are the orientation and distance of the previously observed features $Y_g(t)$, and ${\widehat{\mathrm{\varphi }}}_b(t+1)$ and ${\widehat{\mathrm{\rho }}}_b(t+1)$ are the parameters of the predicted features ${\widehat{Y}}_b(t+1)$ with respect to the a priori state ${\widehat{X}}^{-}(t+1)$ obtained by Equations (2)–(4). The pairing process is performed by comparing every possible newly observed line features $Y_b(t+1)$ with predicted line features ${\widehat{Y}}_b(t+1)$ using Mahalanobis distance defined as

$$\begin{array}{l}{\chi }_{\gamma ,d}^2\ge {(Y_b^i-{\widehat{Y}}_b^j)}^{\text{T}}{(C^i+C^j)}^{-1}(Y_b^i-{\widehat{Y}}_b^j)\hspace{0.17em}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}i=1,\cdots ,n\hspace{0.17em}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}j=1,\cdots ,m\end{array}$$

(16)

where n and m indicate the number of the newly observed and predicted line features, Cⁱ and C^j are the respective covariance matrices of the line features. Cⁱ was calculated from the former process (Equation (14)), C^j is calculated from the UKF prediction step, and ${\chi }_{\gamma ,d}^2$ is a number taken from a χ² distribution with d = 2 degrees of freedom and probability level γ = 99% on which the hypothesis of pairing correctness is rejected [46]. Then, the pairs that satisfy both conditions, which is to say, that are less than ${\chi }_{\gamma ,d}^2$ and one-to-one matches, are accepted and retained for the calculation of the measurement innovation (the observed minus the predicted value) [35], which is used later to estimate the a posteriori state $\widehat{X}(t+1)$. If the pair only satisfies the former condition, that is, if it shows the one-to-many match, it is considered incorrect and is neglected. The non-matched observations greater than ${\chi }_{\gamma ,d}^2$ are transformed to the global frame and added to the next iterations as new features [47].

2.6. Unscented Kalman Filter

The UKF is a variant of the Kalman filter which is specifically aimed at problems with nonlinear models, which not only gives better performance than that of the EKF, but also has several benefits in terms of ease of implementation. Its superior performances over that of the EKF algorithm have been reported in many SLAM studies [48,49,50,51,52]. Figure 5 describes the process of UKF SLAM. The mapping functions f and h represent the nonlinear, deterministic state and measurement models. The random variables w and v represent the process and measurement noise, and their noise covariance Q and R are assumed to be independent of each other, following the normal probability distributions, respectively. The UKF starts with the unscented transformation which computes the effect of a nonlinear function upon a mean X and covariance P. It operates by computing a deterministic sample set (sigma points) which is then propagated through the non-linearity [52]. In control update, L is the dimension of X, λ is the scaling parameter, and W^X and W^P are the weight for the X and P, respectively. Once the sigma points χ are obtained from the previous position $\widehat{X}(t)$ and covariance P(t) in step (1), a current state (mean ${\widehat{X}}^{-}(t+1)$ and covariance $P^{-}(t+1)$) is predicted in steps (2) and (3). Using the predicted mean and covariance, the sigma points are recalculated in step (4). In step (5) of measurement update, Y indicates newly observed features, and the predicted mean ${\widehat{Y}}^{-}$ and covariance $P_{YY}$ and $P_{XY}$ of the measurement are calculated using the newly updated sigma points ψ for the measurement model h. Finally, the difference $\nu$ between the observed Y and the predicted features ${\widehat{Y}}^{-}$ is multiplied by Kalman gain and used to correct the current system’s position in step (6) and covariance in step (7). For additional details, please see Thrun et al. [53], Andrade-Cetto, Vidal-Calleja and Sanfeliu [48], and Terejanu [54].

Figure 5. Process of Unscented Kalman filter.

Figure 5. Process of Unscented Kalman filter.

3. Experimental Results

3.1. Implementation of Kinematic Scanning System

Figure 6a shows the kinematic 3D laser scanning system developed in our research. It measures approximately 35 cm (length) × 35 cm (width) × 78 cm (height). The platform is equipped with odometry, and carries a laptop computer (used for storing the data of each sensor) and three 2D laser range finders (Hokuyo UTM-30LX). The front laser range finder is mounted horizontally to map unknown environments and correct the position of the scanning system. The other two are mounted vertically to scan the profiles of surrounding environments while the scanning system moves. The 3D point cloud is obtained by registering those vertical profiles on the system’s trajectory. The scan area is 270° in the horizontal direction (with 1081 points) and 180° in the vertical direction (with 721 points), and the interval angle is 0.25°. This research assumes that the intrinsic sensor calibration is completed, and the extrinsic calibration process of the developed kinematic scanning system is given in Jung et al. [55] Since the current system is not designed for automatic navigation, the surveyor needs to manually move it in scanning an indoor space, as shown in Figure 6b.

Figure 6. (a) Kinematic 3D laser scanning system and (b) system operation.

Figure 6. (a) Kinematic 3D laser scanning system and (b) system operation.

A pilot implementation of the kinematic scanning system was conducted for two typical indoor places, a seminar room and a corridor at Yonsei University (Figure 7). The seminar room is a relatively small and simple structure including much clutter, whereas the corridor is longer and includes several pillars and a corner, though less clutter. The size of the seminar room is about 8.8 m (length) by 8.3 m (width), and the corridor is approximately 27.7 m long and 2.6 m wide.

Figure 7. Test sites: (a,b) are the seminar room; (c,d) are the corridor.

Figure 7. Test sites: (a,b) are the seminar room; (c,d) are the corridor.

For optimum filtering results, exact knowledge of the process and measurement noise covariance matrix (Q and R) is important. In practice, however, they are usually unknown and come from intensive empirical analysis [56,57]. To specify Q and R, one of the methods is to fix one of them and vary the other one by trial and error to find the smallest value that yields stable state estimates [58]. In this study, the diagonal element values of Q were determined to be (0.001 m)² for x, y and (0.001 rad)² for θ, and the diagonal element values of R were determined to be (0.019 rad)² for ϕ and (0.024 m)² for ρ.

In the experiment, three travels were performed for each test site in order to verify the effects of the proposed constrained SLAM approach. During the operation, if an obstacle was too far to be perceived, the feature detection process did not occur, causing the accumulation of odometry errors due to the absence of SLAM update feedback [59]. In practice, it was found that the line-feature extractions by the incremental algorithm with Hokuyo UTM-30LX increased linearly according to the threshold distance d (Figure 2); for example, if d was 0.01 m, line features up to about 10 m could be extracted, and 0.02 m was appropriate for 20 m. Because a too-large d for a small space extracts unnecessary line features, leading to computational complexity in the SLAM process, a proper threshold that takes due account of the size of the scan area should be adopted. Accordingly, in the present study, the threshold was determined to be 0.01 m for the small seminar room, and 0.02 m for the long corridor.

Figure 8 shows the trajectories of the mobile system in two test sites. For the seminar room, the mobile system started to move at the lower-right corner, and traveled along the counter-clockwise path, revisiting the start point to obtain a complete map. Meanwhile, for the corridor, the mobile system started to move from the lower-right point and completed one-way travel. As the mobile system moved along the trajectory of the seminar room, its positional uncertainties in the sideward direction (Figure 9a,c,e) and the forward direction (Figure 9b,d,f) were recorded within two standard deviations (95% upper confidence level) [60]. In the figure, the red and blue graphs indicate the positional uncertainties of the standard SLAM and the constrained SLAM, respectively. Note that the mobile system was forced to turn sharply at the four corners. This generally would degrade the quality of navigation, since the odometry errors would quickly accumulate particularly for the sideward direction. Accordingly, in the first travel (Figure 9a), both the standard and constrained SLAMs show continuous increases of uncertainty until the mobile system reached the first turn (around 3000 time steps). After which, the standard SLAM shows drastic divergences of uncertainty, which were mainly due to failure in the data association phase, leading to loss of information for the correction in measurement update. Meanwhile, the constrained SLAM maintains smooth growth of uncertainty until the second turn (around 5000 time steps) and shows convergences, indicating that the system re-observed the line features in the beginning, which reduces the uncertainty for the line features as well as the system’s pose [53]. Similarly, in Figure 9c,e, the constrained SLAM maintains the consistent pattern, smooth convergences after divergences of uncertainty, whereas the standard SLAM shows abnormal divergences of uncertainty after half the time steps. The superiority of the constrained SLAM also can be found in the forward direction (Figure 9b,d,f): overall, the constrained SLAM successfully maintains the convergences during the entire time step, whereas the standard SLAM shows the drastic divergences of uncertainty again.

Figure 8. Trajectories of kinematic laser scanning: (a) the seminar room; (b) the corridor.

Figure 8. Trajectories of kinematic laser scanning: (a) the seminar room; (b) the corridor.

Figure 10 provides further qualitative evidence of the feasibility of the constrained approach. Note that the estimated mobile system’s trajectory is improved by matching newly observed features with previously stored features, thus it is desirable to obtain as much pairings as possible in the data association [38]. In the figure, the vertical axis denotes the number of features, and the horizontal axis represents the time steps. The green graph indicates the number of newly observed line features for each time step, and the red and blue graphs indicate the number of matched line features without and with the constrained approach, respectively. There are apparent discrepancies in the results without and with the constrained approach: all of the tests show that the constrained approach had higher matching numbers, commonly after the third turns (the red graph between 6000 and 9000 time steps in Figure 10), which resulted in the failures to maintain convergences of uncertainty for the standard SLAM in Figure 9. The overall matching rates in the seminar room test without and with the constrained approach were calculated as 45.4% and 85.8% for the first travel, 50.7% and 69.2% for the second travel, and 46.7% and 76.9% for the third travel, respectively. This result demonstrated the primary advantage of the proposed constrained approach for successive data association, as achieved by reducing the uncertainties of the adjusted line parameters and continuing to improve the localization accuracy.

Figure 9. Uncertainty estimates of the seminar room test resulting from UKF SLAM without (red graphs) and with (blue graphs) the constrained approach: sideward direction (a,c,e); and forward direction (b,d,f).

Figure 9. Uncertainty estimates of the seminar room test resulting from UKF SLAM without (red graphs) and with (blue graphs) the constrained approach: sideward direction (a,c,e); and forward direction (b,d,f).

Figure 10. Comparisons of data association results for the seminar room test: (a) first travel; (b) second travel; and (c) third travel.

Figure 10. Comparisons of data association results for the seminar room test: (a) first travel; (b) second travel; and (c) third travel.

The effects of the constrained approach were also found in the other test site, the corridor. This time, it completed one-way travel, thus all of the travels showed continuous growth of the system’s positional uncertainties in the sideward directions (Figure 11a,c,e). Compared with the seminar room tests, the differences between the standard SLAM and the constrained SLAM are not noticeable (possibly due to its straight and simple trajectory). As the system traveled along the trajectory, however, the standard SLAM started to show slightly larger divergences of uncertainty than the constrained SLAM. In fact, the difference is more noticeable in the forward directions (Figure 11b,d,f): after the mobile system reached half the time step, the standard SLAM shows the abnormal divergences of uncertainty. This can be explained also by the low matching numbers in the data associations in Figure 12, red graphs: the number of matched line features without the constrained approach (red graphs) gradually decreased, which lead to a lack of information and the abnormal divergences of system’s positional uncertainty. Satisfactory results, by contrast, could be achieved with the constrained SLAM: the application showed continuous convergences of uncertainty; likewise, the number of matched line features appeared to be well maintained at every time step (Figure 12, blue graphs). The overall matching rates in the corridor tests without and with the constrained approach were calculated as 55.8% and 88.0% for the first travel, 62.5% and 95.0% for the second travel, and 56.8% and 83.2% for the third travel, respectively.

Figure 11. Uncertainty estimates of the corridor test resulting from UKF SLAM without (red graphs) and with (blue graphs) the constrained approach: sideward direction (a,c,e) and forward direction (b,d,f).

Figure 11. Uncertainty estimates of the corridor test resulting from UKF SLAM without (red graphs) and with (blue graphs) the constrained approach: sideward direction (a,c,e) and forward direction (b,d,f).

Figure 12. Comparisons of data association results for the corridor test: (a) first travel; (b) second travel; and (c) third travel.

Figure 12. Comparisons of data association results for the corridor test: (a) first travel; (b) second travel; and (c) third travel.

3.2. Visualization of Point-Cloud Data

The feasibility of the proposed constrained approach was further investigated with reference to the completeness of the mapping results. According to the highest line matching success rate, the first travel for the seminar room (85.8%) and the second travel for the corridor (95.0%) were selected. The first travel for the seminar room consisted of 9072 time steps (232.0 s) and resulted in a map with about 13.4 million points; the second travel for the corridor had 11,627 (324.8 s) time steps and yielded a map with about 20.2 million points. Note that the current scanning system provides complete observations of the surrounding environments; in Figure 13, Figure 14, Figure 15 and Figure 16, the ceiling points are omitted for the purposes of a clearer comparison.

A qualitative visual inspection of the seminar room revealed that the mapping accuracies with the odometry information only (Figure 13a) degraded quickly with distance traveled, due to the accumulated errors. In comparison, Figure 13b,c show that for the same scene, the mapping results from the use of the feature information were much more consistent. However, in the detailed view of the seminar room (Figure 14a,b), it is evident that the standard SLAM approach, showing a noticeable drift at the end, was not satisfactory (Figure 14a), whereas the mapping reconstruction by the constrained SLAM correspond more closely to the real environment (Figure 14b). Likewise, the visual inspection of the corridor results (Figure 15 and Figure 16) demonstrated the usefulness of the constrained approach for indoor mapping.

Figure 13. Point-cloud data acquisitions of the seminar room by kinematic scanning: (a) odometry only; UKF SLAM (b) without and (c) with the constraint approach.

Figure 13. Point-cloud data acquisitions of the seminar room by kinematic scanning: (a) odometry only; UKF SLAM (b) without and (c) with the constraint approach.

Figure 14. Detailed view of the seminar room (a) without and (b) with the constraint approach.

Figure 14. Detailed view of the seminar room (a) without and (b) with the constraint approach.

Figure 15. Point-cloud data acquisitions of the corridor by kinematic scanning: (a) odometry only; UKF SLAM (b) without and (c) with the constraint adjustment.

Figure 15. Point-cloud data acquisitions of the corridor by kinematic scanning: (a) odometry only; UKF SLAM (b) without and (c) with the constraint adjustment.

Figure 16. Detailed view of the corridor (a) without and (b) with the constraint approach.

Figure 16. Detailed view of the corridor (a) without and (b) with the constraint approach.

3.3. Accuracy Assessment

To test the feasibility and acceptability of the point-cloud acquisitions for the purpose of input to as-built BIM creation, the metric quality was assessed with the method proposed by Hong et al. [61] and the guidelines of the General Services Administration (GSA) for BIM accuracy [30]. The accuracy assessment was based on the well-distributed and clearly identifiable points such as corners of doors, walls, and windows as depicted in Figure 17. For the reference, the measurements acquired by a highly-accurate total station were used. First, the accuracy of the point-cloud data was assessed by means of the Euclidean average distance error (${\delta }_{avg}$)

$${\delta }_{avg}=\frac{1}{n}{\displaystyle \sum _{i=1}^n|\text{R}{a}_i-\text{T}-b_i|}$$

(17)

where a_i is the i-th check point in the point-cloud data, b_i is the corresponding check point acquired by the total station, and R and T are the rotation and translation parameters for 3D Helmert transformation. Note that the scale was not considered in this transformation [62]. In Figure 17a, a total of 27 points were extracted from the seminar room, among which 14 were used to calculate the transformation model parameters (yellow points), and the remaining 13 for the validation (red points). For the corridor in Figure 17b, a total of 38 points were extracted: 19 for the transformation model (yellow points), and 19 for the validation (red points). The error vectors in the x, y, and z directions together with the corresponding average errors are listed in Table 1 (the seminar room) and Table 2 (the corridor). The Euclidean average distance error was calculated to 0.034 m for the seminar room and 0.043 m for the corridor, which satisfied the error tolerance (level 1) for point-cloud acquisition (0.051 m) according to the GSA guidelines [30]. Additionally, the quality of the point-cloud data was assessed by the Root Mean Square Error (RMSE) and the Spherical Accuracy Standard (SAS). The RMSE was calculated as

$$RMSE=\sqrt{\frac{1}{n}{\displaystyle \sum _{i=1}^n{\left(a_i^t-b_i\right)}^2}}$$

(18)

where $a_i^t$ indicates the point transformed to the coordinates of the total station. The RMSEs for x, y, z directions also are listed in Table 1 and Table 2. The SAS, which represents the spherical radius of a 90% probability sphere [63], is defined as

$$SAS=2.5\times 0.3333\times \left(RMS{E}_x+RMS{E}_y+RMS{E}_z\right)$$

(19)

The calculated SAS value was 0.050 m for the seminar room and 0.067 m for the corridor. This represents a positional accuracy of the two point-cloud acquisitions at the 90% confidence level. The main factors affecting the higher error for the corridor are: (1) one-way travel that did not allow for revisiting the starting point; and (2) the relatively small number of matched line features in the corridor (3.63 per time step) relative to that achieved for the seminar room (3.79 per time step).

Figure 17. Check point distribution of (a) the seminar room; and (b) the corridor.

Figure 17. Check point distribution of (a) the seminar room; and (b) the corridor.

Table 1. Accuracy assessment results for point-cloud data of the seminar room (unit: meter).

Point ID	Error Vector X	Error Vector Y	Error Vector Z	Error
1	−0.021	−0.013	0.005	0.025
3	−0.023	0.000	−0.013	0.027
5	0.003	−0.006	−0.015	0.016
7	0.010	0.061	0.021	0.066
9	0.030	−0.015	−0.013	0.036
11	−0.019	0.030	0.022	0.042
13	−0.021	0.013	0.002	0.025
15	0.018	0.001	0.017	0.025
17	0.022	−0.011	−0.001	0.025
19	0.031	0.009	−0.008	0.034
21	0.000	0.000	−0.004	0.004
23	0.007	−0.014	0.008	0.018
25	0.041	0.004	−0.001	0.041
27	0.029	−0.045	−0.002	0.053
Average error				0.034
RMSE	0.024	0.024	0.012	0.036
SAS				0.050

Table 1. Accuracy assessment results for point-cloud data of the seminar room (unit: meter).

Point ID	Error Vector X	Error Vector Y	Error Vector Z	Error
1	−0.021	−0.013	0.005	0.025
3	−0.023	0.000	−0.013	0.027
5	0.003	−0.006	−0.015	0.016
7	0.010	0.061	0.021	0.066
9	0.030	−0.015	−0.013	0.036
11	−0.019	0.030	0.022	0.042
13	−0.021	0.013	0.002	0.025
15	0.018	0.001	0.017	0.025
17	0.022	−0.011	−0.001	0.025
19	0.031	0.009	−0.008	0.034
21	0.000	0.000	−0.004	0.004
23	0.007	−0.014	0.008	0.018
25	0.041	0.004	−0.001	0.041
27	0.029	−0.045	−0.002	0.053
Average error				0.034
RMSE	0.024	0.024	0.012	0.036
SAS				0.050

Table 2. Accuracy assessment results for point-cloud data of the corridor (unit: meter).

Point ID	Error Vector X	Error Vector Y	Error Vector Z	Error
1	−0.031	0.009	0.000	0.032
4	−0.059	0.019	−0.002	0.062
6	−0.062	0.023	0.022	0.069
8	−0.010	0.003	0.017	0.020
10	−0.013	−0.015	0.004	0.020
12	0.014	0.014	−0.034	0.039
14	−0.002	0.036	−0.039	0.053
16	0.011	0.025	0.011	0.030
18	0.007	−0.012	0.006	0.015
21	0.010	−0.024	0.007	0.026
22	0.008	−0.027	−0.007	0.029
24	0.016	−0.046	−0.011	0.050
25	0.056	−0.026	−0.002	0.062
28	−0.007	−0.033	−0.001	0.034
29	0.027	−0.010	−0.009	0.030
32	0.019	−0.025	−0.012	0.034
34	0.088	−0.022	−0.005	0.090
36	−0.002	−0.037	0.015	0.040
38	−0.063	−0.026	0.053	0.086
Average error				0.043
RMSE	0.036	0.025	0.019	0.048
SAS				0.067

Table 2. Accuracy assessment results for point-cloud data of the corridor (unit: meter).

Point ID	Error Vector X	Error Vector Y	Error Vector Z	Error
1	−0.031	0.009	0.000	0.032
4	−0.059	0.019	−0.002	0.062
6	−0.062	0.023	0.022	0.069
8	−0.010	0.003	0.017	0.020
10	−0.013	−0.015	0.004	0.020
12	0.014	0.014	−0.034	0.039
14	−0.002	0.036	−0.039	0.053
16	0.011	0.025	0.011	0.030
18	0.007	−0.012	0.006	0.015
21	0.010	−0.024	0.007	0.026
22	0.008	−0.027	−0.007	0.029
24	0.016	−0.046	−0.011	0.050
25	0.056	−0.026	−0.002	0.062
28	−0.007	−0.033	−0.001	0.034
29	0.027	−0.010	−0.009	0.030
32	0.019	−0.025	−0.012	0.034
34	0.088	−0.022	−0.005	0.090
36	−0.002	−0.037	0.015	0.040
38	−0.063	−0.026	0.053	0.086
Average error				0.043
RMSE	0.036	0.025	0.019	0.048
SAS				0.067

4. Conclusions

The present study proposed a new line-feature-based SLAM technique incorporating the constrained least squares method for line adjustments. The superiority of the proposed approach, compared with the conventional methods, is its reduction of the adjusted lines’ uncertainties for successful data associations, which consequently leads to more accurate systems’ pose estimations. The experimental results showed accurate reconstructions of 3D scenes, demonstrating the proposed method’s potential utility for indoor mapping. Moreover, the proposed constrained adjustment method can be simply applied to any line-feature-based SLAM applications to indoor environments satisfying the parallel and orthogonal assumptions.

In this light, we are currently looking into how this technique can be applicable to other feature-based methods. As regards feature extraction, interesting for the purposes of further research is the geometric constraints concept, which could make possible the implementation of new features (e.g., curves and circles) and their geometric relationships for various indoor conditions. Future work will also focus on combining new sensors such as camera and IMU (Inertial Measurement Unit) for improved navigation quality. The combined utilization of these sensors with laser scanners allows new sensor readings for redundancy, increasing localization performance and long-term stability. In addition, autonomous navigation for the developed kinematic scanning system is needed, because uncertainties arising from manual operation can lead to incorrect localization and mapping results. However, for the purposes of obtaining a complete map of complex and cluttered indoor environments, fully automated navigation is not practical at the present stage. The viable solution is semi-autonomous navigation such as marker-based SLAM, whereby the system’s location is identified with the marker attached to a surveyor or controlled by remote control. Ultimately, the proposed kinematic scanning system is applicable to the as-built BIM, where it can be used for fast and efficient raw point-cloud data acquisition. The next phase of the research will involve the automated recognition of objects from the point-cloud data, which should be followed to keep up with the requirements of SCAN-to-BIM conversion.