Multi-sensor fusion three-dimensional point cloud noise reduction method and system and acquisition equipment

Abstract

The invention discloses a multi-sensor fusion three-dimensional point cloud noise reduction method and system and acquisition equipment, and relates to the technical field of three-dimensional map construction. The method comprises the following steps: receiving a three-dimensional point cloud map of a laser original point cloud and a camera original point cloud, and carrying out point cloud registration; and carrying out continuous interpolation processing on the laser original point cloud, and calculating to obtain a laser dense point cloud. The sparse laser point cloud obtained by vertical scanning of the single-line laser radar is subjected to clustering segmentation and nearest neighbor interpolation processing, a dense single-line laser radar point cloud model is obtained, and continuous reference data is provided for data fusion of a depth camera and the single-line laser radar. A two-stage random forest regression method combining point cloud space coordinates and point cloud PFH descriptors is adopted, coarse-to-fine space prediction from noise point cloud of the depth camera to reference point cloud is achieved, the noise level of the depth camera is reduced, and meanwhile the overall three-dimensional map construction precision of the system is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of three-dimensional map construction, and specifically to a three-dimensional point cloud denoising method, system and acquisition device for multi-sensor fusion. Background Art

[0002] Camera sensors are widely used in three-dimensional map construction due to their advantages of rich information acquisition and flexible measurement. However, in complex underground environments, camera sensors are easily affected by factors such as lighting, dust and fog, and lack of surface texture, resulting in large measurement noise, which seriously affects the accuracy of three-dimensional reconstruction. Therefore, it is necessary to use additional sensors to make up for the measurement limitations of the measurement system composed of a single camera sensor in complex environments, improve the anti-interference performance of the measurement system, and achieve low-noise and high-precision three-dimensional map construction.

[0003] Among current multi-sensor fusion methods, the most commonly used sensor fusion type is the combined measurement of a three-dimensional lidar and a camera sensor. For example, the existing patent (application number: CN202110538817.4) discloses a three-dimensional reconstruction method for vehicles based on multi-sensor fusion. This method uses a measurement system that fuses a camera and a three-dimensional lidar for three-dimensional construction of vehicles. Although this combined measurement method of a three-dimensional lidar and a camera sensor can improve the measurement accuracy to a certain extent, there are still two problems: one is the problem of data redundancy between the camera and the three-dimensional lidar; the other is that the fusion method between multi-sensor data does not make full use of the advantages of different sensors, and the degree of point cloud denoising is not high enough. Therefore, the present invention proposes a three-dimensional point cloud denoising method, system and acquisition device for multi-sensor fusion. Summary of the Invention

[0004] The purpose of the present invention is to provide a three-dimensional point cloud denoising method, system and acquisition device for multi-sensor fusion, which can fully fuse the sparse and low-noise laser point cloud obtained by vertical scanning of a single-line lidar with the high-noise and dense color point cloud obtained by a depth camera, reduce three-dimensional measurement noise, and thus improve the three-dimensional reconstruction accuracy of underground spaces.

[0005] According to the first aspect of the present invention, to achieve the above object, the present invention provides the following technical solution: A three-dimensional point cloud denoising method for multi-sensor fusion, comprising the following steps:

[0006] Receiving the three-dimensional point cloud map of the original laser point cloud L 1 and the original camera point cloud D 1 and performing point cloud registration;

[0007] Performing continuous interpolation processing on the original laser point cloud L 1 to calculate and obtain the dense laser point cloud L 2 ;

[0008] Input the laser dense point cloud L 2 and the original camera point cloud D 1 into the pre - constructed random forest regression model for preliminary point cloud prediction, obtaining the preliminarily denoised original camera point cloud D 2 ;

[0009] Calculate the feature vectors composed of PFH descriptors and spatial coordinates of the laser dense point cloud L 2 and the preliminarily denoised original camera point cloud D 2 , and import them into the random forest regression model again for secondary prediction, obtaining the finely denoised camera point cloud D 3 .

[0010] Furthermore, the original laser point cloud L 1 is acquired by a single - line lidar, and the original camera point cloud D 1 is acquired by a depth camera.

[0011] Furthermore, receive the three - dimensional point cloud map of the original laser point cloud L 1 and the original camera point cloud D 1 and perform point cloud registration, specifically as follows:

[0012] (31) Use the depth camera to collect the RGB and Depth images inside the target component, and combine the ORB - SLAM3 algorithm for feature extraction, pose estimation, and dense mapping to obtain the original camera point cloud D 1 ;

[0013] (32) Use the single - line lidar to collect multiple frames of two - dimensional laser data inside the target component. Use the Hector - SLAM algorithm to convert the distance and angle information contained in each frame of laser scan data into three - dimensional point cloud data. For each laser point, calculate the x and y coordinates of the point according to the distance and angle information, and set the Z coordinate to a consistent initial value;

[0014] The initial three - dimensional coordinates of each frame of point cloud can be expressed as:

[0015] L i ={(x i,j ,y i,j ,z 0 )|j = 1,2,...,n i}

[0016] where z 0 is the initial value of the Z coordinate, and n i is the number of points in each frame of laser data;

[0017] (33) Calculate the mileage of each frame of lidar point cloud based on the angle information recorded by the angle encoder and the lead of the lead screw, and update the Z coordinate to a dynamic value that changes with time and mileage. The update formula for the Z coordinate can be directly expressed as:

[0018]

[0019] where Δzi is the change in the Z coordinate corresponding to each frame of lidar point cloud, p is the lead of the lead screw, and θi is the angle data of the i-th frame recorded by the angle encoder;

[0020] (34) According to the feedback data of the angle encoder, update the Z coordinate of each frame of lidar scan point cloud to reflect the actual horizontal displacement when the single-line lidar scans inside the target component. Finally, stack each frame of lidar point cloud in sequence according to the calculated mileage information to form the complete original lidar point cloud L 1 ;

[0021] (35) Through the ICP registration algorithm, use its optimization objective function to minimize the corresponding point error between the original camera point cloud D 1 and the original lidar point cloud L 1 . The optimization objective function is:

