A Depth-Based Weighted Point Cloud Registration for Indoor Scene

Abstract

Point cloud registration plays a key role in three-dimensional scene reconstruction, and determines the effect of reconstruction. The iterative closest point algorithm is widely used for point cloud registration. To improve the accuracy of point cloud registration and the convergence speed of registration error, point pairs with smaller Euclidean distances are used as the points to be registered, and the depth measurement error model and weight function are analyzed. The measurement error is taken into account in the registration process. The experimental results of different indoor scenes demonstrate that the proposed method effectively improves the registration accuracy and the convergence speed of registration error.

1. Introduction

To obtain a complete scene point cloud map, it is necessary to measure the point cloud from different positions or perspectives several times, and each measurement result is in different coordinate systems. Therefore, the coordinate system transformation should be carried out for the point clouds measured at different positions, and the same point in the point cloud measured at different positions should be transformed to the same position [1,2]. This process is called point cloud registration. For the map reconstruction of point clouds in static scenes, the coordinate transformation between point clouds can be considered as rigid transformation. Point cloud registration can be generally divided into two processes: rough registration and fine registration. Rough registration can provide initial values of optimization for fine registration. While using the point cloud reconstruction device with motion estimation functions such as inertial measurement unit or rotating table, the relative motion between point clouds can be obtained directly, so it is easy to coarse registration of point cloud. Otherwise, the transformation matrix between two point clouds should be calculated.

To reduce the influence of measurement errors on registration accuracy, a novel point cloud registration method is proposed in this paper. Firstly, the transfinite and interference points in two point clouds are filtered by preprocess. The matching relations of points between two point clouds are found by iterative closest point method. To improve the registration accuracy and speed, the point pairs with smaller Euclidean distances are used for registration, and the weights of each point pair are assigned according to the depth values. Finally, the rotation and translation matrices are calculated and the registration errors are obtained. The depth camera is used to collect environmental point clouds, and the experimental results show that the proposed method can effectively improve the registration accuracy and speed.

The major contributions of this study are as follows:

(1)

during the iterative registration process, only the point pairs with smaller Euclidean distances are used, which improves the accuracy of registration;

(2)

by incorporating the measurement error into the registration process, the weights are assigned to each point pair according to the measurement depth error and the convergence speed of registration error is improved.

The rest of this paper is organized as follows. Section 2 discusses the related work. The notations, assumptions and problem definition are given in Section 3. Section 4 presents the novel registration method for point clouds. We provide the experimental results and analysis in Section 5, and conclude the paper and discuss the future work in Section 6.

2. Related Work

On rough registration based on two-dimensional features, Lin et al. [3] proposed an efficient three-dimensional registration method based on a two-dimensional local feature matching, which is demonstrated that the method is robust to a larger difference of viewing angle. Many three-dimensional feature points can also be used for registration, such as Fast Point Feature Histograms (FPFH) features [4] and local three-dimensional feature. Yang et al. [5] proposed a registration method to provide marker-free and multi-view registration based on the semantic feature points extracted. This method mainly took advantage of the limited position and attitude of the sensor (only moving on the horizontal plane and rotating in gravity direction), and it was suitable for large-scale outdoor scene such as Terrestrial Laser Scanning (TLS). However, it needed to establish point cloud grids during the point cloud segmentation, which would increase the amount of calculation.

At present, the point cloud fine registration method used widely is the Iterative Closest Point (ICP) algorithm which was proposed by Besl et al. [6]. The basic principle is to iteratively calculate the nearest points between each point in the registration points and the fixed reference point, and the two nearest points are as matching points. The ICP algorithm can be used for registration of broken line points, curve points, surface points, dense points and triangular points. It is widely used in point cloud registration because it does not need the matching relationship between point sets and also has a faster calculation speed. However, the ICP algorithm also has the following shortcomings: the point clouds to be registered should have a large overlap area; easy to fall into local optimum; high requirements for initial values. For this reason, many variant ICP algorithms based on measurement scene and sensor type were proposed [7]. Chen et al. [8] presented an approach that works on range data directly and registers successive views with enough overlapping area to get an accurate transformation between views. It was performed by minimizing a functional that did not require point-to-point matches. Segal et al. [9] proposed a generalization of the ICP algorithm which takes into account the locally planar structure of both scans in a probabilistic model.

