A map road drawing method based on vtk and point cloud

Abstract

The present application discloses a method for drawing a map road based on vtk and point cloud, which relates to the field of autonomous driving, including: using vtk to obtain point cloud data of a road, and using an octree to perform spatial division on the point cloud data to obtain point cloud data after spatial division; performing coordinate transformation on the point cloud data after spatial division to align the z-axis of the point cloud coordinate system with the z-axis of the camera coordinate system; drawing a road reference line by interactively taking points on the screen according to the point cloud data after coordinate transformation; generating a road boundary and a center line according to the road reference line through geometric transformation; clustering and edge extraction of the point cloud within a preset range of the road boundary by using the DBSCAN algorithm, and smoothing by using a B-spline curve to obtain a final road boundary as a drawn map road; in view of the low efficiency of road drawing in the prior art, the present application improves the drawing efficiency.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving, and in particular to a method for drawing a map road based on vtk and point cloud. Background Art

[0002] With the development of technologies such as the Global Positioning System (GPS) and the Inertial Navigation System (INS), vehicle-mounted mobile measurement systems have been widely used. The massive point cloud data collected by sensors such as vehicle-mounted laser radars and panoramic cameras provide a data basis for the extraction of road facilities and ground features and the production of high-precision maps. High-precision maps are one of the core elements of autonomous driving and are crucial for functions such as precise vehicle positioning, environmental perception, and path planning.

[0003] Traditional navigation maps mainly serve manual driving and have relatively low requirements for the geometric accuracy of roads. However, with the rapid development of autonomous driving technology, autonomous vehicles have put forward higher requirements for the accuracy of environmental information, and existing navigation maps can no longer meet the needs of autonomous driving. High-precision maps can describe the geometric shape, topological structure and traffic facilities on the road with centimeter-level accuracy, enabling unmanned vehicles to accurately locate their own positions, understand the road environment, and make correct decision-making and control.

[0004] In the process of making high-precision maps, roads are one of the most important elements, and their extraction and reconstruction methods have attracted much attention. Using 3D LiDAR point cloud data to extract roads is a key technology in making high-precision maps. Point cloud data has the characteristics of high precision, high density, and rich information. It can truly reflect the spatial information of road scenes and is an important data source for road vectorization.

[0005] In the existing technology, road extraction mainly adopts a layered and block-based strategy, that is, the road boundary and road centerline are treated as different elements and extracted separately. This method requires manual annotation, editing and modification of the boundary and centerline respectively, which is labor-intensive and inefficient. In addition, due to the lack of effective post-processing methods, the extracted road vector data often has problems such as uneven lines and low accuracy, which makes it difficult to meet the quality requirements of autonomous driving maps. Summary of the invention

[0006] In response to the problem of low road drawing efficiency in the prior art, the present application provides a map road drawing method based on VTK and point cloud. First, an octree is used to spatially divide the road point cloud, and the point cloud coordinate system is aligned with the camera coordinate system through coordinate transformation. Then, road reference lines are drawn on the transformed point cloud through screen interaction, and the initial positions of the road boundary and center line are generated based on the reference lines. Next, the DBSCAN algorithm is used to extract and cluster point clouds near the road boundary, and a smooth road boundary curve is obtained by fitting a uniform B-spline curve. Finally, the fitted road boundary curve is output as a vectorized representation of the map road, which improves the drawing efficiency.

[0007] The purpose of this application is achieved through the following technical solutions.

[0008] The present application provides a method for drawing a map road based on vtk and point cloud, including: using vtk to obtain the point cloud data of the road, and using octree to spatially divide the point cloud data to obtain the point cloud data after spatial division; using the octree algorithm, the large-scale road point cloud data is divided into multiple small spatial areas, and the number of point clouds in each area is relatively small, which is convenient for subsequent processing and analysis. At the same time, the hierarchical structure of the octree also facilitates the rapid retrieval and neighborhood search of the point cloud. The time complexity of the octree algorithm is O (nlogn), where n is the number of point clouds, and it can efficiently process large-scale point cloud data. The point cloud data after spatial division is transformed into coordinates, and the z axis of the point cloud coordinate system is aligned with the z axis of the camera coordinate system; according to the point cloud data after coordinate transformation, the road reference line is drawn by interactively taking points on the screen; according to the road reference line, the road boundary and center line are generated by geometric transformation; the point cloud within the preset range of the road boundary is clustered and edge extracted by the DBSCAN algorithm, and the B-spline curve is used for smoothing to obtain the final road boundary as the drawn map road.

[0009] Furthermore, the z-axis of the point cloud coordinate system is aligned with the z-axis of the camera coordinate system, including: constructing a transformation matrix between the camera coordinate system and the point cloud coordinate system: ,in, represents the point in the camera coordinate system, R represents the rotation matrix, T represents the translation vector, Represents a point in the point cloud coordinate system; according to the transformation matrix, each point cloud data point after the space is divided Multiply by the transformation matrix to get the transformed point ; The transformed points As the point cloud data points after coordinate transformation; according to all the transformed point cloud data points, the coordinate system of the point cloud is updated to obtain point cloud data in which the z axis of the point cloud coordinate system is aligned with the z axis of the camera coordinate system.

[0010] Furthermore, drawing a road reference line includes: selecting a point located at the road boundary on the point cloud data after coordinate transformation through screen interaction. ,point Located in the camera coordinate system; point According to the transformation matrix, the corresponding point is obtained in the point cloud coordinate system. ; with a point To query a point, use the octree to search for the point Point cloud subset within a preset range; in the point cloud subset, calculate each point To point The Euclidean distance of ; Point As the first point of the road reference line, Reversely map back to the camera coordinate system to obtain the corresponding screen coordinates; starting from the screen coordinates, repeat the above process and select the second reference point to the nth reference point; connect all reference points in sequence according to the selection order of each point to generate a road reference line.