[0022]

[0023] where R is the rotation matrix and T is the translation vector, and respectively represent the corresponding points in the depth camera and lidar point clouds. By iteratively optimizing R and T, the coincidence degree between the original camera point cloud D 1 and the original lidar point cloud L 1 is maximized to achieve point cloud registration.

[0024] Further, perform continuous interpolation processing on the original lidar point cloud L 1 to calculate the dense lidar point cloud L 2 , as follows:

[0025] (41) Perform clustering segmentation on the original lidar point cloud L 1 using the density-based spatial clustering algorithm. By specifying the minimum neighborhood radius ∈ and the minimum number of points P min , determine whether a point is a core point, boundary point, or noise point in the original lidar point cloud L 1 . If the neighborhood of a point contains at least P min points, then this point is a core point, and all points in its neighborhood belong to the same cluster; by continuously expanding the neighborhood of the core point, finally achieve the clustering segmentation of the original lidar point cloud L 1 ; The condition for the core point is:

[0026] |{q ∈ P | ||p, q|| ≤ δ}| ≥ P min ;

[0027] Wherein, ||p, q|| represents the distance between points p and q, and P is a point cloud data set;

[0028] (42) For the original laser point cloud L 1 The layer point cloud segmented by clustering in it, starting from the first layer point cloud, traverse each point and find its nearest neighbor point in the next layer; Let P 1 and P 2 be the point cloud data of the first layer and the second layer respectively, containing i and j points. For each point p 1i in the first layer, find the corresponding nearest neighbor point p 2j in the second layer;

[0029] The definition of the nearest neighbor point is as follows:

[0030] j = argmin k Pp 1i - p 2j P

[0031] Wherein, Pp 1i - p 2j P represents the Euclidean distance between points p 1i p 2j ;

[0032] (43) In this way, all pairs of nearest neighbor points (p 1i , p 2j ) between adjacent layer point clouds can be found. Linear interpolation is performed on all pairs of nearest neighbor points of adjacent layer point clouds to obtain a dense and continuous single-line lidar point cloud. Let p 1i = (x 1i , y 1i , z 1i ) and p 2j = (x 2j , y 2j , z 2j ) be a pair of nearest neighbor points. Linear point cloud interpolation is performed between the two points to generate a new point p m :

[0033] p m (t) = (1 - t) · p 1i + t · p 2j , t ∈ [0, 1]

[0034] Wherein t is an interpolation parameter; t is taken at equal intervals from 0 to 1 to obtain an interpolation point cloud set P interp :

[0035]

[0036] where N t is the number of interpolation points; interpolation operations are performed on the nearest neighbor point pairs of all adjacent layers to obtain the laser dense point cloud L 2 .

[0037] Furthermore, the laser dense point cloud L 2 and the original camera point cloud D 1 are input into a pre-constructed random forest regression model for preliminary point cloud prediction to obtain the preliminarily denoised original camera point cloud D 2 , specifically as follows:

[0038] (51) Use all the point cloud spatial coordinate data in the laser dense point cloud L 2 as the training data set X one , and all the point cloud spatial coordinate data in the original camera point cloud D 1 as the target data Y one :

[0039]

[0040] Randomly sample B times with replacement from X one =(P l1 , P l2 , …, P lm ), with the number of samples per sampling being m. Each sampling will generate the b-th training subset as X one-b =(P l1-b , P l2-b , …, P lm-b ), and simultaneously generate a target subset Y one-b =(P d1-b , P d2-b , …, P dn-b ) with the number of samples being n;

[0041] (52) Use each subset pair (X one-b , Y one-b ) composed of a training subset and a target subset to train each decision tree T one-b . In each decision tree, node splitting is performed by selecting the optimal feature and splitting point to minimize the mean squared error:

[0042] For the i-th splitting node z one-i , select the feature j one-i and the splitting point s one-i to minimize the objective function, and the objective function is:

[0043]

[0044] where yi is the predicted value of the camera point cloud in the i-th splitting node, R left1 , R right1 respectively represent the sample sets of the left and right child nodes based on the point cloud spatial feature j 1 and the splitting point s 1 ; are respectively the means of the camera raw point cloud data Y one-b in these sample sets; Repeat the process of step (52) B times to generate B decision trees (T one-1 , T one-2 , …, T one-B ) for the first-stage prediction process;

[0045] (53) When predicting the camera raw point cloud D 1 , each decision tree T b will give a predicted value. For the i-th point in the camera raw point cloud D 1 , the prediction result of the b-th decision tree is expressed as The final result is the average of the predicted values of all decision trees:

[0046]

[0047] where is the final predicted coordinate in the first stage, and B is the total number of decision trees;

[0048] (54) The prediction process of the random forest regression model uses the mean squared error as the loss function to measure the error between the predicted point cloud and the true point cloud. The loss function is:

[0049]

[0050] where is the average of the predicted values of all decision trees for the i-th point in the camera raw point cloud D 1 after the first-stage prediction, is the corresponding reference point in the laser dense point cloud L 2 for the i-th point in the camera raw point cloud D 1 ;

[0051] (54) Finally, the predicted points in all the optimized camera raw point clouds D 1 are merged to obtain the preliminary denoised camera point cloud D 2 .

[0052] Furthermore, calculate the laser dense point cloud L 2 and the preliminarily denoised camera raw point cloud D 2The feature vector composed of the PFH descriptor and spatial coordinates is imported into the random forest regression model again for secondary prediction to obtain the camera fine denoised point cloud D 3 , as follows:

[0053] (61) Calculate the laser dense point cloud L 2 for each point in the PFH descriptor of 2 the PFH descriptor of the camera pre-denoised point cloud D Combine and with the corresponding L 2 and D 2 in the point cloud coordinates to obtain the laser point cloud feature vector and the camera point cloud feature vector

[0054] (62) Using the camera point cloud feature vector as the input label and the laser point cloud feature vector as the output label, train the random forest regression model, establish the mapping relationship from the input label to the output label, and perform model training based on this relationship; in the process of training the decision tree of the random forest regression model, each node selects the best combination feature j of the point cloud coordinates and the PFH descriptor two-i and the split point s two-i , so that the mean square error on the left and right child nodes is minimized. The objective function for node splitting is:

[0055]

[0056] where R left2 , R right2 respectively represent the sample sets of the left and right child nodes based on the point cloud spatial feature j two-i and the split point s two-i , are the means in the left and right child node sample sets respectively. Repeat the process of step (62) C times to generate C decision trees (T two-1 , T two-2 ,…, T two-C ) for the second-stage prediction process;

[0057] (63) The prediction process of the random forest regression model in the second stage also uses the mean square error as the loss function to measure the error between the predicted point cloud and the real point cloud. By minimizing the loss function, the model is continuously optimized to reduce the error between the prediction result and the real value. The loss function is:

[0058]

[0059] where is the initially denoised point cloud D of the camera 2 is the average value of the predicted values of all decision trees for the i-th point after the second-stage prediction, is found in the laser-dense point cloud L 2 the corresponding reference point of the i-th point in the initially denoised point cloud D of the camera 2 ;

[0060] (64) Merge the predicted points in all the initially denoised point clouds D of the camera 2 that have been optimized to obtain the finely denoised point cloud D of the camera 3 .

[0061] According to the second aspect of the present invention, the present invention provides a multi-sensor fusion three-dimensional point cloud denoising system for implementing the above multi-sensor fusion three-dimensional point cloud denoising method, including:

[0062] A receiving module for receiving the three-dimensional point cloud maps of the laser raw point cloud L 1 and the camera raw point cloud D 1 and performing point cloud registration;

[0063] A calculation module for performing continuous interpolation processing on the laser raw point cloud L 1 to calculate and obtain the laser-dense point cloud L 2 ;

[0064] A preliminary prediction module for inputting the laser-dense point cloud L 2 and the camera raw point cloud D 1 into a pre-constructed random forest regression model for preliminary point cloud prediction to obtain the initially denoised camera raw point cloud D 2 ;

[0065] A secondary prediction module for calculating the feature vectors composed of the PFH descriptors and spatial coordinates of the laser-dense point cloud L 2 and the initially denoised camera raw point cloud D 2 and importing them again into the random forest regression model for secondary prediction to obtain the finely denoised point cloud D of the camera 3 .

[0066] According to the third aspect of the present invention, the present invention provides a collection device for collecting the above laser raw point cloud and camera raw point cloud, including a rectangular pipeline, a single-line lidar, a depth camera, a sensor bracket, a guide rail slider, a lead screw, an angle encoder, a stepping motor, a power supply, a motor controller, a motor driver, and an industrial computer;

[0067] The lead screw is installed at the bottom of the inner wall of the rectangular pipeline. The guide rail slider is threadedly connected to the lead screw. The sensor bracket is installed on the guide rail slider. The depth camera is horizontally installed on the sensor bracket by threads. The single-line lidar is vertically installed on the sensor bracket by threads. The stepping motor is fixedly connected to the head end of the lead screw through a coupling. The angle encoder is fixedly connected to the tail end of the lead screw through a coupling. The power supply, motor controller, motor driver, and industrial computer are all fixed on the surface of the platform extending from the outside of the rectangular pipeline.

[0068] Furthermore, an installation frame is rotatably connected to the end of the lead screw. A guide block is fixedly connected to the bottom of the guide rail slider. A guide groove adapted to the guide block is formed on the side wall of the installation frame.

[0069] Furthermore, the stepping motor is fixedly installed on the installation frame.

[0070] The present invention has at least the following beneficial effects:

[0071] (1) By performing clustering segmentation and nearest neighbor interpolation processing on the sparse laser point cloud obtained by the vertical scanning of the single-line lidar, the present invention obtains a dense single-line lidar point cloud model, providing continuous reference data for the data fusion of the depth camera and the single-line lidar. And a two-stage random forest regression method combining the point cloud spatial coordinates and the point cloud PFH descriptor is adopted to realize the coarse-to-fine spatial prediction of the depth camera noise point cloud to the reference point cloud, reducing the noise level of the depth camera and improving the overall three-dimensional map construction accuracy of the system at the same time.

[0072] (2) The present invention adopts the combined measurement method of horizontal scanning of the depth camera and vertical scanning of the single-line lidar, which can fully realize the complementary measurement fields of view of the sensors and effectively avoid data overlap and redundancy between different sensors.

[0073] Of course, it is not necessary for any product implementing the present invention to simultaneously achieve all the above-mentioned advantages. Description of the Drawings

[0074] Figure 1 is a three-dimensional schematic diagram of the acquisition device in Embodiment 3 of the present invention;

[0075] Figure 2 is a flow schematic diagram of the method in Embodiment 1 of the present invention;

[0076] Figure 3 is the original laser point cloud diagram in Embodiment 1 of the present invention;

[0077] Figure 4 is the original camera point cloud diagram in Embodiment 1 of the present invention;

[0078] Figure 5 is the dense laser point cloud diagram in Embodiment 1 of the present invention;

[0079] Figure 6 is the initial noise reduction point cloud map of the camera in the first embodiment of the present invention;

[0080] Figure 7 is the fine noise reduction point cloud map of the camera in the first embodiment of the present invention.

[0081] Reference numerals:

[0082] 1, rectangular duct; 2, single-line lidar; 3, depth camera; 4, sensor bracket; 5, guide rail slider; 6, lead screw; 7, angle encoder; 8, stepper motor; 9, power supply; 10, motor controller; 11, motor driver; 12, industrial computer. Detailed implementation manners

[0083] Next, the technical solutions in the embodiments of the present disclosure will be clearly and completely described with reference to the accompanying drawings in the embodiments of the present disclosure. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, rather than all the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present disclosure.

[0084] Embodiment 1:

[0085] Please refer to Figure 1 and Figure 2 , the present invention provides a technical solution: a three-dimensional point cloud noise reduction method for multi-sensor fusion, which operates on the rectangular duct 1 as the target component, and includes the following steps:

[0086] S1. Use the single-line lidar 2 and the depth camera 3 to scan the inside of the rectangular duct 1 to obtain the three-dimensional point cloud map of the laser raw point cloud L 1 and the original camera raw point cloud D 1 , and perform point cloud registration, specifically as follows:

[0087] S11: First, start the stepper motor 8 to drive the lead screw 6, drive the depth camera 3 and the single-line lidar 2 installed on the sensor bracket 4 to scan inside the rectangular duct 1. During this process, the depth camera 3 collects the RGB and Depth images inside the rectangular duct 1, and combines the ORB-SLAM3 algorithm for feature extraction, pose estimation, and dense mapping to obtain the camera raw point cloud D 1 , Figure 4 in (a), Figure 4 in (b), and Figure 4 in (c) are respectively the stereo view, side view, and front view of the camera raw point cloud D 1 ;

[0088] S12: The single-line lidar 2 collects multiple frames of two-dimensional laser data inside the rectangular pipeline under the drive of the lead screw 6. The Hector-SLAM algorithm is used to convert the distance and angle information contained in each frame of laser scan data into three-dimensional point cloud data. For each laser point, the x and y coordinates of the point are calculated according to the distance and angle information, and the Z coordinate is set to a consistent initial value; the initial three-dimensional coordinates of each frame of point cloud can be expressed as:

[0089] L i ={(x i,j ,y i,j ,z 0 )|j=1,2,...,n i};

[0090] where z 0 is the initial value of the Z coordinate, and n i is the number of points in each frame of laser data;

[0091] S13: Calculate the odometry of each frame of laser point cloud through the angle information recorded by the angle encoder 7 and the lead of the lead screw 6, and update the Z coordinate to a dynamic value that changes with time and odometry. The scanning frequency of the single-line lidar is 10Hz, and the recording frequency of the angle encoder 7 is set to be the same as that of the single-line lidar 2. Therefore, the update formula of the Z coordinate can be directly expressed as:

[0092]

[0093] where Δzi is the change in the Z coordinate corresponding to each frame of laser point cloud, p is the lead of the lead screw, and θi is the angle data of the i-th frame recorded by the angle encoder; according to the feedback data of the angle encoder 7, update the Z coordinate of each frame of laser scan point cloud to reflect the actual horizontal displacement of the single-line lidar 2 when scanning inside the rectangular pipeline 1; finally, stack each frame of laser point cloud in sequence according to the calculated odometry information to form the complete original laser point cloud L 1 , Figure 3 in (a), Figure 3 in (b) and Figure 3 in (c) are the three-dimensional view, side view and front view of the original laser point cloud L 1 respectively;

[0094] S14: Through the ICP registration algorithm, use its optimization objective function to minimize the corresponding point error between the original camera point cloud D 1 and the original laser point cloud L 1 . The optimization objective function is:

[0095]

[0096] where R is the rotation matrix and T is the translation vector, and respectively represent the corresponding points in the depth camera and the lidar point cloud; by iteratively optimizing R and T, the coincidence degree of the two groups of point clouds is maximized to achieve accurate point cloud registration;

[0097] S2. Perform continuous interpolation processing on the original lidar point cloud L of the rectangular pipeline 1 constructed by the single-line lidar 2 1 to obtain the dense lidar point cloud L 2 , specifically as follows:

[0098] As Figure 2 , Figure 4 , Figure 5 and Figure 6 shown, S3 is specifically:

[0099] S21: First, perform clustering segmentation processing on the original lidar point cloud L obtained in step S1 1 , using the density-based spatial clustering algorithm, by specifying the minimum neighborhood radius ∈ and the minimum number of points P min to determine whether a point is a core point, a boundary point or a noise point in the original lidar point cloud L 1 . If the neighborhood of a point contains at least P min points, then this point is a core point, and all points in its neighborhood belong to the same cluster; by continuously expanding the neighborhood of the core point, the clustering segmentation of the original lidar point cloud L 1 is finally realized; the condition for the core point is:

[0100] ∣{q∈P∣dist(p,q)≤ò}∣≥P min ;

[0101] where dist(p,q) represents the distance between point p and q, and P is the point cloud data set;

[0102] S22: For the layer point cloud segmented from the original lidar point cloud L 1 , starting from the first layer point cloud, traverse each point and find its nearest neighbor point in the next layer; let P 1 and P 2 be the point cloud data of the first layer and the second layer respectively, containing i and j points. For each point p 1i in the first layer, find the corresponding nearest neighbor point p 2j in the second layer; the definition of the nearest neighbor point is:

[0103] j=argmin k Pp 1i -p 2j P;

[0104] where, Pp 1i -p2j P represents point p 1i p 2j The Euclidean distance between them; in this way, all the nearest neighbor point pairs between adjacent layer point clouds can be found (p 1i , p 2j ); perform linear interpolation on all the nearest neighbor point pairs of adjacent layer point clouds to obtain a dense and continuous single-line lidar point cloud. Let p 1i =(x 1i , y 1i , z 1i ) and p 2j =(x 2j , y 2j , z 2j ) be a pair of nearest neighbor point pairs, perform linear point cloud interpolation between the two points to generate a new point p m ,

[0105] p m (t)=(1 - t)·p 1i +t·p 2j , t ∈ [0, 1]

[0106] where t is the interpolation parameter; take equally spaced values of t from 0 to 1 to obtain the interpolated point cloud set P interp :

[0107]

[0108] where N t is the number of interpolation points; perform the above interpolation operation on all the nearest neighbor point pairs of adjacent layers, and finally obtain the dense lidar point cloud L 2 , which provides benchmark data support for the subsequent regression model, Figure 5 in (a), Figure 5 in (b) and Figure 5 in (c) are the three-dimensional view, side view and front view of the dense lidar point cloud L 2 respectively;

[0109] S3. Import the dense lidar point cloud L 2 and the original camera point cloud D 1 into the random forest regression model for preliminary point cloud prediction to obtain the preliminarily denoised original camera point cloud D 2 ;

