Irregular roadway colored point cloud hole filling method

Abstract

The invention discloses an irregular roadway colored point cloud hole filling method, and belongs to the field of underground mine three-dimensional modeling and point cloud data processing. According to the method, firstly, dense slicing is performed on roadway point clouds, slices are projected to a two-dimensional plane for polar coordinate sorting, disordered point clouds are ordered, then the slices are used as data points, a control point solution matrix is solved by adopting a chasing method, initial control points are rapidly acquired, the control points are optimized by utilizing a progressive iterative approximation algorithm (PIA), and the point clouds are optimized by utilizing the PIA. According to the method, a fitting curve is obtained, the error between the fitting curve and an original point cloud is effectively reduced, non-uniform cubic B-spline adaptive fitting interpolation is carried out according to the point spacing to realize uniform filling of holes, finally, K-nearest neighbor search is carried out on interpolation points, the RGB mean value of neighborhood points is calculated and assigned to the interpolation points, and color repair of hole areas is realized. The method has a good repairing effect on various types of holes of the irregular roadway of the mine, uniform color assignment can be carried out on point clouds at the same time, and the modeling quality and efficiency are effectively improved.

Description

Technical Field

[0001] The invention discloses a method for filling holes in a colored point cloud of an irregular tunnel, and belongs to the field of three-dimensional modeling and point cloud data processing of underground mines. Background Art

[0002] Mineral resources are one of the country's important natural resources. Underground mines account for a large proportion of mineral resources and have high mining risks. Some underground mines do not produce according to design and change the mining scope without authorization, which leads to major safety hazards and violations of regulations and procedures. By constructing a three-dimensional model of an underground mine, we can more accurately understand the structure and mining status of the mine, effectively evaluate and manage safety risks in the mining process, and ensure the safety of mine production. Three-dimensional laser scanning technology has the characteristics of high precision, high density, and easy operation. It has become an important means of spatial data collection for underground mine shaft engineering. The high-precision point cloud data obtained can realize the rapid construction of three-dimensional models.

[0003] Since there are a large number of pipeline systems, lighting fixtures, mine cars and other equipment and facilities distributed in the tunnel, it is inevitable that the tunnel wall will be blocked during data collection, forming different types of holes such as internal holes, boundary holes and through holes. In addition, there is often water accumulation in the tunnel, which has a high absorption and scattering ability for laser beams, which will also cause holes, affecting the modeling accuracy and integrity of the 3D model and increasing the cost of later model processing. Therefore, hole filling is a basic work for spatial modeling of underground mine shafts and tunnels based on 3D point clouds, which is of great value.

[0004] At present, hole filling methods can be mainly divided into three categories. One is the hole repair based on the grid, which mainly triangulates the original point cloud, locates the hole according to the topological relationship of the triangulated contour line, and then performs local expansion to repair the hole. This type of method is too time-consuming to repair when the data volume is large, and has poor adaptability to the hole type. It is not suitable for the complex structural characteristics of the laneway and it is difficult to achieve a good repair effect. The other type is the hole repair method based on scattered point clouds, which directly identifies the edge points of the hole on the scattered points and performs interpolation filling inside the hole. This type of method usually has a large amount of calculation, and the accuracy of hole boundary recognition has a great influence on the filling effect, and the recognition effect of through holes of pipelines is poor. In recent years, with the continuous development of deep learning methods for three-dimensional point clouds, research on point cloud completion has also made certain breakthroughs. However, deep learning relies heavily on data sets. In real scenes, due to the occlusion of various objects, complete point cloud data is often difficult to obtain. Therefore, methods based on deep learning are difficult to promote to real application scenarios.

[0005] In summary, a method for filling holes in irregular lanes with colored point clouds is proposed. Lane point cloud slices are used as type value points, and the PIA algorithm is used to optimize the cubic B-spline curve fitting. Adaptive interpolation is performed according to the point spacing. This method not only has a good repairing effect on various types of holes, but also can uniformly color the repaired point cloud, effectively improving the quality and efficiency of modeling. Summary of the invention

