2.1

ICP algorithm

A point cloud P_i is a result of a single LiDAR scan. It contains a number of two-element vectors, which specify polar coordinates of points registered in the sensor’s surroundings at a given time moment i. To determine the change of LiDAR’s pose between two consecutive scans, the ICP algorithm was implemented¹⁴. Its task is to find an optimal linear transformation from P₁ into P₂ by minimizing the mean square error (MSE) of displacement between corresponding points in both point clouds.

First, the implemented algorithm calculates the cross-covariance matrix of point clouds P₁ and P₂. Then, the Singular Value Decomposition (SVD) of the matrix is performed in accordance with to the following formula:

where U and V^T are orthogonal matrices and W is a diagonal matrix. As the polar decomposition theorem¹⁵ states, the product of U and V^T can be interpreted as a rotation matrix R. R determines the difference in the orientation between both point clouds:

Knowing R and the geometrical centers μ₁ and μ₂ of each point cloud, one can determine the translation t between two consecutive LiDAR positions:

The obtained t and R can be used to match both clouds and determine the change in position and orientation of the sensor between registrations. The result of 5 iterations of the implemented ICP algorithm is shown in Figure 1.

Figure 1.

Left: two registered point clouds registered in consecutive scans. Right: both point clouds after ICP matching.

2.2

Clustering algorithm

To construct a map, the SLAM implementation proposed in this paper uses shapes recognition. Simple structures (lines and L-shapes) are searched for in data recorded by the laser scanner. The procedure of identifying shapes in a point cloud is facilitated by points clustering. The idea behind the operation of clustering scans is to divide registered points into disjoint sets. The conditions of belonging to a given set are defined by means of metrics, i.e. functions determining the relative distance between elements of the set¹⁶. This operation is used to preprocess information. Separating disjoint subgroups (clusters) enables to perform an analysis of each of them individually. Given that an appropriate metric was implemented, resulting sets of points represent real objects, such as fragments of walls, doors, windows, etc. Hence, further data processing and map creation could be faster and less complex¹⁶.

The basic metric used to cluster points, which are defined using cartesian coordinates system, is the Euclidean distance. If a separation between two consecutive points is smaller than a given threshold, they are assigned to the same cluster. However, this criterion is not sufficient to reliably cluster a point cloud. Spatial density of consecutive registrations depends on the angle at which a laser beam is reflected from a given surface. The more it deviates from the right angle, the greater are the relative distances between successive points and the smaller are clusters. Moreover, the range from the sensor is affecting the spatial density of a cloud as well. The range at which a measurement is taken is directly proportional to the density of points. To optimize the clustering process, a straight-line criterion was employed. As the clustering algorithm is going to work on the data collected indoors, numerous flat surfaces are expected to be scanned, i.e. walls, doors, etc. Therefore, a metric was introduced that forces the algorithm to group three consecutive points if the angle between them is close to π.

However, not all points identified as lying along a line should be grouped together. In some cases, the angle and range at which the elements of the environment are observed limits the capability to correctly identify the surfaces on which they are located. Therefore, a metric was implemented that prevents the addition of points to a given cluster if the range or angle of observation were different than the predefined accepted values.

The result of the clustering algorithm is shown in Figure 2. Each point cluster is drawn using a different color and the location of the LiDAR is marked with a red circle.

Figure 2.

Left: The result of the clustering algorithm single run. Right: a plan view of a scanned corridor.

The right side of the Figure 2 shows a plan view of the scanned corridor. The wall protrusions that are not visible when analyzing the point cloud due to the small incidence angle (and the resulting insufficient density of measuring points) are marked in red.

2.3

Shape identification algorithm

After properly performed point cloud clustering, the SLAM algorithm proceeds to identification of shapes among obtained clusters. The algorithm detects straight lines and corners (L-shapes). If a given cluster cannot be assigned to any of these two categories, it is left unprocessed for further analysis. During latter algorithm iterations, it is combined with newly recorded points.

Shapes are identified in the following order. First, an analysis of the covariance matrix of coordinates of points in a cluster is performed. Provided that the value of one of the diagonal elements of the matrix is significantly larger than the value of the other element, the cluster is identified either as a horizontal line (variance in the x direction is relatively large), or a vertical line (variance in the y direction is relatively large). For clusters that do not meet the conditions described above, principal component analysis (PCA) is performed. If one of the eigenvalues of the covariance matrix is significantly larger than the other, then the cluster is identified as a line as well. The operation of the algorithm described above is shown in Figure 3, where all the lines were correctly identified among the clusters in Figure 2.

Figure 3.

Identification of lines among point clusters.

If a given cluster fails to meet the previously described conditions, the algorithm proceeds to a more complex procedure. It checks whether a group of points can be classified as a corner (L-shape). In the first step, the shape of a rectangle is fitted into the cluster according to the SBRF (Search Based Rectangle Fitting) algorithm¹⁷, which minimizes the expression:

where θ is the angle of rotation of the rectangle (for the value 0 one of the edges is parallel to the abscissa), x_i and y_i are the coordinates of subsequent points from the cluster, and c₁, c₂ are the coefficients that satisfy the equation of two perpendicular lines x cos θ + y sin θ = c₁ and –x sin θ + y cos θ = c₂. These lines are sides of the rectangle.

In the next step of the algorithm, three vertices of the rectangle, that describe the cluster most accurately, are selected. Then, four values are calculated:

If all the calculated values are within the experimentally determined limits, the shape is classified as a corner. If only the last condition is not met, the cluster can be identified as a corner or as a straight line. The final decision is made by comparing MSE of distance between a point cloud fragment and a closest line and corner.