[0110] As Figure 2 , Figure 4 , Figure 5 and Figure 6 shown, in S3, the dense lidar point cloud L 2 is used as the benchmark data, and the original camera point cloud D 1As the target data to be predicted, both are imported into the random forest regression model, and by learning the spatial distribution characteristics of the dense laser point cloud L 2 , the redistribution of the original camera point cloud D 1 in the same spatial coordinate system is initially predicted, so as to reduce the original camera point cloud D 1 An association of the point cloud spatial coordinate mapping between the two is established, and the regression model is trained accordingly. This process uses the continuous point cloud model of a single-line lidar as the training data, and the regression model is trained through its spatial coordinate characteristics, specifically as follows:

[0111] S31: All the point cloud spatial coordinate data in the dense laser point cloud L 2 are used as the training data set X one , and all the point cloud spatial coordinate data in the original camera point cloud D 1 are used as the target data Y one :

[0112]

[0113] Randomly sample B times with replacement from X one =(P l1 ,P l2 ,…,P lm ), and the number of samples for each sampling is m; the b-th training subset generated each time is X one-b =(P l1-b ,P l2-b ,…,P lm-b ), and at the same time generate a target subset Y one-b =(P d1-b ,P d2-b ,…,P dn-b ) with the corresponding number of samples being n;

[0114] S32: Using the subset pair (X one-b ,Y one-b ) composed of each training subset and the target subset, train each decision tree T one-b ; in each decision tree, node splitting is performed by selecting the optimal feature and splitting point to minimize the mean squared error; for example, for the i-th splitting node z one-i , select the feature j one-i and the splitting point s one -i to minimize the objective function, and the objective function is:

[0115]

[0116] In the formula, y i is the predicted value of the camera point cloud in the i-th splitting node, R left1 , Rright1 respectively represent the sample sets of the left and right child nodes based on the point cloud spatial feature j 1 and the splitting point s 1 ; repeat the process B times in S32 to generate B decision trees (T are the means of the original camera point cloud data Y in these sample sets respectively one-b , T one-1 , T one-2 , …, T one-B ) for the first-stage prediction process;

[0117] S33: When making a prediction on the original camera point cloud D 1 , each decision tree T b will give a prediction value. For the i-th point in the original camera point cloud D 1 , the prediction result of the b-th decision tree is expressed as The final result is the average of the prediction values of all decision trees: where

[0118]

[0119] is the final predicted coordinate at this stage, and B is the total number of decision trees;

[0120] S34: The prediction process of the random forest regression model uses the mean squared error as the loss function to measure the error between the predicted point cloud and the true point cloud. By minimizing the loss function, the model is continuously optimized to reduce the error between the prediction result and the true value. The loss function is:

[0121]

[0122] where is the average of the prediction values of all decision trees for the i-th point in the original camera point cloud D 1 after the first-stage prediction, Figure 6 is the corresponding reference point in the laser-dense point cloud L 2 for the i-th point in the original camera point cloud D 1 ; finally, the predicted points in all the original camera point clouds D 1 after the optimization is completed are merged to obtain the preliminary noise-reduced camera point cloud D 2 . Figure 6 The (a) in Figure 6 , the (b) in Figure 6 and the (c) in 2 are respectively the stereo view, side view and front view of the preliminary noise-reduced camera point cloud D

[0123] For example Figure 2 , Figure 4, Figure 5 , Figure 6 and Figure 7 As shown, through the preliminary noise reduction processing at S4, the noise of the camera's preliminary noise-reduced point cloud D 2 is significantly reduced compared to the camera's original point cloud D 1 , and the local geometric features of the camera's preliminary noise-reduced point cloud D 2 have tended to be consistent with the laser dense point cloud L 2 ; The regression optimization of S4 aims to further adjust the prediction result based on the PFH descriptor features, making the camera's preliminary noise-reduced point cloud D 2 more unified with the laser dense point cloud L 2 in terms of local geometric characteristics;

[0124] S4. Calculate the feature vectors composed of the PFH descriptors and spatial coordinates of the laser dense point cloud and the camera's preliminary noise-reduced point cloud, and import them into the random forest regression model again for secondary prediction to obtain the camera's fine noise-reduced point cloud D 3 , specifically as follows:

[0125] S41: Calculate the PFH descriptor of each point 2 in the laser dense point cloud L , and the PFH descriptor of the camera's preliminary noise-reduced point cloud D 2 ; Combine and with the corresponding point cloud coordinates in L 2 and D 2 respectively for feature combination to obtain the laser point cloud feature vector and the camera point cloud feature vector

[0126] S42: Use the camera point cloud feature vector as the input feature and the laser point cloud feature vector as the output label to train the random forest regression model, establish the mapping relationship from the input label to the output label, and perform model training based on this relationship; In the process of training the decision tree of the random forest regression model, each node selects the best combination feature j two-i of the point cloud coordinates and the PFH descriptor and the splitting point s two-i to minimize the mean square error on the left and right child nodes. The objective function for node splitting is:

[0127]

[0128] where R left2 , R right2 respectively represent based on the point cloud spatial feature j two-i and the splitting point s two-iThe sample sets of the left and right child nodes, are the means in the left and right child node sample sets respectively; repeat the process of S42 C times to generate C decision trees (T two-1 , T two-2 , …, T two-C ) for the second-stage prediction process;

[0129] S43: The prediction process of the random forest regression model in the second stage also uses the mean squared error as the loss function to measure the error between the predicted point cloud and the true point cloud. By minimizing the loss function, the model is continuously optimized to reduce the error between the prediction result and the true value. The loss function is:

[0130]

[0131] where is the average of the predicted values of all decision trees for the i-th point after the second-stage prediction of the camera pre-denoised point cloud D 2 , and is the corresponding reference point found in the laser dense point cloud L 2 for the i-th point in the camera pre-denoised point cloud D 2 ; finally, the predicted points in all the camera pre-denoised point clouds D 2 after the optimization is completed are merged to obtain the camera fine-denoised point cloud D 3 . Figure 7 In (a) of Figure 7 , Figure 7 in (b) and 3 in (c) of

[0132] are respectively the stereo view, side view, and front view of the camera fine-denoised point cloud D 3 .