[0011] Further, using the octree to search for points Point cloud subset within a preset range, including: The cuboid search range is centered on ; starting from the root node of the octree, recursively traverse each node: if the bounding box of the current node is within the cuboid search range, the point cloud data corresponding to the corresponding node is added to the point cloud subset; if the bounding box of the current node partially overlaps with the cuboid search range, then: if the number of point clouds contained in the current node is less than the preset threshold, all the point cloud data contained in the current node are added to the point cloud subset; otherwise, recursively perform the above judgment on its eight child nodes; if the bounding box of the current node does not intersect with the cuboid search range, skip the corresponding node; after the recursion is completed, merge all the point cloud data obtained by the search as the point An initial point cloud subset is obtained; the normal vector of each point in the initial point cloud subset is calculated, and the angle between the normal vector and the preset road normal vector is calculated; if the angle is greater than a threshold, the corresponding point is regarded as a noise point and deleted from the initial point cloud subset to obtain a final point cloud subset.

[0012] Furthermore, through geometric transformation, the road boundary and center line are generated, including: traversing the points on the road reference line, for every two adjacent points and : Establish the straight line equation of P1 and P2: ; Calculate the unit normal vector of the line equation ; According to the unit normal vector and the preset road width w, we get the translation equation 1 and the translation equation 2; solve the translation equation 1 and the translation equation 2 to get the intersection point , as a candidate point of the road boundary; calculate the vector ,in, The coordinates of the road boundary points determined at the previous time step; calculate the vector ; Calculate vector and vector Angle ; If θ is less than ,in is the preset threshold, then keep as the new road boundary point; if θ is greater than or equal to , then discard the translation equation 2 and let , return to recalculate the intersection , until θ is less than ; After the traversal is completed, the left road boundary point set is obtained respectively and the right road boundary point set ; respectively and Sort by the road direction coordinate values ​​of the points; divide the sorted and , get an equal number of sampling points and ; Connect in sequence and The midpoints of the points corresponding to the subscripts in the form an ordered point set , as the road centerline; output , and , serving as the left road boundary, right road boundary and road centerline respectively.

[0013] Among them, the threshold Represents the current candidate boundary point The boundary point determined by the previous The connection vector , and the road direction vector The maximum allowable angle between The smaller it is, the lower the tolerance for changes in the position of the boundary points is, requiring the lines between the boundary points to be more parallel to the direction of the road. The larger the value is, the greater the deviation between the boundary points is allowed to be, and the higher the tolerance is. The value range is [0°, 90°].

[0014] Furthermore, the translation equation is expressed as follows: ; The second expression of the translation equation is as follows: ; where w represents the road width.

[0015] Furthermore, the DBSCAN algorithm is used to cluster and extract edges of the point cloud within the preset range of the road boundary, including: obtaining the left road boundary point set and the right road boundary point set ; respectively and Each boundary point in :by is the center, the road half width w / 2 is the radius, w is the road width, and the extraction Nearby point cloud subset ; Use DBSCAN algorithm to analyze the point cloud subset Perform clustering and obtain clusters; for each cluster :Use K-Means algorithm in Get distance The nearest point ;calculate To the straight line Distance ,in are adjacent boundary points; Select The smallest point As a boundary point The update point of replace , update the boundary point set; repeat the above steps until and All boundary points in are updated and a new boundary point set is obtained. and ; respectively in and Use cubic uniform B-spline curve to fit the left and right road boundary curves and Output and as the final road boundary.

[0016] Furthermore, the DBSCAN algorithm is used to analyze the point cloud subsets. Clustering, including: setting the radius parameter ε and the minimum point threshold MinPts of the DBSCAN algorithm; randomly selecting A point p in is marked as visited; with p as the center and ε as the radius, find The set of all points whose distance from p is less than ε ;like , then p is marked as a noise point; if , then create a new cluster C, add p to C, and Add all unvisited points in to the candidate set S; when S is not empty, execute: take a point q from S; take q as the center and ε as the radius, find The set of all points whose distance from q is less than ε ;if ,Will All unvisited points in are added to S; if q does not belong to any cluster, add q to C; when When , mark q as a noise point; output All clusters obtained in , as the result of DBSCAN clustering.

[0017] Furthermore, and Use cubic uniform B-spline curve to fit the left and right road boundary curves and , including: or The point set in is sorted according to the road direction coordinate value to obtain the sorted boundary point set or ;choose or The first n+1 points in are used as control points ; According to the control point ,calculate The first-order difference entropy matrix of ; According to the first-order difference quotient matrix, calculate The second order difference entropy matrix of ; According to the second-order difference quotient matrix, calculate the third-order difference quotient matrix: ; Based on control points and each order difference quotient matrix, using cubic uniform B-spline basis function to construct the left or right road boundary curve or The parametric equation of and They are respectively used as the fitted left and right road boundary curves.

[0018] Furthermore, the parametric equation is expressed as follows: ; ; Among them, l stands for left, representing the left boundary of the road; r stands for right, representing the right boundary of the road. Represents the coordinates of the i-th control point , i is the number of the control point, the value range is i=0,1,...,n, represents an ordered set of points consisting of all control points. Control points are the characteristic polygon vertices of the B-spline curve and determine the overall shape of the curve. n represents the degree of the B-spline curve. For cubic uniform B-spline, n is always equal to 3, and n+1 represents the total number of control points, that is, The number of midpoints. Represents the i-th cubic B-spline basis function, where the subscript i corresponds to the i-th control point , the subscript 3 indicates that the degree of the basis function is 3, that is, three times; u is the basis function The independent variable represents the parameter coordinates of the points on the curve; the basis function Determine the control point The influence weight of the point on the curve with parameter u; u is the parameter variable of the B-spline curve, which represents the parameter coordinates of the point on the curve. Each u value uniquely determines a point C(u) on the curve. The value range of u is [0, 1], which represents from the starting point to the end point of the curve. By changing the u value, the entire B-spline curve can be obtained continuously.