[0006] The purpose of the present invention is to provide a method for filling holes in a color point cloud on an irregular tunnel surface, so as to solve the problem that various types of holes in mine tunnels are difficult to fill at one time and the color information of the hole area is missing.

[0007] A method for filling holes in color point clouds of irregular lanes, comprising:

[0008] S1. Perform voxel filtering on the point cloud, extract the real point closest to the voxel grid centroid, reduce the point cloud density, and make the scattered point cloud regular, extract the center line of the point cloud, and perform dense slicing along the vertical direction of the center line to obtain point cloud slices;

[0009] S2. For each unordered point cloud slice, sort the point cloud slice by polar coordinate angle calculation;

[0010] S3. The sorted slice point cloud obtained in S2 is used as the type value point input, and the control point solution matrix of the B-spline curve is decomposed by LU using the chasing method to obtain the initial control point;

[0011] S4. Use the PIA algorithm to iteratively optimize the initial control points to reduce the error between the control point generated curve and the real point cloud;

[0012] S5. Construct the slice point cloud into a closed curve. According to the spacing between the interpolated points, perform non-uniform cubic B-spline closed curve fitting interpolation on the basis of the original value points. Perform K-nearest neighbor search on the interpolated points, calculate the RGB mean of the neighborhood points, color the interpolated points, and repair the holes in the colored point cloud. Perform voxel filtering on the data after interpolation to obtain regular lane point cloud data.

[0013] S1 includes, S1.1. Projecting the sorted point cloud onto the xoy plane, determining the direction D of the tunnel on the xoy plane by calculating the difference between the horizontal coordinate and the vertical coordinate, and then slicing along the direction D, extracting the center point of each slice to obtain the initial center line:

[0014] D = max{x max -x min ,y max -y min};

[0015] In the formula, x min 、xmax ,y min ,y max It is the minimum and maximum values ​​of the horizontal and vertical coordinates in a set of point cloud data.

[0016] S1 includes, S1.2. Using RANSAC algorithm to fit the curve, removing the points with problems, the fitted straight line is represented by point-to-point formula:

[0017]

[0018] In the formula, (x, y, z) represents the three-dimensional coordinates of a point, L = (m, n, p) is the direction vector, (x0, y0, z0) is the coordinates of the known center point, and a three-dimensional point (x 11 ,y 11 , z 11 ) projected onto the straight line is:

[0019] (x p ,y p ,z p )=(x0,y0,z0)+k*(m,n,p);

[0020] In the formula, (x p ,y p , z p ) is the projected coordinate, and the parameter k is:

[0021]

[0022] The point cloud data projected onto the straight line is the data along the direction of the laneway. It is re-sliced ​​along the perpendicular direction of the center line to obtain accurate slice data.

[0023] S2 includes:

[0024] S2.1. Perform a two-dimensional plane fitting on each slice, project the three-dimensional points onto the two-dimensional plane, obtain the convex hull contour, and calculate the center point of the convex hull;

[0025] S2.2. Calculate the polar coordinate angle between each point and the center point, sort them according to the angle, map the points back to the three-dimensional space, and obtain ordered point cloud slices. The calculation formula of the polar angle α is as follows:

[0026] α=arctan2(yy c ,xx c );

[0027] Let Y = yy c , X=xx c ,but:

[0028]

[0029] In the formula, x c ,y c is the coordinate of the center point.

[0030] S3 includes, S3.1. Taking the sorted slice point cloud as a set of input type value point sets P i , for a cubic B-spline curve, the type value point set P i and control point set K i The relationship is:

[0031] P i =(K i-1 +4K i +K i+1 )(i=1,2,3,...,n);

[0032] In the formula, there are n equations and n+2 unknowns. For a closed curve, the control point K0=K n , K n+1 =K1.

[0033] S3 includes, S3.2. Inverse control point equations are:

[0034]

[0035] In the formula, there are n equations and n unknowns. The system of equations has a unique solution. The control points K0, K1, ..., K are inversely solved according to the input slice point cloud. n-1 , from K0 = K n , K n+1 = K1, and get all n+2 control points K0, K1, ..., K n+1 .