Another type of method for fine registration is based on probability distribution. For example, the registration method based on normal distribution transformation is not to establish the matching relationship between point clouds, but to divide the point clouds into blocks, and then use the probability distribution of these point clouds to represent the point clouds. The matching problem is transformed into the expectation maximization problem, which is used to deal with sparse point clouds from laser radar. Agamennoni et al. [10] presented a point cloud registration method based on probability distribution. Unlike the “point to point” matching registration, the method was a weighted matching registration. The transformation matrix was estimated by expectation maximization, and the accuracy of registration is significantly improved.

We proposed a point-based rigid registration method which provided different weights for different locations and directions of the measured points based on the measurement error distribution properties of laser tracker [11]. The experimental results showed that the proposed method clearly performs better for the same fiducial point number and distribution than the traditional method.

After rough and fine registrations, two point clouds are transformed into the same coordinate. Due to the accumulated error, it is necessary to do global registration and optimization for registered point clouds. In the present achievements, the influence of measurement errors on registration is seldom considered in the field of visual measurements. Because the point cloud registration is the basis of constructing a complete three-dimensional point cloud map or other subsequent processing, it is very important to consider the measurement error of point cloud in the registration process.

3. Problem Definition

The goal of this paper is to improve the ICP algorithm by using point pairs with smaller Euclidean distances for registration. At the same time, the measurement error is considered in the registration process, and the weights are assigned to each point pair according to the measurement error. The key of this paper is to establish the measurement error model

$$e=ϵ(x,y,z,\mathbf{p})$$

(1)

where $(x,y,z)$ represents the coordinate value of point and $\mathbf{p}$ contains the intrinsic parameters of depth camera. Based on the error e, the weight function affecting registration is constructed

$$w=\omega \left(e\right)$$

(2)

The amount of point pairs with smaller Euclidean distances for registration is notated as Q. The rotary and translation matrices can be calculated by

$$\mathbf{R},\mathbf{T}=f(\mathbf{Y},\mathbf{X},Q,w)$$

(3)

where $\mathbf{X}$ and $\mathbf{Y}$ are the source and target point clouds.

To focus more on verifying the feasibility of the proposed method, this study only considers the point cloud pre-processing and registration process, and makes the following assumptions:

(1)

the source and target point clouds are obtained by depth camera from two different static postures;

(2)

there is a large overlap area between the source and target point clouds.

The object of this paper is to determine the functions $ϵ(·)$, $\omega (·)$, and $f(·)$ under assumed conditions, and verify the method by experiments.

4. Method

4.1. Principle of Point Cloud Registration

In three-dimensional reconstruction, it is necessary to change the position and attitude of the camera for multiple scene acquisition, and then get the whole three-dimensional point cloud of the target scene by point cloud registration and fusion. Therefore, each point cloud is measured in different local coordinate systems. The main goal of this paper is to improve the registration accuracy of two point clouds.

The origin of the local coordinate system is located at the center of the camera, which is notated as $o_c{x}_c{y}_c{z}_c$. Point cloud registration is to transform point clouds in different coordinate systems into the same coordinate system, and ensure that the same point in different point clouds can coincide as much as possible after registration. The flow chart of point clouds registration is shown in Figure 1.

Figure 1. The flow chart of point clouds registration.

The goal of point cloud registration is to transform local point clouds into the global coordinate system.

$${\left\{{\mathbf{X}}^{\prime }\right\}}_g={\mathbf{H}}_g^c{\left\{\mathbf{X}\right\}}_c$$

(4)

where ${\left\{\mathbf{X}\right\}}_c={\{X_1,X_2,\dots ,X_N\}}_c$ represents the source point cloud in local coordinate system with N points to be registered, ${\left\{{\mathbf{X}}^{\prime }\right\}}_g$ is the registered point cloud in global coordinate system. The target point cloud to be registered can be either a registered point cloud or a point cloud fused by the previous measurements in global coordinate system.

The target point cloud is notated as $\left\{\mathbf{Y}\right\}$ and the source point cloud is notated as $\left\{\mathbf{X}\right\}$. The key to registration is to calculate the transformation matrix ${\mathbf{H}}_g^c$, and minimize the error function

$${ϵ}_r=\sum _{i=1}^N{\parallel Y_i-{\mathbf{H}}_g^c{X}_i\parallel }^2$$