[0132] In summary, the present invention performs clustering segmentation and nearest neighbor interpolation processing on the sparse laser point cloud obtained by the vertical scanning of the single-line lidar, obtains a dense single-line lidar point cloud model, provides continuous reference data for the data fusion of the depth camera and the single-line lidar, and uses a two-stage random forest regression method combining the point cloud spatial coordinates and the point cloud PFH descriptor to achieve the coarse-to-fine spatial prediction of the depth camera noisy point cloud to the reference point cloud, reduces the noise level of the depth camera, and improves the overall three-dimensional map construction accuracy of the system.

[0133] Embodiment 2:

[0134] This embodiment provides a three-dimensional point cloud noise reduction system for multi-sensor fusion, which is used to implement the three-dimensional point cloud noise reduction method for multi-sensor fusion described in Embodiment 1, and includes:

[0135] A receiving module, which is used to receive the laser raw point cloud L 1 and the camera raw point cloud D 1and perform point cloud registration on the 3D point cloud map;

[0136] A calculation module for performing continuous interpolation processing on the original laser point cloud L 1 to calculate the dense laser point cloud L 2 ;

[0137] A preliminary prediction module for inputting the dense laser point cloud L 2 and the original camera point cloud D 1 into a pre - constructed random forest regression model for preliminary point cloud prediction to obtain the preliminarily denoised original camera point cloud D 2 ;

[0138] A secondary prediction module for calculating the feature vectors composed of the PFH descriptors and spatial coordinates of the dense laser point cloud L 2 and the preliminarily denoised original camera point cloud D 2 and importing them again into the random forest regression model for secondary prediction to obtain the finely denoised camera point cloud D 3 .

[0139] Specifically, the above - mentioned receiving module, calculation module, preliminary prediction module, and secondary prediction module can be embedded in a computer processing system. The computer, based on the multi - sensor fusion - based 3D point cloud denoising method provided above, calls the above - mentioned modules to complete the task of 3D map construction; the above - mentioned receiving module, calculation module, preliminary prediction module, and secondary prediction module can perform operations according to the specific steps given by the multi - sensor fusion - based 3D point cloud denoising method.

[0140] It should be noted that the division of each module of the above - mentioned system is only a logical function division. In actual implementation, they can be fully or partially integrated into a physical entity, or physically separated. And these modules can all be implemented in the form of software called by processing elements; they can also all be implemented in the form of hardware; they can also be partially implemented in the form of software called by processing elements and partially implemented in the form of hardware. For example, the receiving module can be a separately established processing element, or can be integrated in a certain chip of the above - mentioned device. In addition, it can also be stored in the memory of the above - mentioned device in the form of program code, and called and executed by a certain processing element of the above - mentioned device to perform the functions of the above - mentioned signal processing module. The implementation of other modules is similar. In addition, these modules can be fully or partially integrated together or independently implemented. The processing element mentioned here can be an integrated circuit with signal processing capabilities. In the implementation process, each step of the above - mentioned method or each of the above - mentioned modules can be completed through the integrated logic circuit of the hardware in the processor element or the form of software instructions.

[0141] For example, the above modules may be one or more integrated circuits configured to implement the above methods, such as: one or more Application Specific Integrated Circuits (ASICs), or one or more Digital Signal Processors (DSPs), or one or more Field Programmable Gate Arrays (FPGAs), etc. Again, when a certain module above is implemented in the form of processing element scheduler code, the processing element may be a general-purpose processor, such as a Central Processing Unit (CPU) or other processors that can call program code. Again, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC). For example, the above modules may be one or more Application Specific Integrated Circuits (ASICs), or one or more Digital Signal Processors (DSPs), or one or more Field Programmable Gate Arrays (FPGAs), etc. Again, when a certain module above is implemented in the form of processing element scheduler code, the processing element may be a general-purpose processor, such as a Central Processing Unit (CPU) or other processors that can call program code. Again, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0142] Embodiment 3:

[0143] This embodiment provides a collection device for collecting the above-mentioned laser raw point cloud and camera raw point cloud, including a rectangular pipeline, a single-line lidar, a depth camera, a sensor bracket, a guide rail slider, a lead screw, an angle encoder, a stepping motor, a power supply, a motor controller, a motor driver, and an industrial computer;

[0144] The lead screw is installed at the bottom of the inner wall of the rectangular pipeline. The guide rail slider is threadedly connected to the lead screw. The sensor bracket is installed on the guide rail slider. The depth camera is horizontally installed on the sensor bracket by threads. The single-line lidar is vertically installed on the sensor bracket by threads. The stepping motor is fixedly connected to the head end of the lead screw through a coupling. The angle encoder is fixedly connected to the tail end of the lead screw through a coupling. The power supply, motor controller, motor driver, and industrial computer are all fixed on the surface of the platform extending from the outside of the rectangular pipeline. The single-line lidar and the depth camera are used to collect three-dimensional data. The power supply is used to supply power to the industrial computer. The motor controller and the motor driver are used to drive the stepping motor to rotate. The industrial computer is used to receive and process the data from the single-line lidar, depth camera, and angle encoder.

[0145] Furthermore, an installation frame is rotatably connected to the end of the lead screw. A guide block is fixedly connected to the bottom of the guide rail slider. A guide groove adapted to the guide block is provided on the side wall of the installation frame. Starting the stepping motor to drive the lead screw to rotate forward or backward, using the meshing relationship between the guide rail slider and the lead screw, the guide rail slider can be horizontally moved left and right on the lead screw. When the guide rail slider moves horizontally, the provided guide block and guide groove can limit and guide the guide rail slider to prevent the guide rail slider from rotating with the lead screw.

[0146] Furthermore, the stepping motor is fixedly installed on the installation frame for providing support.

[0147] It should be noted that in this article, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or also includes elements inherent to such process, method, article or device.

[0148] For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances. When an element is referred to as "assembled on", "installed on", "fixed on" or "set on" another element, it can be directly on the other element or there may also be an intermediate element. When an element is considered to be "connected" to another element, it can be directly connected to the other element or there may be an intermediate element at the same time. The terms "vertical", "horizontal", "upper", "lower", "left", "right" and similar expressions used herein are only for the purpose of illustration and do not represent the only implementation.