[0036] S3 includes, S3.3. Using the chasing method to solve the system of equations, the coefficient matrix on the left side of the inverse control point system of equations is defined as H, the vector on the right side of the equation is P, the solution vector is K, and LU decomposition is performed as follows:

[0037] LU = H;

[0038] Where L is the lower triangular matrix and U is the upper triangular matrix;

[0039] Solve using forward substitution:

[0040] LY 1 =P;

[0041] Where Y 1 is the temporary solution vector;

[0042] Solve using backward substitution:

[0043] UK=Y 1 ;

[0044] Get the final solution vector K.

[0045] S4 includes, S4.1. constructing a node vector according to the number of control points m 1 Construct a node point vector N k , generate m between (0, 2π) 1 A sequence of values ​​t j (j=1,2,...,m 1 ), take the maximum value a and the minimum value b of the numerical sequence to form N k :

[0046] N k =[a,a,a,a,t2,t3,...,t m-3 ,t m-2 ,b,b,b,b];

[0047] S4.2. Establish a basis function matrix. According to the given node vector and parameter value, the De Boor-Cox recursive formula is used to calculate the basis function value of each order at each node in turn. The obtained basis function matrix B has a size of m0×n0, where m0 is the number of nodes in the node vector and n0 is the number of positions to be interpolated, that is, the number of sliced ​​point clouds. The formula is:

[0048]

[0049] In the formula, i0 represents the node index in the node vector, which takes the value of (0, m0-1), k0 represents the order of the basis function, that is, the order of the basis function at each node, represents the value of the k0-order basis function at i0, represents the value of the k0-1 order basis function at i0, represents the value of the basis function based on order k0-1 at i0+1; t′ is a parameter, indicating the position to be interpolated; Represents the i0th node in the node vector; and They respectively represent the i0+k0th node and the i0+k0+1th node in the node vector.

[0050] S4 includes, S4.3. Optimizing control points, according to the control point set K i And the basis function matrix B, calculate the interpolation difference Δ between the control point and the B-spline curve in the x, y, and z directions x , Δ y , Δ z :

[0051]

[0052] In the formula, K x , K y , K z is the coordinate value of the control point in the x, y, and z directions, X K , Y K , Z K is the coordinate vector of the control point in the x, y, and z directions;

[0053] Calculate the norm e of the error for error control, and the error threshold is 10 -5 Or iterate more than 100 times:

[0054]

[0055] In the formula, Δ xi′ , Δ yi′ , Δ zi′ is the interpolation difference of the i′th control point in the x, y, and z directions.

[0056] S5 includes, S5.1. Input P i Connect them end to end, that is, let P n+1 =P0, the number of type value points is n+1, and the total equation P(t) of B-spline curve fitting is:

[0057]

[0058] Where t is a parameter, t∈[0,1], F i,k (t) is the basis function of the B-spline curve, and the basis function formula is:

[0059]

[0060] S5.2. Set the point spacing. Divide the distance between the two interpolated points by the point spacing to get the number of points to be interpolated. This will achieve density adaptive interpolation. In a cubic B-spline curve, four points determine a cubic curve. According to the B-spline formula, input the x, y, and z coordinates of the four control points (x1, y1, z1), (x2, y2, z2) (x3, y3, z3) (x4, y4, z4), and you can calculate the interpolation point (X t , Y t , Z t ):

[0061]

[0062] S5.3. Calculation (X t , Y t , Z t ), calculate the RGB mean of the neighborhood points, assign color to the current interpolation point, supplement the color loss of the hole, and obtain the complete lane color point cloud data.

[0063] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention proposes a method for filling holes in colored point clouds of irregular tunnels, which can be applied to different types of point cloud holes in underground mine tunnels; the control point accuracy of traditional cubic B-spline curve fitting is optimized, and the interpolation result is more accurate; the different sparsity between the missing part and the complete part of the point cloud is taken into account, and adaptive non-uniform interpolation is achieved according to the point spacing, so that the overall point cloud after interpolation has good uniformity; while effectively repairing the point cloud holes, the concave and convex conditions of the inner wall of the tunnel are restored; the point cloud of the hole part is evenly colored, which lays a good data foundation for real-life 3D modeling. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a complete flow chart of the present invention;