[0019] The DBSCAN algorithm is used to cluster and extract the edge of the road boundary point cloud, and the discrete points and noise points are eliminated to obtain a more accurate and smooth road boundary point set. Then, the cubic uniform B-spline curve is used to fit the boundary point set to obtain a continuous and smooth road boundary curve, which can better represent the shape and direction of the road.

[0020] Compared with the prior art, the advantages of this application are:

[0021] Traditional road drawing methods require marking the road boundary and center line separately, which is labor-intensive and inefficient. This application draws a single reference line on the point cloud and uses geometric transformation to automatically generate the road boundary and center line, which greatly reduces the workload of manual editing and significantly improves the efficiency of road drawing.

[0022] Road point clouds are often affected by obstacles such as vehicles and pedestrians, as well as local point cloud missing. This application uses octrees to divide point clouds into spaces, and draws reference lines by combining screen interactive point extraction, which can flexibly avoid obstacle interference and improve the robustness of reference line extraction. At the same time, the DBSCAN clustering algorithm is used to extract road boundary point clouds, overcoming the interference of noise points and ensuring the reliability of road boundary extraction.

[0023] Existing road vectorization methods often have low accuracy, and the generated road boundaries deviate from the actual road edges. Based on coordinate transformation, this application extracts road boundaries by iteratively calculating boundary points, ensuring the geometric consistency of the boundary and the point cloud. Furthermore, a uniform B-spline curve is used to fit the boundary points, making the vectorized road boundary smoother and straighter, and more consistent with the real road shape.

[0024] Due to the differences between the acquisition angle and coordinate system of the point cloud and the map coordinate system, the reference line drawn directly on the point cloud is often inconsistent with the road direction. This application transforms the reference line from the screen coordinate system to the point cloud coordinate system by constructing a transformation matrix between the point cloud coordinate system and the camera coordinate system, ensuring that the generated road boundary and center line are consistent with the actual road direction, thereby improving the applicability of the method.

[0025] Road vector data generated by traditional methods often requires a lot of manual editing and modification. This application uses point cloud spatial information and a series of algorithms such as coordinate transformation, geometric calculation, point cloud clustering, and curve fitting to achieve automated processing from road reference lines to vectorized road boundaries, minimizing the amount of manual editing and improving the automation level of road mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] The present application will be further described in the form of exemplary embodiments, which will be described in detail by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, wherein:

[0027] Figure 1 is an exemplary flow chart of a method for drawing a map road based on vtk and point cloud according to some embodiments of the present application;

[0028] Figure 2 This is an exemplary flowchart for generating road boundary lines according to some embodiments of the present application. DETAILED DESCRIPTION

[0029] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0030] Figure 1This is an exemplary flow chart of a method for drawing a map road based on vtk and point cloud as shown in some embodiments of the present application, including: using vtk to obtain point cloud data of the road, and using an octree to spatially divide the point cloud data to obtain point cloud data after spatial division; performing coordinate transformation on the point cloud data after spatial division, and aligning the z-axis of the point cloud coordinate system with the z-axis of the camera coordinate system; drawing a road reference line by interactively taking points on the screen according to the point cloud data after coordinate transformation; generating a road boundary and a center line through geometric transformation according to the road reference line; using the DBSCAN algorithm to cluster and extract edges of the point cloud within a preset range of the road boundary, and using a B-spline curve for smoothing to obtain the final road boundary as the drawn map road.

[0031] In this application, the VTK (Visualization Toolkit) library is used to read and visualize the road point cloud. First, the road point cloud file is read through the pcl::io::load PCD File function to obtain the original point cloud data cloud. Then, the point cloud is spatially divided using the octree algorithm. The specific steps are as follows: Create an octree object octree, set the resolution parameter resolution of the spatial division to 0.1m, that is, the side length of each octree node is 0.1m. The minimum size of the octree node is related to the point cloud density. Too large will result in too rough division, and too small will increase the amount of calculation. Through many experiments, it is determined that a resolution of 0.1m can better balance the calculation efficiency and division accuracy. Call the octree.setInput Cloud (cloud) function to set the original point cloud cloud as the input data of the octree.

[0032] By calling the octree.add Points From Input Cloud() function, the input point cloud is recursively divided according to the set resolution parameter resolution to generate an octree structure. Specifically, the function will traverse each point of the input point cloud, calculate the octree node to which it belongs based on the coordinates of the point, and add the point to the corresponding node. If the number of points in a node exceeds the preset threshold (usually 1) and the size of the node is larger than the resolution resolution, the node is further divided and eight child nodes are recursively generated until the size of all nodes is no larger than the resolution or the number of points does not exceed the threshold. Through this recursive division method, the original point cloud is divided into multiple small areas whose size does not exceed the resolution, and each area corresponds to a leaf node of the octree. Each leaf node stores all point cloud data within the range of the node, including point coordinates, color and other attribute information.

[0033] By traversing all the leaf nodes of the octree, the spatial information and point cloud data of each node can be obtained. For each leaf node, the minimum and maximum coordinate values ​​of the node are obtained by calling the it.get Bounding Box (min_pt, max_pt) function to determine the spatial range of the node. Then, the center coordinate pt of the node is calculated, that is, the average of the minimum and maximum coordinate values ​​is taken as the representative point of the node. The representative points of all leaf nodes are stored in a new point cloud object cloud_octree in sequence as the point cloud data after spatial division. Each point in cloud_octree corresponds to an octree node, whose coordinates are the center coordinates of the node, representing all the original point cloud data within the range of the node. Compared with the original point cloud, cloud_octree has much fewer points, but it can still better represent the spatial distribution characteristics of the original point cloud.