(5)

where $Y_i$ is one point of target point cloud in global coordinate system which is matched to $X_i$.

For point cloud registration in static scenes, it can be considered that different point clouds satisfy the rigid transformation. Therefore, the Formula (5) can be written as

$${ϵ}_r=\sum _{i=1}^N{\parallel Y_i-{\mathbf{R}}_g^c{X}_i-{\mathbf{T}}_g^c\parallel }^2$$

(6)

where ${\mathbf{R}}_g^c$ and ${\mathbf{T}}_g^c$ are the rotation and translation matrices of rigid body transformation.

There are two main issues in solving Formula (6): (1) finding the point $Y_i$ in target point cloud $\left\{{\mathbf{Y}}_i\right\}$ which matches the point $X_i$; (2) optimization solution.

Assuming that a matching relationship has been found, it can be solved as follows:

(1)

calculate the centers of target point cloud and point cloud to be registered

$$\begin{array}{c}\hfill \overline{X}=\frac{1}{N}\sum _{i=1}^N{X}_i\\ \hfill \overline{Y}=\frac{1}{N}\sum _{i=1}^N{Y}_i\end{array}$$

(7)

(2)

calculate the covariance matrix

$$\Sigma =\frac{1}{N}\left[(\mathbf{Y}-\overline{Y}){(\mathbf{X}-\overline{X})}^T\right]$$

(8)

(3)

construct symmetric matrix

$$Q(\Sigma )=\left[\begin{array}{cc}tr(\Sigma )& {\Delta }^T\\ \Delta & \Sigma +{\Sigma }^T-tr(\Sigma ){\mathbf{I}}_{3\times 3}\end{array}\right]$$

(9)

where $\Delta =[{(\Sigma -{\Sigma }^T)}_{23},{(\Sigma -{\Sigma }^T)}_{31},{(\Sigma -{\Sigma }^T)}_{12}]$

(4)

perform eigenvalue decomposition of matrix $Q(\Sigma )$, and the quaternion ${\mathbf{v}}_g^c$ corresponding to the rotation matrix ${\mathbf{R}}_g^c$ is obtained by taking the eigenvector corresponding to the maximum eigenvalue.

$${\mathbf{v}}_g^c=\mathcal{Q}\left({\mathbf{R}}_g^c\right)$$

(10)

The translation matrix is

$$\mathbf{T}=\overline{Y}-{\mathbf{R}}_g^c\overline{X}$$

(11)

4.2. Depth Measurement Error Model

To improve the 3D point cloud modeling of complex indoor environment, a novel point cloud registration method based on Intel${}^{\circledR }$ RealSense${}^{\mathrm{TM}}$ depth camera is proposed in this paper, which can reduce the influence of measuring errors. The Intel${}^{\circledR }$ RealSense${}^{\mathrm{TM}}$ depth camera adopts active infrared (IR) stereo vision technology, which is shown in Figure 2. The depth perception based on stereo vision is implemented by two image sensors and an infrared projector. The infrared projector projects non-visible structured IR pattern to improve depth accuracy in scenes. The depth calculation process in presented in the right of Figure 2. The depth image processor obtains the scene data by the two image sensors, and the depth values for each pixel can be calculated by correlating the points on the left image to the right image.

Figure 2. The depth perception based on active infrared stereo vision technology.

In fact, there will be measurement error in depth camera. If the measurement error is not considered, the original error will be accumulated and enlarged when the depth map or point cloud is applied, resulting in poor results.

To analyse and calculate the depth error, the symbols for camera parameters are defined as follows: the depth between the origin of camera and the object is z, the size of subpixel is $p_s$, the focal length is $l_f$, and the baseline of two cameras is $l_b$. The horizontal resolution is $r_X$, and the horizontal field of view is ${\theta }_{HFOV}$. The RMS error represents the depth noise for a localized plane fit to the depth values and is defined as [12]

$$e=\frac{z^2{p}_s}{l_f{l}_b}$$

(12)

where the focal length is

$$l_f=\frac{r_X}{2tan\left(\frac{{\theta }_{HFOV}}{2}\right)}$$

(13)

It can be concluded that the depth error is proportional to the square of the depth value.