[0065] Figure 2 It is a point cloud data map containing holes;

[0066] Figure 3 Project the roadway point cloud onto a plane diagram;

[0067] Figure 4 Schematic diagram for polar coordinate sorting;

[0068] Figure 5 It is a slice map of unordered point cloud;

[0069] Figure 6 It is an ordered and unordered point cloud slice map;

[0070] Figure 7 Point cloud slices after hole filling;

[0071] Figure 8 This is the complete point cloud image after the holes are filled. DETAILED DESCRIPTION

[0072] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0073] The complete flow chart of the present invention is as follows Figure 1 As shown, a method for filling holes in a color point cloud of an irregular lane comprises:

[0074] S1. Perform voxel filtering on the point cloud to extract the real point closest to the voxel grid centroid and reduce the point cloud density, such as Figure 2As shown, the scattered point cloud containing holes is regularized, the center line of the point cloud is extracted, and dense slicing is performed along the vertical direction of the center line to obtain point cloud slices;

[0075] S2. For each unordered point cloud slice, sort the point cloud slice by polar coordinate angle calculation;

[0076] S3. The sorted slice point cloud obtained in S2 is used as the type value point input, and the control point solution matrix of the B-spline curve is decomposed by LU using the chasing method to obtain the initial control point;

[0077] S4. Use the PIA algorithm to iteratively optimize the initial control points to reduce the error between the control point generated curve and the real point cloud;

[0078] S5. Construct the slice point cloud into a closed curve. According to the spacing between the interpolated points, perform non-uniform cubic B-spline closed curve fitting interpolation on the basis of the original value points. Perform K-nearest neighbor search on the interpolated points, calculate the RGB mean of the neighborhood points, color the interpolated points, and repair the holes in the colored point cloud. Perform voxel filtering on the data after interpolation to obtain regular lane point cloud data.

[0079] S1 includes, S1.1. Projecting the sorted point cloud onto the xoy plane, such as Figure 3 As shown, by calculating the difference between the horizontal and vertical coordinates, the direction D of the tunnel on the xoy plane is determined, and then slices are sliced ​​along the direction D, and the center point of each slice is extracted to obtain the initial center line:

[0080] D = max{x max -x min ,y max -y min};

[0081] In the formula, x min 、x max ,y min ,y max It is the minimum and maximum values ​​of the horizontal and vertical coordinates in a set of point cloud data.

[0082] S1 includes, S1.2. Using RANSAC algorithm to fit the curve, removing the points with problems, the fitted straight line is represented by point-to-point formula:

[0083]

[0084] In the formula, (x, y, z) represents the three-dimensional coordinates of a point, L = (m, n, p) is the direction vector, (x0, y0, z0) is the coordinates of the known center point, and a three-dimensional point (x 11 ,y 11 , z 11) projected onto the straight line is:

[0085] (x p ,y p ,z p )=(x0,y0,z0)+k*(m,n,p);

[0086] In the formula, (x p ,y p , z p ) is the projected coordinate, and the parameter k is:

[0087]

[0088] The point cloud data projected onto the straight line is the data along the direction of the laneway. It is re-sliced ​​along the perpendicular direction of the center line to obtain accurate slice data.

[0089] S2 includes:

[0090] S2.1. Perform a two-dimensional plane fitting on each slice, project the three-dimensional points onto the two-dimensional plane, obtain the convex hull contour, and calculate the center point of the convex hull;

[0091] S2.2. Read each unordered slice, such as Figure 5 As shown, according to Figure 4 The principle shown is to calculate the polar coordinate angle between each point and the center point, sort them according to the angle size, and map the points back to three-dimensional space, such as Figure 6 As shown, ordered point cloud slices are obtained, and the calculation formula of the polar angle α is as follows:

[0092] α=arctan2(yy c ,xx c );

[0093] Let Y = yy c , X=xx c ,but:

[0094]

[0095] In the formula, x c ,y c is the coordinate of the center point.

[0096] S3 includes, S3.1. Taking the sorted slice point cloud as a set of input type value point sets P i , for a cubic B-spline curve, the type value point set P i and control point set K i The relationship is:

[0097] P i =(K i-1 +4K i+K i+1 )(i=1,2,3,...,n);

[0098] In the formula, there are n equations and n+2 unknowns. For a closed curve, the control point K0=K n , K n+1 =K1.

[0099] S3 includes, S3.2. Inverse control point equations are:

[0100]

[0101] In the formula, there are n equations and n unknowns. The system of equations has a unique solution. The control points K0, K1, ..., K are inversely solved according to the input slice point cloud. n-1 , from K0 = K n , K n+1 = K1, and get all n+2 control points K0, K1, ..., K n+1 .

[0102] S3 includes, S3.3. Using the chasing method to solve the system of equations, the coefficient matrix on the left side of the inverse control point system of equations is defined as H, the vector on the right side of the equation is P, the solution vector is K, and LU decomposition is performed as follows:

[0103] LU = H;

[0104] Where L is the lower triangular matrix and U is the upper triangular matrix;

[0105] Solve using forward substitution:

[0106] LY 1 =P;

[0107] Where Y 1 is the temporary solution vector;

[0108] Solve using backward substitution:

[0109] UK=Y 1 ;

[0110] Get the final solution vector K.

[0111] S4 includes, S4.1. constructing a node vector according to the number of control points m 1 Construct a node point vector N k , generate m between (0, 2π) 1 A sequence of values ​​t j (j=1,2,...,m 1 ), take the maximum value a and the minimum value b of the numerical sequence to form N k :

[0112] Nk =[a,a,a,a,t2,t3,...,t m-3 ,t m-2 ,b,b,b,b];

[0113] S4.2. Establish a basis function matrix. According to the given node vector and parameter value, the De Boor-Cox recursive formula is used to calculate the basis function value of each order at each node in turn. The obtained basis function matrix B has a size of m0×n0, where m0 is the number of nodes in the node vector and n0 is the number of positions to be interpolated, that is, the number of sliced ​​point clouds. The formula is:

[0114]

[0115] In the formula, i0 represents the node index in the node vector, which takes the value of (0, m0-1), k0 represents the order of the basis function, that is, the order of the basis function at each node, represents the value of the k0-order basis function at i0, represents the value of the k0-1 order basis function at i0, represents the value of the basis function based on order k0-1 at i0+1; t′ is a parameter, indicating the position to be interpolated; Represents the i0th node in the node vector; and They respectively represent the i0+k0th node and the i0+k0+1th node in the node vector.

[0116] S4 includes, S4.3. Optimizing control points, according to the control point set K i And the basis function matrix B, calculate the interpolation difference Δ between the control point and the B-spline curve in the x, y, and z directions x , Δ y , Δ z :

[0117]

[0118] In the formula, K x , K y , K z is the coordinate value of the control point in the x, y, and z directions, X K , Y K , Z K is the coordinate vector of the control point in the x, y, and z directions;

[0119] Calculate the norm e of the error for error control, and the error threshold is 10 -5 Or iterate more than 100 times:

[0120]

[0121] In the formula, Δ xi′ , Δ yi′ , Δ zi′ is the interpolation difference of the i′th control point in the x, y, and z directions.

[0122] S5 includes, S5.1. Input P i Connect them end to end, that is, let P n+1 =P0, the number of type value points is n+1, and the total equation P(t) of B-spline curve fitting is:

[0123]

[0124] Where t is a parameter, t∈[0,1], F i,k (t) is the basis function of the B-spline curve, and the basis function formula is:

[0125]

[0126] S5.2. Set the point spacing. Divide the distance between the two interpolated points by the point spacing to get the number of points to be interpolated. This will achieve density adaptive interpolation. In a cubic B-spline curve, four points determine a cubic curve. According to the B-spline formula, input the x, y, and z coordinates of the four control points (x1, y1, z1), (x2, y2, z2) (x3, y3, z3) (x4, y4, z4), and you can calculate the interpolation point (X t , Y t , Z t ), the interpolation result of a single slice is as follows Figure 7 As shown, the missing parts have been evenly filled:

[0127]