[0034] The point cloud data cloud_octree after spatial division is passed to the vtk Poly Data object of VTK for subsequent visualization and interactive operations. Vtk Poly Data is the basic data structure used to store geometric data in VTK, which can store various types of geometric elements such as points, lines, and surfaces. The point coordinate data in cloud_octree is assigned to the point set of vtk Poly Data through the vtk Points object, and then these points are organized into the topological structure of the point cloud through the vtk Cell Array object to construct a vtk Poly Data object representing the point cloud. Using the rich visualization and interactive tools provided by VTK, the vtk Poly Data object can be rendered, rotated, scaled, and color-coded to intuitively display the point cloud data after spatial division, providing visualization support for subsequent road extraction and editing. At the same time, VTK also supports interactive operations such as mouse picking and keyboard events. Users can select specific areas in the point cloud with the mouse, or enter parameters through the keyboard to interact with the point cloud data, realizing semi-automatic processing of road extraction and editing.

[0035] The purpose of performing coordinate transformation on the point cloud data after spatial division is to align the z-axis of the point cloud coordinate system with the z-axis of the camera coordinate system to achieve consistency between the point cloud and the camera perspective. Specifically, first construct a transformation matrix between the camera coordinate system and the point cloud coordinate system. The matrix consists of a rotation matrix R and a translation vector T, which is used to describe the spatial transformation relationship between the two coordinate systems. Among them, the rotation matrix R is a 3x3 matrix that describes the rotation transformation of the point cloud coordinate system relative to the camera coordinate system. The rotation matrix can be used to align the coordinate axes of the point cloud coordinate system with the coordinate axes of the camera coordinate system. The translation vector T is a 3x1 column vector that describes the translation transformation of the point cloud coordinate system relative to the camera coordinate system. The translation vector can be used to translate the origin of the point cloud coordinate system to the origin of the camera coordinate system.

[0036] Construct the transformation matrix between the camera coordinate system and the point cloud coordinate system: ,in, represents the point in the camera coordinate system, R represents the rotation matrix, T represents the translation vector, Represents a point in the point cloud coordinate system; for each point cloud data point after space division , multiply its coordinate value by the transformation matrix to get the transformed point ,Right now: , where R is the rotation matrix and T is the translation vector. Through this coordinate transformation, the points in the point cloud coordinate system are converted to the camera coordinate system, realizing the alignment of the point cloud and the camera perspective. As the point cloud data point after coordinate transformation, update the coordinate information of the point cloud. Since the rotation part R in the transformation matrix aligns the z-axis of the point cloud coordinate system with the z-axis of the camera coordinate system, the coordinate system of the transformed point cloud data point is aligned with the z-axis of the camera coordinate system. In this way, the coordinate system of the point cloud is consistent with the camera's viewing direction, which is convenient for subsequent visualization and road extraction operations. According to all the transformed point cloud data points, update the coordinate system of the point cloud to obtain point cloud data with the z-axis of the point cloud coordinate system aligned with the z-axis of the camera coordinate system. The transformation matrix [RT] needs to be determined based on the external parameters (position and attitude) of the camera and the definition of the point cloud coordinate system. Generally, the parameters of the transformation matrix can be estimated by calibrating the relative position relationship between the camera and the lidar, or by matching the feature points in the point cloud and the image. Once the transformation matrix is ​​determined, coordinate transformation can be performed on all point cloud data points to obtain point cloud data aligned with the camera's viewing direction.

[0037] Figure 2This is an exemplary flow chart for generating road boundary lines according to some embodiments of the present application. According to the point cloud data after coordinate transformation, the road reference line is drawn by screen interactive point selection: on the point cloud data after coordinate transformation, points located at the road boundary are selected by screen interactive method, which is recorded as pview. Specifically, the mouse event callback function vtk Inter actor Style Track ball Camera provided by VTK can be used to monitor the mouse click operation on the screen. When the user clicks the left mouse button in the display window, the screen coordinates (sx, sy) of the mouse click position are obtained and passed to the coordinate conversion function vtk Prop Picker::Pick() of VTK, which can convert the screen coordinates into point coordinates in three-dimensional space.

[0038] The parameters of the function vtk Prop Picker::Pick() include screen coordinates (sx, sy), renderer vtkRenderer, and the selected object vtk Prop. Among them, the renderer vtk Renderer is responsible for projecting the data in the three-dimensional space onto the two-dimensional screen, and vtk Prop represents the currently rendered point cloud object. The function calculates the ray equation of the mouse click position in the three-dimensional space based on the screen coordinates and the projection matrix of the renderer, and then intersects the ray with the point cloud data to obtain the three-dimensional point coordinates where the ray intersects the point cloud. This three-dimensional point coordinate is the point pview selected by the screen interaction, which is located in the camera coordinate system.

[0039] In order to point To convert to the point cloud coordinate system, we need to solve the transformation matrix between the camera coordinate system and the point cloud coordinate system. The homogeneous coordinates of are , the corresponding points in the point cloud coordinate system The homogeneous coordinates of are , then: , where [RT] is the transformation matrix from the camera coordinate system to the point cloud coordinate system, and inv([RT]) represents the inverse matrix of the transformation matrix. By multiplying the coordinates of the point cloud by the inverse matrix of the transformation matrix, we can get the corresponding points in the point cloud coordinate system. The coordinates of .

[0040] In actual calculation, you can use matrix operation libraries such as Eigen to calculate coordinate transformation. First, transform the point The coordinates are stored in a 4x1 column vector Then the inverse matrix inv_trans_mat of the transformation matrix [RT] from the camera coordinate system to the point cloud coordinate system is expressed as a 4x4 matrix. Through matrix multiplication view_pt.transpose() * inv_trans_mat, we get the point The homogeneous coordinates can then be converted into non-homogeneous coordinates.