As described in Section 4.1, in general ICP algorithms, the weights of different pairs of points are equal when the registration error is minimized. In fact, as the depth increases, the measurement error will increase and the credibility of measurement result will also change. To quantify the impact of errors on measurement results, the scalar $\eta$ of relative error is defined and simplified as

$$\eta =\frac{e}{z}=z\times \frac{2p_{s}tan\left(\frac{{\theta }_{HFOV}}{2}\right)}{r_X{l}_b}$$

(14)

When the point cloud registration error is minimized, the weights of different point pairs are defined as w

$$w=\frac{1}{\eta }$$

(15)

4.3. Depth-Based Weighted ICP Registration

Section 4.1 assumes that the matching relations of two point clouds to be registered are known, and the transformation matrix can be calculated. However, for two point clouds with large amount of points, it is difficult to find point pairs between them. The classic ICP algorithm is to calculate rotation and translation matrices of two point clouds by finding closest point pairs iteratively.

During iterative closest points, all the points in source point cloud are used to match points in target point cloud. Then the distances between point $X_i$ in source point cloud and all points $\left\{\mathbf{Y}\right\}$ in target point cloud are calculated. The point corresponding to minimum distance (nearest neighbor) is taken as the matching point of $X_i$.

$${\dot{Y}}_i=\underset{Y_i\in \left\{\mathbf{Y}\right\}}{argmin}{\parallel Y_i-X_i\parallel }^2$$

(16)

Using Formula (16), a new target point cloud $\{\dot{\mathbf{Y}}\}$ that matches all the points in the source cloud can be obtained. In fact, the two point clouds obtained in different positions and attitudes cannot completely coincide. For more accurate registration, we choose $Q(Q<N)$ point pairs with smaller Euclidean distances as the object point clouds to calculate the transformation matrix and registration error. The Q is generally the number of overlap points of two point clouds. The number is difficult to be calculated accurately, so it can use an approximate value.

$$\begin{array}{cc}\hfill {\mathbf{X}}^{\prime }& =\underset{Q}{argmin}\{\mathbf{X}\}\hfill \\ \hfill {\mathbf{Y}}^{\prime }& =\underset{Q}{argmin}\{\dot{\mathbf{Y}}\}\hfill \end{array}$$

(17)

Based on new target and source point clouds ${\mathbf{Y}}^{\prime }$ and ${\mathbf{X}}^{\prime }$, the transformation matrix ${\mathbf{H}}_g^c$ can be calculated by Formulas (7)–(11).

$${\mathbf{H}}_g^c=argmin{ϵ}_r$$

(18)

The coordinate transformation for source point cloud is performed by Formula (4), and the registration error is calculated by Formula (6). If the registration error changes less than the pre-set threshold ${ϵ}_{TH}$ or the number of iterations is greater than the pre-set threshold $N_{TH}$, the algorithm terminates; otherwise the iteration will continue.

The above is the procedure of general ICP algorithm, and the weights of different point pairs are equal while minimizing the registration error. To reduce the influence of measurement errors on point cloud registration results, the depth error should be incorporated into the registration process by weighting. Therefore, different weights can be assigned to different point pairs according to the depth errors, and the registration accuracy can be improved during the least square optimization process. The error function can be rewritten as follows:

$$ϵ=\sum _{i=1}^Q{w}_i{\parallel Y_i^{\prime }-{\mathbf{R}}_g^c{X}_i^{\prime }-{\mathbf{T}}_g^c\parallel }^2=\sum _{i=1}^Q\frac{1}{z\times \frac{2p_{s}tan\left(\frac{{\theta }_{HFOV}}{2}\right)}{r_X{l}_b}}{\parallel Y_i^{\prime }-{\mathbf{R}}_g^c{X}_i^{\prime }-{\mathbf{T}}_g^c\parallel }^2$$

(19)

By continuously minimizing the objective error function $ϵ$, the optimal rotation and translation matrices can be obtained to improve the registration accuracy of point clouds.

5. Experiments and Results

To improve the accuracy of point cloud registration in complex indoor scenes, this paper proposed a novel point cloud registration method by reducing the number of point pairs and incorporating the measurement errors into the registration process. The Section 4 presents the detailed depth error model and registration method. This section will verify the effectiveness of the proposed method through multiple scene registration experiments and result analysis.

5.1. Depth Camera