[0128] S5.3. Calculation (X t , Y t , Z t ), calculate the RGB mean of the neighborhood points, assign color to the current interpolation point, and fill in the color loss of the hole, such as Figure 8 As shown, the complete lane color point cloud data is obtained.

Claims

1. A method for filling holes in irregular lane color point clouds. It is characterized in that include: S1. Perform voxel filtering on the point cloud, extract the real point closest to the voxel grid centroid, reduce the point cloud density, and make the scattered point cloud regular, extract the center line of the point cloud, and perform dense slicing along the vertical direction of the center line to obtain point cloud slices; S2. For each unordered point cloud slice, sort the point cloud slice by polar coordinate angle calculation; S3. The sorted slice point cloud obtained in S2 is used as the type value point input, and the control point solution matrix of the B-spline curve is decomposed by LU using the chasing method to obtain the initial control point; S4. Use the PIA algorithm to iteratively optimize the initial control points to reduce the error between the control point generated curve and the real point cloud; S5. Construct the slice point cloud into a closed curve. According to the spacing between the interpolated points, perform non-uniform cubic B-spline closed curve fitting interpolation on the basis of the original value points. Perform K-nearest neighbor search on the interpolated points, calculate the RGB mean of the neighborhood points, color the interpolated points, and repair the holes in the colored point cloud. Perform voxel filtering on the data after interpolation to obtain regular lane point cloud data.

2. According to claim 1, a method for filling holes in color point clouds of irregular lanes, It is characterized in that S1 includes, S1.

1. Projecting the sorted point cloud onto the xoy plane, determining the direction D of the tunnel on the xoy plane by calculating the difference between the horizontal coordinate and the vertical coordinate, and then slicing along the direction D, extracting the center point of each slice to obtain the initial center line: D=max{x max -x min ,y max -y min }; In the formula, x min 、x max ,y min ,y max It is the minimum and maximum values ​​of the horizontal and vertical coordinates in a set of point cloud data.

3. According to claim 2, a method for filling holes in color point clouds of irregular lanes, It is characterized in that S1 includes, S1.

2. Using RANSAC algorithm to fit the curve, removing the points with problems, the fitted straight line is represented by point-to-point formula: In the formula, (x, y, z) represents the three-dimensional coordinates of a point, L = (m, n, p) is the direction vector, (x 0 ,y 0 , z 0 ) is the coordinate of the known center point, and a three-dimensional point (x 11 ,y 11 , z 11 ) projected onto the straight line is: (x p ,y p ,z p )=(x 0 ,y 0 ,z 0 )+k*(m,n,p); In the formula, (x p ,y p , z p ) is the projected coordinate, and the parameter k is: The point cloud data projected onto the straight line is the data along the direction of the laneway. It is re-sliced ​​along the perpendicular direction of the center line to obtain accurate slice data.

4. According to claim 3, a method for filling holes in color point clouds of irregular lanes, It is characterized in that S2 include: S2.

1. Perform a two-dimensional plane fitting on each slice, project the three-dimensional points onto the two-dimensional plane, obtain the convex hull contour, and calculate the center point of the convex hull; S2.

2. Calculate the polar coordinate angle between each point and the center point, sort them according to the angle, map the points back to the three-dimensional space, and obtain ordered point cloud slices. The calculation formula of the polar angle α is as follows: α=arctan2(yy c ,xx c ); Let Y = yy c , X=xx c ,but: In the formula, x c ,y c is the coordinate of the center point.

5. According to claim 4, a method for filling holes in color point clouds of irregular lanes, It is characterized in that S3 includes, S3.

1. Taking the sorted slice point cloud as a set of input type value point sets P i , for a cubic B-spline curve, the type value point set P i and control point set K i The relationship is: P i =(K i-1 +4K i +K i+1 )(i=1,2,3,...,n); In the formula, there are n equations and n+2 unknowns. For a closed curve, the control point K 0 =K n , K n+1 =K 1 .

6. A method for filling holes in color point clouds of irregular lanes according to claim 5, It is characterized in that S3 includes, S3.