[0041] At the point To query a point, use the octree to search for the point When you want to set a point cloud subset within a certain range, first set the point The search range of the cuboid with the center as the point cloud can be adjusted according to parameters such as the width of the road and the point cloud density. Usually, the length and width of the cuboid can be set to 2-3 times the width of the road, and the height can be set to 1-2 times the height of the road to ensure that the road area can be fully covered.

[0042] Then, starting from the root node of the octree, recursively traverse each node. For the currently traversed node, determine whether its bounding box is within the cuboid search range. If the bounding box of the current node is completely within the search range, the point cloud data corresponding to the node is directly added to the point cloud subset. If the bounding box of the current node partially overlaps with the search range, further determine whether the number of point clouds contained in the node is less than the preset threshold m. If it is less than the threshold m, all the point cloud data contained in the node are added to the point cloud subset; otherwise, recursively perform the above judgment process on its eight child nodes. If the bounding box of the current node does not intersect with the search range at all, skip the node and do not traverse its child nodes.

[0043] After the recursive traversal is completed, all the point cloud data obtained by the search are merged as point In order to further remove the noise points in the subset, each point in the initial point cloud subset can be calculated The normal vector , and calculate With the preset road normal vector If the angle α is greater than the preset threshold β (such as 30°), the point is considered as a noise point and deleted from the initial point cloud subset. The final point cloud subset is point Nearby road point cloud data.

[0044] In the point cloud subset, calculate each point To point The Euclidean distance di of the point is selected as the nearest neighbor. . As the first point of the road reference line, it is reversely mapped back to the camera coordinate system to obtain the corresponding screen coordinates .by As the starting point, repeat the above process and search for the next nearest neighbor point in the remaining point cloud subset as the second point of the road reference line. And so on, select the third, fourth, ..., nth reference point until the preset number of reference points is met or the end point of the road is reached.

[0045] When generating road reference lines according to the selection order of reference points, first , i=1, 2, ..., n, connect each reference point in the order of selection to form a broken line. The Euclidean distance The reference points are selected from near to far, so the distance between adjacent reference points is small, and the generated polyline can better fit the direction of the road.

[0046] In order to make the reference line smoother, several new points can be generated by interpolation between adjacent reference points. and The distance between , we can calculate the number of interpolation points m=floor(d / s) according to the preset interpolation interval s, and then and Insert m new points uniformly between The coordinates of the interpolation point qj can be calculated by linear interpolation: ; After the interpolation is completed, the original reference points and interpolation points are merged to obtain a denser point set ,in , Indicates the number of interpolation points between the i-th and i+1-th reference points.

[0047] Point Set For local smoothing, the B-spline curve fitting method can be used. The B-spline curve is a parametric curve that is represented by a linear combination of control points and basis functions. If the order of the B-spline curve is k, then the number of its control points is N+k. The coordinates of the control points can be solved by the least squares method so that the B-spline curve fits the point set. After solving the control points, the sampling points on the curve are calculated through the parametric equation of the B-spline curve, which are used as the reference line points after smoothing.

[0048] To improve the accuracy of the reference line, you can The principal component analysis (PCA) method is used to calculate the main direction of the local point cloud. As the center, all point cloud data within the spherical neighborhood with a radius of r are selected to form a local point cloud set .right Perform PCA analysis and calculate the eigenvalues ​​and eigenvectors of the covariance matrix. The eigenvector corresponding to the maximum eigenvalue is Represents the main direction of the local point cloud, the eigenvector corresponding to the second largest eigenvalue Indicates the secondary direction perpendicular to the main direction. The tangent direction at With the main direction Alignment makes the reference line perpendicular to the local road boundary. and If the angle between and Align. Tangent direction after alignment It can be expressed as: ;according to Update reference line points , and re-fit the B-spline curve to obtain the corrected reference line. Through the above steps, a reference line that is consistent with the direction of the road and whose local direction is perpendicular to the road boundary can be obtained. This reference line comprehensively considers the spatial distribution characteristics and local geometric characteristics of the road point cloud, and can better represent the center line of the road. In practical applications, the parameters of the algorithm can also be adjusted according to the type, width, curvature and other attributes of the road, such as the interpolation interval, neighborhood radius, and the order of the B-spline curve, to obtain a more accurate and stable reference line. At the same time, the reference line can be further optimized and corrected in combination with other road features, such as lane lines, curbs, etc., to improve the accuracy and integrity of road modeling.

[0049] According to the road reference line, the road boundary and center line are generated through geometric transformation; specifically, every two adjacent points on the road reference line are traversed. and , establish their straight line equations: ; Then, calculate the unit normal vector of the line equation The unit normal vector can be obtained by cross product It is obtained by normalizing with a non-parallel vector (such as (0, 0, 1)) to obtain a unit vector. According to the unit normal vector n and the preset road width w, two translation equations are obtained, which represent the candidate points of the left and right road boundaries respectively: Translation equation 1: ; Translation equation 2: ; Solve translation equation 1 and translation equation 2 to get the intersection point , as candidate points of the road boundary. In order to ensure that the generated road boundary points are consistent with the road direction, the candidate points need to be further screened.

[0050] Calculate candidate points The road boundary point determined at the previous time step The vector formed , and adjacent reference points and The vector formed . Calculate the vector and The angle θ is expressed by the formula ;in, Representation vector and The dot product of and Represents vectors and The mold length.

[0051] When generating road boundary points, in order to ensure that the boundary points are smooth and continuous with the road direction and avoid sharp corners, candidate points need to be screened. Specifically, the candidate points are calculated. The road boundary point determined at the previous time step The connection vector , and the line vector between adjacent reference points P1 and P2 , calculate the vector and If the angle θ is less than the preset threshold , such as 10°, then the candidate point is considered In line with the road direction, keep as the new road boundary point.