[0149] Although embodiments of the present invention have been shown and described, those of ordinary skill in the art will appreciate that various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the invention. The scope of the invention is defined by the appended claims and their equivalents.

[0150] In the description of this specification, the descriptions referring to the terms "one embodiment", "example", "specific example", etc. mean that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present disclosure. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

Claims

1. A three-dimensional point cloud denoising method based on multi-sensor fusion, characterized in that: The following steps are involved: Receive the 3D point cloud map of the laser original point cloud L1 and the camera original point cloud D1 and perform point cloud registration; Perform continuous interpolation processing on the original laser point cloud L1 to calculate the laser dense point cloud L2; The laser dense point cloud L2 and the camera original point cloud D1 are input into the pre-built random forest regression model for preliminary point cloud prediction to obtain the camera original point cloud D2 with preliminary denoising; The feature vector composed of the PFH descriptor and spatial coordinates of the laser dense point cloud L2 and the camera original point cloud D2 with preliminary denoising is calculated, and then imported into the random forest regression model for secondary prediction to obtain the camera fine denoising point cloud D3.

2. The 3D point cloud denoising method based on multi-sensor fusion according to claim 1, characterized in that: The laser original point cloud L1 is obtained by collecting single-line laser radar, and the camera original point cloud D1 is obtained by collecting depth camera.

3. The multi-sensor fusion 3D point cloud denoising method according to claim 2, characterized in that: Receive the 3D point cloud map of the laser original point cloud L1 and the camera original point cloud D1 and perform point cloud registration as follows: (31) Use the depth camera to collect RGB and Depth images inside the target component, and combine the ORB-SLAM3 algorithm to perform feature extraction, pose estimation and dense mapping to obtain the camera original point cloud D1; (32) Using a single-line laser radar to collect multiple frames of two-dimensional laser data inside the target component, the distance and angle information contained in each frame of laser scanning data is converted into three-dimensional point cloud data using the Hector-SLAM algorithm. For each laser point, the x and y coordinates of the point are calculated based on the distance and angle information, and the z coordinate is set to a consistent initial value; The initial three-dimensional coordinates of each frame point cloud can be expressed as: L i ={(x i,j ,y i,j ,z0)|j=1,2,...,n i } Where z0 is the initial value of the Z coordinate, n i is the number of points in each frame of laser data; (33) The mileage of each frame of laser point cloud is calculated through the angle information recorded by the angle encoder and the lead of the lead screw, and the Z coordinate is updated to a dynamic value that changes with time and mileage. The update formula of the Z coordinate can be directly expressed as: Where Δzi is the Z coordinate change corresponding to each frame of laser point cloud, p is the lead of the lead screw, and θi is the angle data of the i-th frame recorded by the angle encoder; (34) According to the feedback data of the angle encoder, the Z coordinate of each frame of the laser scanning point cloud is updated to reflect the actual horizontal displacement of the single-line laser radar when scanning inside the target component. Finally, each frame of the laser point cloud is sequentially superimposed according to the calculated mileage information to form a complete laser original point cloud L1; (35) Through the ICP registration algorithm, the objective function is optimized to minimize the corresponding point error between the camera original point cloud D1 and the laser original point cloud L1. The optimization objective function is: Where R is the rotation matrix, T is the translation vector, and They represent the corresponding points in the depth camera and lidar point clouds respectively. By iteratively optimizing R and T, the overlap between the camera original point cloud D1 and the laser original point cloud L1 is maximized to achieve point cloud registration.

4. The 3D point cloud denoising method of multi-sensor fusion according to claim 3, characterized in that: The laser original point cloud L1 is continuously interpolated to obtain the laser dense point cloud L2, as follows: (41) Cluster segmentation is performed on the original laser point cloud L1, and a density-based spatial clustering algorithm is used to specify the minimum neighborhood radius ∈ and the minimum number of points P. min , to determine whether a point is a core point, boundary point or noise point in the laser original point cloud L1, if the neighborhood of a point contains at least P min points, then this point is the core point, and all points in its neighborhood belong to the same cluster; by continuously expanding the neighborhood of the core point, the cluster segmentation of the original lidar point cloud L1 is finally achieved; The core conditions are: ∣{q∈P∣||p,q||≤ò}∣≥P min In the formula, ||p,q|| represents the distance between points p and q, and P is the point cloud dataset; (42) For the circle point cloud segmented from the original laser point cloud L1, start from the first circle point cloud, traverse each point, and find its nearest neighbor in the next circle; let P1 and P2 be the point cloud data of the first and second circles, respectively, containing i and j points. For each point p in the first circle 1i , find the nearest neighbor point p corresponding to it in the second circle 2j ; The nearest neighbor point is defined as: j=argmin k ||p 1i -p 2j || In the formula, ||p 1i -p 2j || represents point p 1i p 2j The Euclidean distance between (43) In this way, the nearest neighbor point pairs (p 1i ,p 2j ), perform linear interpolation on the nearest neighbor point pairs of all adjacent circle point clouds to obtain a dense and continuous single-line lidar point cloud. Let p 1i =(x 1i ,y 1i ,z 1i ) and p 2j =(x 2j ,y 2j ,z 2j ) is a pair of nearest neighbor points. Linear point cloud interpolation is performed between the two points to generate a new point p m : p m (t)=(1-t)·p 1i +t·p 2j ,t∈[0,1] Where t is the interpolation parameter; t is taken at equal intervals from 0 to 1 to obtain the interpolation point cloud set P interp : Where N t is the number of interpolation points; interpolation operation is performed on the nearest neighbor point pairs of all adjacent circles to obtain the laser dense point cloud L2.