2. Inverse control point equations are: In the formula, there are n equations and n unknowns. The system of equations has a unique solution. The control point K is inversely solved based on the input slice point cloud. 0 , K 1 , ..., K n-1 , by K 0 =K n , K n+1 =K 1 , get all n+2 control points K 0 , K 1 , ..., K n+1 .

7. A method for filling holes in color point clouds of irregular lanes according to claim 6, It is characterized in that S3 includes, S3.

3. Using the chasing method to solve the system of equations, the coefficient matrix on the left side of the inverse control point system of equations is defined as H, the vector on the right side of the equation is P, the solution vector is K, and LU decomposition is performed as follows: LU = H; Where L is the lower triangular matrix and U is the upper triangular matrix; Solve using forward substitution: LY 1 =P; Where Y 1 is the temporary solution vector; Solve using backward substitution: UK=Y 1 ; Get the final solution vector K.

8. The method for filling holes in color point clouds of irregular lanes according to claim 7, It is characterized in that S4 includes, S4.

1. constructing a node vector according to the number of control points m 1 Construct a node point vector N k , generate m between (0, 2π) 1 A sequence of values ​​t j (j=1,2,...,m 1 ), take the maximum value a and the minimum value b of the numerical sequence to form N k : N k =[a,a,a,a,t 2 ,t 3 ,...,t m-3 ,t m-2 ,b,b,b,b]; S4.

2. Establish the basis function matrix. According to the given node vector and parameter value, the De Boor-Cox recursive formula is used to calculate the basis function value of each order at each node in turn. The obtained basis function matrix B has a size of m. 0 ×n 0 , where m 0 is the number of nodes in the node vector, n 0 is the number of positions to be interpolated, that is, the number of sliced ​​point clouds, and the formula is: In the formula, i 0 Represents the node index in the node vector, and its value is (0, m 0 -1), k 0 represents the order of the basis function, that is, the order of the basis function at each node, Indicates that based on k 0 The basis function of order i 0 The value at Indicates that based on k 0 The -1 order basis function is i 0 The value at Indicates that based on k 0 The -1 order basis function is i 0 The value at +1; t′ is a parameter, indicating the position to be interpolated; Represents the i-th node in the node vector 0 nodes; and Respectively represent the i-th node vector 0 +k 0 nodes and the i-th 0 +k 0 +1 node.

9. The method for filling holes in color point clouds of irregular lanes according to claim 8, It is characterized in that S4 includes, S4.

3. Optimizing control points, according to the control point set K i And the basis function matrix B, calculate the interpolation difference Δ between the control point and the B-spline curve in the x, y, and z directions x , Δ y , Δ z : In the formula, K x , K y , K z is the coordinate value of the control point in the x, y, and z directions, X K , Y K , Z K is the coordinate vector of the control point in the x, y, and z directions; Calculate the norm e of the error for error control, and the error threshold is 10 -5 Or iterate more than 100 times: In the formula, Δ xi ′、Δ yi ′、Δ zi ′ is the interpolation difference of the ith control point in the x, y, and z directions.

10. The method for filling holes in color point clouds of irregular lanes according to claim 9, It is characterized in that S5 includes, S5.

1. Input P i Connect them end to end, that is, let P n+1 =P 0 , the number of type value points is n+1, and the total equation P(t) of B-spline curve fitting is: Where t is a parameter, t∈[0,1], F i,k (t) is the basis function of the B-spline curve, and the basis function formula is: S5.

2. Set the point spacing. Divide the distance between the two points currently being interpolated by the point spacing to get the number of points to be interpolated, and implement density adaptive interpolation. In a cubic B-spline curve, 4 points determine a cubic curve. According to the B-spline formula, input the x, y, and z coordinates of the 4 control points (x 1 ,y 1 , z 1 )、(x 2 ,y 2 , z 2 )(x 3 ,y 3 , z 3 )(x 4 ,y 4 , z 4 ), the interpolation point (X t , Y t , Z t ): S5.

3. Calculation (X t , Y t , Z t ), calculate the RGB mean of the neighborhood points, assign color to the current interpolation point, supplement the color loss of the hole, and obtain the complete lane color point cloud data.

Images