[0052] If the angle θ is greater than or equal to the threshold , then the candidate point is considered The road direction has deviated and needs to be adjusted. The specific adjustment method is: discard the current translation equation 2, halve the preset road width w, recalculate the translation equation 2, and get a new candidate point Then, recalculate the new candidate points The boundary point with the previous road The angle θ is determined to be less than the threshold value. If it is still greater than or equal to , then continue to halve the road width w and repeat the above process until a candidate point that meets the angle condition is found , or the road width w is reduced to a certain lower limit (such as 0.1m).

[0053] Through the above adjustment process, when the road width changes or the road is curved, the position of the road boundary point can be adaptively adjusted to make it as parallel to the road direction as possible, avoiding sharp corners or the boundary point deviating from the center of the road. At the same time, since the road width w is gradually reduced, the adjusted boundary point is still within the original road width range, and the boundary point will not be completely out of the road. Traverse all adjacent reference point pairs and obtain the left road boundary point set and the right road boundary point set In order to facilitate subsequent processing, it is necessary to and Sort the points by their road direction coordinate values ​​(such as x coordinates). After sorting, the points in the point set will be arranged in order from the starting point to the end point in the road direction to form an ordered point set. This ensures that the road centerline points generated later are also arranged in order in the road direction.

[0054] In order to generate the road centerline, the sorted left and right boundary point sets need to be and Perform equal-proportional division to obtain equal number of sampling point sets and First, the number of sampling points m is calculated based on the length of the road (such as the difference in the road direction coordinate values ​​of the left and right boundary points) and the expected centerline point density (such as the number of centerline points per meter of road). and Select m points uniformly from the sample point set and Specifically, for the i-th sampling point, its road direction coordinate value is It can be calculated by linear interpolation: ;in, and Respectively The minimum and maximum values ​​of the road direction coordinates. ,exist Find and The closest point, as The i-th sampling point in . The generation method of is similar.

[0055] Finally, connect and The midpoint of the sampling points corresponding to the subscript in the form of a road centerline point set The i-th centerline point Coordinates Can be The i-th point in and The i-th point in The coordinates of are averaged to obtain: ; ; ;in, and Respectively and The coordinates of .

[0056] Through the above steps, the left and right road boundary point sets are generated using the road reference line. and , and the road centerline point set , completing the reconstruction of the road geometry model. This method provides road direction and width information through reference lines, while taking into account the smoothness and continuity of the road direction, and adaptively adjusts the road width and centerline point density, which can better adapt to roads of different shapes and widths.

[0057] The DBSCAN algorithm is used to cluster and extract the edge of the point cloud within the preset range of the road boundary, and the B-spline curve is used for smoothing to obtain the final road boundary as the drawn map road; First, we get the left road boundary point set and the right road boundary point set , which respectively contain and For Each boundary point in and Each boundary point in , taking this point as the center and the road half-width w / 2 as the radius, extract the or is the center of the sphere, and w / 2 is the point cloud subset with the radius of the sphere or Point cloud subset or Contains boundary points or The nearby point cloud data excludes irrelevant points that are far away, providing a more accurate and efficient data basis for subsequent clustering processing.

[0058] Then, for each point cloud subset or , clustering is performed using the DBSCAN algorithm. The DBSCAN algorithm requires setting two key parameters: the radius parameter ε and the minimum point number threshold MinPts. Among them, ε represents the maximum distance between two points. If the distance between two points is less than ε, they are considered to belong to the same cluster; MinPts represents the minimum number of points contained in a cluster. If the number of points in the ε-neighborhood of a point is less than MinPts, the point is marked as a noise point.

[0059] On the point cloud subset or When clustering, the DBSCAN algorithm first randomly selects or Take a point p in as the starting point and mark p as visited. With p as the center and ε as the radius, or Find all points whose distance to p is less than ε, forming the ε-neighborhood of p .if The number of points in If p is less than MinPts, p is marked as a noise point; if If p is greater than or equal to MinPts, a new cluster C is created, p is added to C, and All unvisited points in are added to the candidate set S.

[0060] Next, when the candidate set S is not empty, take a point q from S, take q as the center and ε as the radius, and or Find all points whose distance to q is less than ε, forming the ε-neighborhood of q .if Greater than or equal to MinPts, then All unvisited points in are added to the candidate set S; if q does not belong to any existing cluster, q is added to the current cluster C. When q is less than MinPts, mark q as a noise point. Repeat the above process until the candidate set S is empty, that is, the processing is completed. or For all points in or The DBSCAN clustering results include Clusters .

[0061] Through the above steps, the left road boundary point set Each point in , and get its corresponding point cloud subset of Clusters ; For each point in the right road boundary point set {Pr} , and get its corresponding point cloud subset of Clusters . These clusters exclude noise points and outliers, retain the main point cloud structure near the boundary points, and lay the foundation for subsequent boundary extraction and smoothing. The radius parameter ε of the DBSCAN algorithm can be set according to half of the road width w, such as ε=w / 2 or ε=w / 3, to ensure that the scale of the cluster is adapted to the road width. The minimum point count threshold MinPts can be set according to the point cloud density and noise level, and is usually between 10 and 50. A larger MinPts can improve the reliability of clustering, but may miss some small boundary features; a smaller MinPts can detect more boundary features, but may introduce more noise points.

[0062] For each cluster obtained by DBSCAN clustering (i represents the i-th boundary point, j represents the j-th cluster corresponding to the i-th boundary point), using the K-Means algorithm Find the distance boundary point The nearest point Specifically, All points in are used as the input of K-Means algorithm, and the number of cluster centers K is set to 1. As the initial cluster center. Through iterative optimization, we get Middle distance The nearest point .

[0063] Then, calculate To the straight line Distance ,in For the boundary point The adjacent boundary points of . Assume The coordinates of , then the distance The distance from a point to a straight line can be calculated using the formula: ; where “×” represents the cross product of the vector and “||” represents the modulus of the vector. For each boundary point , the closest point generated by all its clusters , select Distance The smallest point As The update point is: ;use Replace the original boundary point set , get the updated boundary point set. Repeat the above steps to process the boundary point set of the left road in turn Each point in and the right road boundary point set Each point in , until all boundary points are updated, and a new left road boundary point set is obtained. and the right road boundary point set . Include Points, Include points, usually , , because some clusters may not produce valid closest points. Finally, the updated left and right road boundary point sets are respectively and Perform cubic uniform B-spline curve fitting to obtain the left and right road boundary curves and .