In this experiment, the point cloud data is obtained by Intel${}^{\circledR }$ RealSense${}^{\mathrm{TM}}$ D415 depth camera. As shown in Section 4.2, the depth camera adopts active infrared stereo vision technology to measure the depth of the environment. It consists of two image sensors, an IR projector, and a RGB sensor, which is shown in Figure 3. The key specifications of depth camera is presented in Table 1.

Figure 3. Intel${}^{\circledR }$ RealSense${}^{\mathrm{TM}}$ D415 depth camera.

Table 1. The key specifications of Intel${}^{\circledR }$ RealSense™ D415.

Item	Value
Depth technology	Active IR stereo
Depth Stream Output Resolution	1280 × 720
Depth Stream Output Frame Rate	30 fps
Minimum Depth Distance (Min-Z)	0.3 m
Maximum Range	Approx. 10 m
Depth Field of View (FOV)	69.4${}^{\circ }$ × 42.5${}^{\circ }$ × 77${}^{\circ }$ (±3${}^{\circ }$)
Baseline	55 mm
Subpixel	0.08

To verify the effectiveness of proposed method, the parameters of the depth camera are determined according to the experimental requirements: $l_b=55$ mm, ${\theta }_{HFOV}={65}^{\circ }$, $r_X=1280$, $p_s=0.08$. Therefore, the depth error e and scalar $\eta$ are calculated as follows:

$$\begin{array}{cc}\hfill e& =0.0014479\times z^2\hfill \\ \hfill \eta & =0.0014479\times z\hfill \end{array}$$

(20)

The depth RMS error and relative error are shown in Figure 4. It can be seen that the absolute RMS error of the measured value is a quadratic function of the measured depth z, and the relative error has a linear relationship with the measured depth z.

Figure 4. The depth RMS error and relative error.

5.2. Registration Results

To verify the effectiveness of the proposed method, we complete the experiments of point cloud acquisition and registration in four different indoor scenes. In each scene, four different methods of registration are implemented: (A) original ICP algorithm; (B) ICP algorithm with weighting; (C) ICP algorithm with Q point pairs, (D) ICP algorithm weighted by Q point pairs, where Q is determined as 0.8 times the number of target point clouds. The experimental results are shown in Figure 5. To show the scene represented by point cloud, the RGB images of target and source point clouds are also presented. The registration RMS error in Method D is calculated by the square root of $ϵ$, where $ϵ$ is calculated by Formula (19).

Figure 5. Comparison of point cloud registration results with different methods. Method A: original ICP algorithm, Method B: ICP algorithm with weighting, Method C: ICP algorithm with Q point pairs, Method D: ICP algorithm weighted by Q point pairs.

According to the registration results, the registration errors of the four methods in four different scenes are all convergent. Method A and Method B use all point pairs to calculate the rotation and translation matrices, but the weights of each pair of points between two methods are different. In Method A, the weights of all point pairs are equal, and in Method B, the greater the measurement error, the smaller the weight. It can be seen that Method B has smaller errors and higher registration accuracy which considers the effect of measurement errors than Method A. Similarly, when Q point pairs are used for registration, such as Method C and Method D, the registration results are also better when the measurement errors are taken into account. The results show that the measurement errors have an impact on the speed and accuracy of registration. When different numbers of point pairs are used for registration, the accuracy of registration is quite different. The registration results of Method C and Method D are obviously better than those of Method A and Method B. Therefore, the ICP algorithm weighted by Q point pairs are proposed in this paper, which can effectively improve the convergence speed of registration error and reduce the registration error.

6. Conclusions

Aiming at the influence of the number of registration points and measurement errors on the registration results, an improved ICP registration method is proposed in this paper. During the iterative registration process, only the point pairs with smaller Euclidean distances are used, which improves the accuracy of registration. To reduce the influence of measurement errors on registration result, the depth measurement error model based on Intel${}^{\circledR }$ RealSense${}^{\mathrm{TM}}$ depth camera is analyzed, and an ICP registration method with error weights is constructed to improve the convergence speed of registration errors. The registration experiments in different indoor scenes demonstrate the effectiveness of the proposed improved ICP algorithm. The effects of measurement error and Euclidean distance of point pairs on registration result are studied in this paper. In future work, point cloud registration based on 4D modeling will be considered, which will make full use of the environmental features, and may improve the registration accuracy and speed [13,14,15].