If any of the first three values does not fall within the specified limits, the analyzed cluster cannot be identified as a line or L-shape. It is then further stored as a set of points. The results of operation of the above described algorithm are shown in Figure 4.

Figure 4.

Identification of lines and L-shapes among point clusters

2.4

Algorithm implementation

A Kalman filter (KF) is an element of the implemented SLAM system that allows for indirect fusion of recorded data after each measurement. The first step of the KF is a prediction of the sensor state vector (position, orientation and speed). The adopted motion model assumes a constant velocity of the laser scanner between measurements. The relationships describing this process can be characterized by using the following formulas:

where is the predicted state vector, containing estimates of the platform’s kinematic parameters, is the state vector estimated by the filter in the previous time step k, Φ is the transition matrix, while the notation k + 1|k means that the prediction relates to the state at the moment k + 1, based on the state from the moment k. The matrices P_k+1|k and P_k|k represent the covariance matrix of prediction and filtration errors respectively, whereas Q_k is the covariance matrix of the process disturbances, modeling random effects acting on the robot’s motion.

A predicted sensor displacement, calculated as a difference between the currently predicted and previously estimated state vectors , is used as the initial condition for the ICP algorithm. This is an important step as the ICP algorithm inherent property is to seek a local minimum of the displacement MSE. If the sensor position between consecutive registrations differs significantly, the algorithm may erroneously indicate the pose transformation, what could lead to an incorrect fusion of subsequent scans.

The translation and rotation calculated by the ICP algorithm are treated as a measurement vector in the update step of the Kalman filter. Update equations are presented below:

where K_k+1 is the Kalman gain matrix, z_k+1 is the measurement vector, h is the non-linear observation function, and S_k+1 is the innovation covariance matrix.

After estimating the state vector, the map is updated. Firstly, the algorithm determines which points in the cloud were registered close to the already identified shapes. If the distance is below the threshold value, points and shapes are merged. Then, the procedures of clustering and shape identification is conducted for the remaining points. The last step of a map update is to check which of the shapes can be merged with each other. For example, if two straight line segments lay close enough and their directions are similar – they can be represented as a single line segment.

3.1

Trajectory tracking

The first test was carried out to check the accuracy of a trajectory tracking. It was done by performing static, omni-directional laser scans at specific locations in a corridor. The subsequent positions of the scanner during the measurements corresponded to a rectilinear motion at a speed of approximately 5 m/s, with a scanning frequency of 10 Hz. As a result, point clouds were registered about every 0,5 m.

Figure 5 shows the result of the implemented algorithm without an active shape recognition module. The platform was moving in the positive direction of the X-axis. It can be seen that despite relatively large distances between subsequent registrations, the Kalman filter predicted the platform’s kinematic parameters accurately enough, so that the ICP algorithm was able to calculate proper approximations of displacements. The analysis of the resulting map and the motion of the sensor shows that shapes and dimensions inside the corridor are correctly reproduced and the trajectory has been correctly determined as well.

Figure 5.

Map fused from 25 consecutive scans. Trajectory is marked with red, and the subsequent poses are marked with blue stars.

An appropriate adjustment of various parameters of different system components has a significant impact on the accuracy of the process of simultaneous location and mapping. For example, a forced reduction of cloud density is often desired to minimize the amount of registered data or limit the unevenness in the spatial distribution of points. Figure 6 illustrates how the modifications in scan data preprocessing scheme could affect the mapping accuracy. After changing the desired minimal distance between points form 0.01m to 0.02m, the algorithm was longer able to achieve expected accuracy. The analysis of the map and sensor trajectory shows, that with consecutive measurements errors increased. The last part of the trajectory does not indicate movement in a straight line, and the location of the walls becomes ambiguous.

Figure 6.

The map after changes in scan data preprocessing scheme.

Another experiment was aimed at verifying the importance of using the KF. The SLAM algorithm was run without the displacement prediction step. This way, the operation of ICP algorithm information about an initial guess was examined. The results of the experiment are illustrated in Figure 7.

Figure 7.

The resulting map without KF displacement prediction.

It seems conspicuous that the lack of displacement prediction precludes correct operation of the SLAM algorithm and that using the KF is necessary.

3.2

ICP and map fusion

The next experiment was conducted to test the clustering and shapes identification algorithms as well as the efficiency of the map update process after subsequent registrations. In the figures below, various stages of a map synthesis are presented. Figure 8 shows how adjacent points are grouped into clusters and then turned into simple shapes. To increase a flexibility of the system the maximum size of point clusters was limited. Hence, there are visible differences of the adjusted clustering algorithm when comparing to the way the point cloud was processed in Figure 2.

Figure 8.

Points clustering with limited cluster size.

A potential merging into larger structures takes place after the shape identification step. In Figure 9, adjacent shapes are checked for collinearity and position on the plane. If conditions for the fusion are met, line segments are merged with line segments or L-shapes.

Figure 9.

Shapes merging into larger structures.

In Figure 9, it can be noticed that not all of the small shapes were a part of the merging process. This is due to the fact, that the criteria which manage this process must minimize the risk of mismatching the shapes as it may result in erroneous decisions about navigation and mapping.

The result of mapping of the entire corridor during the dynamic experiment, where the sensor was registering consecutive scans while moving, is shown in Figure 10. It can be seen that only the last stage, i.e. scanning the right part of the corridor, has visible errors in the data association. For most of the trajectory, the algorithm for simultaneous location and mapping works correctly. The shape of the building interior and the distances between the walls correspond to the real dimensions of the building.

Figure 10.

Map of the corridor and the trajectory of the platform.