[0064] right or The points in are sorted according to the road direction coordinate values ​​(such as x coordinates) to obtain the sorted boundary point set or Then, in or Select the first n+1 points as control points , where n is the number of control points, usually or , that is, the number of control points is 1 / 4 of the number of boundary points. , calculate the first-order difference quotient matrix: ;in, is the parameter value corresponding to the ith control point, usually ti=i / (n-1), that is, the parameter interval [0, 1] is divided into n-1 segments. Based on the first-order difference quotient matrix, the second-order difference quotient matrix is ​​calculated: ; According to the second-order difference quotient matrix, calculate the third-order difference quotient matrix: ; Using control points And each order difference quotient matrix, construct the cubic uniform B-spline basis function , and get the left or right road boundary curve or The parametric equations are: , ;in, is the ith cubic B-spline basis function, is the control point coordinate vector.

[0065] More specifically, the cubic B-spline basis function The specific expression is:

[0066]

[0067] Among them, i is the control point number, and its value range is ; u is the curve parameter, and its value range is u∈[0,1]; is a node vector satisfying ; is the length of the i-th node interval, defined as . Node vector There are many ways to select , the commonly used ones are uniform node vectors and quasi-uniform node vectors. For uniform node vectors, the node coordinates are distributed according to an arithmetic progression, that is: ; For quasi-uniform node vectors, the internal node coordinates are distributed according to an arithmetic progression, with k repeated nodes at the start and end, where k is the order of the B-spline curve. For a cubic B-spline curve, k=3. The definition of a quasi-uniform node vector is as follows: ; ; ; In practical applications, the choice of knot vectors needs to be determined based on the distribution of control points and the desired curve shape. Usually, for uniformly distributed control points, uniform knot vectors can be used; for situations where the control points are dense at the boundary and sparse in the middle, quasi-uniform knot vectors can be used to obtain a more uniform and smooth curve shape. Substituting the parametric equation of the road boundary curve, we can get the left and right road boundary curves and The complete expression is:

[0068]

[0069] in, and are the coordinate vectors of the left and right road boundary curve control points, and are the node vectors of the left and right road boundary curves, and are the lengths of the i-th node interval of the left and right road boundary curves respectively.

[0070] Through the above expressions, the coordinates of the left and right road boundary curves under any parameter u can be easily calculated: and , thus achieving smooth fitting and reconstruction of the road boundary. In actual programming implementation, the number of control points n and the node vector The value of , pre-calculate and store each basis function The expression coefficients in each node interval can improve the calculation efficiency. At the same time, it can also be combined with numerical integration and other methods to calculate the length of the fitting curve, tangent direction and other geometric characteristics to meet the needs of road boundary analysis and application. Output the left and right road boundary curves and The control point coordinates and calculation formula are used to complete the fitting of the left and right road boundaries.

Claims

1. A method for drawing a map road based on vtk and point cloud, characterized in that: include: Use vtk to obtain the point cloud data of the road, and use the octree to spatially divide the point cloud data to obtain the point cloud data after spatial division; Perform coordinate transformation on the point cloud data after spatial division, and align the z-axis of the point cloud coordinate system with the z-axis of the camera coordinate system; According to the point cloud data after coordinate transformation, the road reference line is drawn by interactively picking points on the screen; Generate road boundaries and center lines through geometric transformation according to the road reference line; The DBSCAN algorithm is used to cluster and extract the edges of the point cloud within the preset range of the road boundary, and the B-spline curve is used for smoothing to obtain the final road boundary as the drawn map road; Draw road reference lines, including: Through screen interaction, select point P located at the road boundary on the point cloud data after coordinate transformation. view , click P view Located in the camera coordinate system; Point P view According to the transformation matrix, the corresponding point P is obtained in the point cloud coordinate system. world ; Point P world To query the point, use the octree to search for point P world Point cloud subset within a preset range; In the point cloud subset, calculate each point P i To point P world The Euclidean distance of nearest ; Point P nearest As the first point of the road reference line, point P nearest Reversely map back to the camera coordinate system to get the corresponding screen coordinates; Taking the screen coordinates as the starting point, repeat the above process and select the second reference point to the nth reference point; Connect all reference points in sequence according to the selection order of each point to generate a road reference line; Generate road boundaries and center lines through geometric transformation, including: Traverse the points on the road reference line, for every two adjacent points P1(x1,y1,z1) and P2(x2,y2,z2): Establish the straight line equation of P1 and P2: Calculate the unit normal vector n of the line equation = (n x ,n y ,n z ); According to the unit normal vector n = (n x ,n y ,n z ) and road width w, we get translation equation 1 and translation equation 2; Solve the translation equation 1 and the translation equation 2 to get the intersection point P c (x c ,y c ,z c ), as the road boundary candidate point; Calculate the vector v1 = (x c -x p ,y c -y p ,z c -z p ), where (x p ,y p ,z p ) is the coordinate of the road boundary point determined in the previous time step; Calculate vector v2 = (x2-x1, y2-y1, z2-z1); Calculate the angle between vector v1 and vector v2 If θ is less than θ0, where θ0 is the preset threshold, then keep P c As a new road boundary point; If θ is greater than or equal to θ0, discard the second translation equation and let Return to recalculate the intersection point P c , until θ is less than θ0; After the traversal is completed, the left road boundary point set {P l } and the right road boundary point set {P r }; Respectively {P l } and {P r }Sort the points according to their road direction coordinate values; After the proportional division and sorting, {P l } and {P r }, and obtain a set of equal number of sampling points {P l '} and {P r '}; Connect {P l '} and {P r '}, forming an ordered point set {P c }, as the road centerline; Output {P l }、{P r } and {P c }, serving as the left road boundary, the right road boundary and the road centerline respectively; The translation equation is expressed as follows: The second translation equation is as follows: Where w represents the road width.