5. The multi-sensor fusion 3D point cloud denoising method according to claim 4, characterized in that: The laser dense point cloud L2 and the camera original point cloud D1 are input into the pre-built random forest regression model for preliminary point cloud prediction to obtain the camera original point cloud D2 with preliminary denoising, as follows: (51) Take all the point cloud spatial coordinate data in the laser dense point cloud L2 as the training data set X one , all the point cloud spatial coordinate data of the camera's original point cloud D1 is used as the target data Y one : From X one =(P l1 ,P l2 ,…,P lm ) is randomly sampled B times with replacement, the number of samples in each sampling is m, and each sampling will generate the bth training subset X one-b =(P l1-b ,P l2-b ,…,P lm-b ), and at the same time generate the corresponding target subset Y with a sample number of n one-b =(P d1-b ,P d2-b ,…,P dn-b ); (52) Using each subset pair consisting of a training subset and a target subset (X one-b ,Y one-b ), for each decision tree T one-b Training is performed by selecting the optimal features and splitting points in each decision tree to split the nodes so as to minimize the mean square error: For the i-th split node z one-i , select feature j one-i and split point s one-i Minimize the objective function, the objective function is: Where y i is the camera point cloud prediction value in the i-th split node, R left1 , R right1 Respectively represent the sample sets of the left and right child nodes based on the point cloud spatial feature j1 and the split point s1, are the original point cloud data Y of the camera in these sample sets respectively. one-b Repeat step (52) B times to generate B decision trees (T one-1 ,T one-2 ,…,T one-B ); (53) When predicting the camera’s original point cloud D1, each decision tree T b A prediction value will be given for the i-th point in the camera's original point cloud D1 The prediction result of the bth decision tree is expressed as Final Result is the average of all decision tree predictions: In the formula is the final prediction coordinate of the first stage, and B is the total number of decision trees; (54) The prediction process of the random forest regression model uses the mean square error as the loss function to measure the error between the predicted point cloud and the actual point cloud. The loss function is: In the formula is the average of all decision tree prediction values ​​of the i-th point in the camera original point cloud D1 after the first stage prediction, It is the corresponding reference point of the i-th point in the laser dense point cloud L2 and the camera original point cloud D1; (54) Finally, the predicted points in all the optimized camera original point clouds D1 are merged to obtain the camera preliminary denoised point cloud D2.

6. The multi-sensor fusion 3D point cloud denoising method according to claim 5, characterized in that: The feature vector composed of the PFH descriptor and spatial coordinates of the laser dense point cloud L2 and the camera original point cloud D2 with preliminary denoising is calculated, and then imported into the random forest regression model for secondary prediction to obtain the camera fine denoising point cloud D3, as follows: (61) Calculate each point in the laser dense point cloud L2 PFH descriptor, and PFH descriptor of camera's initial noise reduction point cloud D2 Will and Combine the features with the corresponding point cloud coordinates in L2 and D2 to obtain the laser point cloud feature vector and the camera point cloud feature vector (62) Camera point cloud feature vector As input label, the laser point cloud feature vector As the output label, the random forest regression model is trained to establish a mapping relationship from the input label to the output label, and the model is trained based on this relationship; in the process of training the decision tree of the random forest regression model, each node selects the combined feature j of the best point cloud coordinates and the PFH descriptor two-i and split point s two-i , so that the mean square error on the left and right child nodes is minimized, and the objective function of node splitting is: Where R left2 , R right2 Respectively represent the point cloud spatial features j two-i and split point s two-i The sample set of the left and right child nodes of , are the means of the left and right child node sample sets respectively, and step (62) is repeated C times to generate C decision trees (T two-1 ,T two-2 ,…,T two-C ); (63) The prediction process of the random forest regression model in the second stage also uses the mean square error as the loss function to measure the error between the predicted point cloud and the actual point cloud. By minimizing the loss function, the model is continuously optimized to reduce the error between the predicted result and the actual value. The loss function is: In the formula is the average value of all decision tree prediction values ​​of the i-th point after the second-stage prediction of the camera's initial denoised point cloud D2. is the corresponding reference point found in the laser dense point cloud L2 and the i-th point in the camera preliminary denoised point cloud D2; (64) The predicted points in all the optimized camera preliminary noise reduction point clouds D2 are merged to obtain the camera fine noise reduction point cloud D3.

7. A multi-sensor fusion 3D point cloud denoising system, used to implement the multi-sensor fusion 3D point cloud denoising method according to any one of claims 1 to 6, characterized in that: include: A receiving module is used to receive the three-dimensional point cloud map of the laser original point cloud L1 and the camera original point cloud D1 and perform point cloud registration; The calculation module is used to perform continuous interpolation processing on the laser original point cloud L1 to calculate the laser dense point cloud L2; The preliminary prediction module is used to input the laser dense point cloud L2 and the camera original point cloud D1 into the pre-built random forest regression model for preliminary point cloud prediction, and obtain the camera original point cloud D2 with preliminary noise reduction; The secondary prediction module is used to calculate the feature vector composed of the PFH descriptor and spatial coordinates of the laser dense point cloud L2 and the camera original point cloud D2 with preliminary denoising, and import them into the random forest regression model for secondary prediction to obtain the camera fine denoising point cloud D3.

8. A collection device for collecting the laser original point cloud and the camera original point cloud as claimed in claim 1 or 2, characterized in that: Including rectangular pipe, single-line laser radar, depth camera, sensor bracket, guide rail slider, lead screw, angle encoder, stepper motor, power supply, motor controller, motor driver, industrial computer; The lead screw is installed at the bottom of the inner wall of the rectangular pipe, the guide rail slider is threadedly connected to the lead screw, the sensor bracket is installed on the guide rail slider, the depth camera is horizontally installed on the sensor bracket through threads, the single-line laser radar is vertically installed on the sensor bracket through threads, the stepper motor is fixedly connected to the head end of the lead screw through a coupling, the angle encoder is fixedly connected to the end of the lead screw through a coupling, and the power supply, motor controller, motor driver and industrial computer are all fixed on the platform surface extending from the outside of the rectangular pipe.

9. The acquisition device according to claim 8, characterized in that: The end of the lead screw is rotatably connected to a mounting frame, the bottom of the guide rail slider is fixedly connected to a guide block, and a guide groove matched with the guide block is opened on the side wall of the mounting frame.

10. The acquisition device according to claim 9, characterized in that: The stepping motor is fixedly mounted on the mounting frame.

Images