2. The method for drawing a map road based on vtk and point cloud according to claim 1, characterized in that: Align the z-axis of the point cloud coordinate system with the z-axis of the camera coordinate system, including: Construct the transformation matrix between the camera coordinate system and the point cloud coordinate system: Among them, (x, y, z0 represents the point in the camera coordinate system, R represents the rotation matrix, T represents the translation vector, (x w ,y w ,z w ) represents a point in the point cloud coordinate system; According to the transformation matrix, each point cloud data point P after the space is divided cloud (x w ,y w ,z w ) multiplied by the transformation matrix to obtain the transformed point P camera (x,y,z); The transformed point P camera (x, y, z) is the point cloud data point after coordinate transformation; According to all the transformed point cloud data points, the coordinate system of the point cloud is updated to obtain point cloud data in which the z axis of the point cloud coordinate system is aligned with the z axis of the camera coordinate system.

3. The method for drawing a map road based on vtk and point cloud according to claim 1, characterized in that: Using octree to search for point P world A subset of the point cloud within a preset range, including: Set to point P world The search range of the cuboid centered at ; Starting from the root node of the octree, recursively traverse each node: If the bounding box of the current node is within the cuboid search range, the point cloud data corresponding to the corresponding node is added to the point cloud subset; If the bounding box of the current node partially overlaps with the cuboid search range, perform the following steps: If the number of point clouds contained in the current node is less than the preset threshold, all point cloud data contained in the current node are added to the point cloud subset; otherwise, the above judgment is recursively performed on its eight child nodes; If the bounding box of the current node does not intersect with the cuboid search range, the corresponding node is skipped; After the recursion is completed, all the point cloud data obtained by the search are merged as point P world The initial point cloud subset of Calculate the normal vector of each point in the initial point cloud subset, and calculate the angle between the normal vector and the preset road normal vector; If the angle is greater than the threshold, the corresponding point is regarded as a noise point and deleted from the initial point cloud subset to obtain the final point cloud subset.

4. The method for drawing a map road based on vtk and point cloud according to any one of claims 2 or 3, characterized in that: The DBSCAN algorithm is used to cluster and extract edges of point clouds within the preset range of the road boundary, including: Get the left road boundary point set {P l } and the right road boundary point set {P r }; Respectively {P l } and {P r Each boundary point P in i : P i As the center, the road half width w / 2 is the radius, extract P i Nearby point cloud subset {S i }; Using DBSCAN algorithm to search the point cloud subset {S i } clustering, and get K i Clusters; For each cluster C ij (j=1,2,......,K i ): Using K-Means algorithm in C ij Get the distance P i The nearest point Q ij ; Calculate Q ij To the straight line P i -P j The distance d ij , where P j are adjacent boundary points; In {Q ij }Select d ij The smallest point Q i As the boundary point P i Update point; Use Q i Replace P i , update the boundary point set; Repeat until {P l } and {P r All boundary points in} are updated, and the new boundary point set {P l ”} and {P r ”}; Respectively in {P l ”} and {P r ”} is fitted using a cubic uniform B-spline curve to obtain the left and right road boundary curves C l (u) and C r (u); Output C l (u) and C r (u) serves as the final road boundary.

5. The method for drawing a map road based on vtk and point cloud according to claim 4, characterized in that: Use DBSCAN algorithm to analyze the point cloud subset S i Perform clustering, including: Set the radius parameter ε and the minimum point threshold MinPts of the DBSCAN algorithm; Randomly select S i A point p in , mark p as visited; With p as the center and ε as the radius, find S i The set N(p) of all points whose distance from p is less than ε; If N(p)<MinPts, then p is marked as a noise point; If N(p) ≥ MinPts, create a new cluster C, add p to C, and add all unvisited points in N(p) to the candidate set S; When S is not empty, execute: Take a point q from S; With q as the center and ε as the radius, find S i The set N(p) of all points whose distance from q is less than ε; If N(p) ≥ MinPts, add all unvisited points in N(p) to S; If q does not belong to any cluster, add q to C; When N(p)<MinPts, mark q as a noise point; Output S i All clusters obtained in {C ij ], as the result of DBSCAN clustering.

6. The method for drawing a map road based on vtk and point cloud according to claim 5, characterized in that: Respectively in {P l ”} and {P r ”} is fitted using a cubic uniform B-spline curve to obtain the left and right road boundary curves C l (u) and C r (u) including: P l ”} or {P r The point set in ”} is sorted according to the road direction coordinate value to obtain the sorted boundary point set {P li ”} or {P ri ”}; Select {P li ”} or {P ri The first n+1 points in ”} are used as control points {d i }i=0,1,2,......,n; According to the control point {d i }, calculate {d i The first-order difference entropy matrix of}: According to the first-order difference quotient matrix, calculate {d i The second order difference entropy matrix of}: According to the second-order difference quotient matrix, calculate the third-order difference quotient matrix: Based on the control point {d i } and each order difference quotient matrix, using cubic uniform B-spline basis function to construct the left or right road boundary curve C l (u) or C r (u) parametric equation; Output C l (u) and C r (u) are the left and right road boundary curves obtained by fitting.

7. The method for drawing a map road based on vtk and point cloud according to claim 6, characterized in that: The parametric equation expression is as follows: Where u represents the parameter variable of the B-spline curve, l represents the left boundary of the road, r represents the right boundary of the road, and d i represents the coordinates of the i-th control point; n represents the degree of the B-spline curve; N i,3 (u) represents the i-th cubic B-spline basis function.

Images