CN107818598B - Three-dimensional point cloud map fusion method based on visual correction - Google Patents

Abstract

The invention discloses a three-dimensional point cloud map fusion method based on vision correction, comprising the following steps: 1) processing two point cloud maps to be fused; 2) extracting 3D-SIFT key points of the two three-dimensional point cloud maps; 3 ) Extract IPFH features on 3D-SIFT key points; 4) Find feature matching points by calculating the Euclidean distance between the feature points; 5) Calculate the transformation matrix and rotate the point cloud map; 6) Use the ICP algorithm to combine the two point cloud maps. mix together. The invention extends the SIFT feature into the three-dimensional point cloud, and extracts the 3D-SIFT key points, thereby ensuring the robustness of the feature to the rotation and transformation of the viewing angle; by extracting the IPFH feature, the problem of the wrong weight coefficient of the original FPFH feature is overcome, and at the same time This feature integrates the geometric characteristics of neighboring points to characterize the characteristics of a three-dimensional point, which greatly improves the stability of the algorithm. Through such processing, the present invention can fuse together two three-dimensional point cloud maps with greatly different viewing angles.

Description

A 3D point cloud map fusion method based on vision correction

technical field

The invention relates to the field of computer vision, mainly relates to three-dimensional point cloud map fusion, and specifically provides a method based on vision correction, which can be used to solve the problem of point cloud map fusion failure due to excessive viewing angle differences.

Background technique

With the rapid development of stereo cameras, 3D point cloud data has been widely used in the field of computer vision, such as robot construction of maps, navigation, object recognition, and tracking. When the robot is in a vast external environment or a complex internal environment, although SLAM can build a map to solve the robot's autonomous navigation, due to the complexity and vastness of the environment, various problems will occur when building a map, such as time consumption in the process of building a map. So long that navigation fails. Therefore, in this case, multiple robots are required to jointly build a map, and then stitch their respective maps into a complete map, so as to realize the sharing of maps, which can improve the efficiency of autonomous navigation.

Point cloud map fusion refers to the fusion of point clouds from multiple perspectives through rigid body transformation through point cloud maps collected from different perspectives and locations. Besl, P.J. & McKay, N.d. (1992). A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence, 14(2), 239-256. The Iterative Closest Point (ICP) algorithm is disclosed by finding the nearest Corresponding points, the transformation matrix is estimated, the minimum value of the objective function is calculated, and finally after many iterations, the transformation matrix is calculated, and then the two point clouds are fused together. Only when the algorithm provides an accurate initial value, can a relatively good fusion result be obtained.

In order to calculate the accurate initial value, many researchers calculate the initial value by looking for feature matching. Klasing et al. adopted methods based on optimal averaging to estimate normals, such as PlanePCA, PlaneSVD, QuadSVD methods. However, the normal vector of the point cloud contains less data information, and the change of the point cloud map perspective will lead to the calculation of the point cloud normal vector, which will ultimately affect the effect of point cloud map fusion. Gelfand provides a method for extracting point cloud volume, however, this method does not adapt to point cloud map perspective transformation. Zou calculates the initial value by extracting the Gaussian curvature of the point cloud, but with the change of the point cloud map perspective, the Gaussian curvature of the same object changes, which will lead to wrong feature matching, and thus the point cloud map fusion fails. Rusu et al. proposed a method to extract FPFH features from point clouds, but this method has the problems of weight coefficient overflow and feature matching errors. The RANSAC algorithm is another fusion algorithm. It was originally proposed by Fischler et al. It uses random sampling consistency to match a mathematical model. The algorithm is relatively stable, but the complexity of the method will reach O(n3) in the worst case. When When the number of point clouds is huge, the time consumption of the algorithm is unacceptable.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a three-dimensional point cloud map fusion method based on vision correction with high fusion precision and high efficiency, so as to solve the problems raised in the above background art.

To achieve the above object, the present invention provides the following technical solutions:

A three-dimensional point cloud map fusion method based on vision correction, comprising the following steps:

1) Preprocess the 3D point cloud map. First, input two 3D point cloud map files in pcd format to obtain two point cloud maps to be fused. These two point cloud maps are the target point cloud map point _tgt and The point cloud map point _src to be fused, and then preprocess the two point cloud maps;

2) For the two point cloud maps in step 1), extract the 3D-SIFT key points of the target point cloud map point _tgt and the point cloud map point _src to be fused, respectively;

3) Extract IPFH features from the two 3D-SIFT key points of the point cloud map in step 2), respectively, to obtain the feature vector P of the point cloud map point _src to be fused and the feature vector Q of the target point cloud map point _tgt , where the feature vector P has m feature points, and feature vector Q has n feature points;

4) Calculate the Euclidean distance of the feature points in the feature vector P and the feature vector Q, and find the feature matching point according to the minimum distance;

5) Calculate the rotation matrix through the feature matching points, and use the matrix to rotate the point cloud map point _tgt , change the perspective of the point cloud map, and reduce the perspective difference between the two point cloud maps;

6) The ICP precise fusion algorithm is used to fuse the two 3D point cloud maps after reducing the difference of viewing angles.

As a further solution of the present invention: in step 1), the preprocessing includes denoising and sampling.

As a further scheme of the present invention: in step 2), the concrete steps of 3D-SIFT key point extraction are:

Ⅰ. Detect extreme points in the point cloud scale space, downsample the point cloud map to different degrees to construct a pyramid model, and construct a Gaussian scale space for each set of point cloud maps. The functions used are as follows

Scale space: L(x,y,z,σ)=G(x,y,z,kσ)*P(x,y,z);

Difference of Gaussian function:

The parameter k refers to the number of groups in the pyramid, and σ is the scale;

Construct Gaussian difference space: D(x,y,z, ^ki σ)=L(x,y,z,ki ⁺¹ σ)-L(x,y,z, ^ki σ);

After the DoG space is constructed, the extreme point is found by the method of comparison: for a point, when its value is greater than or less than all points in its neighborhood, it is regarded as an extreme point;

Ⅱ. Determine the direction of key points, and calculate the azimuth and elevation angles of each point in the neighborhood where the extreme point is located. The calculation formula is as follows:

In the formula, d represents the distance between the extreme point and the center point of the neighborhood, θ represents the azimuth angle, φ represents the elevation angle, and x ₀ , y ₀ , and z ₀ refer to the center point in the neighborhood where the extreme point is located;

Count the directions of the azimuth angle θ and the elevation angle φ of each point in the neighborhood: use two histograms to count the directions of the two angles of each point, and the value range of the direction of θ is [0°, 360°]. For the statistical direction of the graph, each column represents 45 degrees; while the value of φ is [-90°, 90°], a 4-column histogram is used to count the direction, and each column represents 45 degrees;

After statistics, the peaks of the two histograms are taken as the azimuth and elevation angles of the extreme points. After calculation, each key point contains x, y, z coordinate information, scale information and direction information, namely (x, y, z ,σ,θ,φ);

Ⅲ. Rotate the coordinate system where a key point p is located to the main direction of the key point. Since the main direction has been calculated before, the following rotation formula is used for calculation

where _pi is the point before rotation, and _pi ′ represents the point after rotation;

Use the rotated point p _i ' to recalculate the aforementioned parameters, including d, θ, φ, and then calculate the geometric relationship between the rotated key point p' and the neighboring point p _i ', the The geometric relationship refers to the angle δ between the vector _p'pi ' and the normal vector n at the key point p before the rotation. The calculation of the angle is as follows, where the numerator represents the dot product of the two vectors, and the denominator Represents the length of the two vectors. For a key point and all neighbor points in the neighborhood, triple data information (θ, φ, δ) will be generated

Divide the three angles of several triples to generate feature descriptors.

As a further scheme of the present invention: in step 3), the specific steps of extracting IPFH features from the area around the 3D-SIFT key point are:

Ⅰ. At the 3D-SIFT key point p(x, y, z, σ, θ, φ), take this point as the center, determine the k neighborhood, and connect the points in the neighborhood to construct point pairs, and calculate each group. the normal vector of the point pair;

Ⅱ. A local u-v-w coordinate system is constructed between each group of point pairs, and the three coordinate axes are defined as follows:

Four eigenvalues are calculated according to the local coordinate system and normal vector. The calculation method is shown in the following formula. The four eigenvalues are called SPFH.

Ⅲ. In the two point cloud maps, re-select the k neighborhood, and use the SPFH adjacent to each 3D point to synthesize IPFH. The synthesis method is as follows:

ω ₀ refers to the average value of the distance between the query point p and each neighborhood point, and ω _i represents the distance between the query point p and a neighboring point p _i in the neighborhood space;

Ⅳ. Count the values of f1, f3, and f4 of each point. Each value is divided into 11 intervals. IPFH uses 33-dimensional data information to describe.

As a further scheme of the present invention: in step 4), the IPFH features are matched, the corresponding relationship is determined, and the criterion is the distance between the feature vectors, and the following matching strategy is adopted: take the IPFH of point cloud map point _src a point A, at the point Find two points B and C closest to the vector A in the cloud map point _tgt , and calculate by the following formula. If the formula conditions are met, select B as its matching point

As a further scheme of the present invention: in step 5), after finding the feature matching points, adopt the method of singular value decomposition to calculate the transformation matrix, and rotate the point cloud map point _src according to the transformation matrix, thereby reducing two point clouds The viewing angle between the maps is different, and the viewing angle correction is completed.

Compared with the prior art, the beneficial effects of the present invention are:

1. The present invention uses 3D-SIFT to extract key points, calculates FPFH features on the key points, and finally calculates the initial rough estimate, which provides an initial transformation matrix for the traditional point cloud registration method, which can reduce the iteration of the registration algorithm. times, improve the registration success rate and accuracy of the algorithm, and reduce the computational cost.

2. The present invention extends the SIFT feature to the three-dimensional point cloud and extracts 3D-SIFT key points, thereby ensuring the robustness of the feature to view rotation and transformation; by extracting the IPFH feature, the problem of the wrong weight coefficient of the original FPFH feature is overcome At the same time, this feature integrates the geometric characteristics of neighboring points to characterize the characteristics of a three-dimensional point, which greatly improves the stability of the algorithm. Through such processing, the present invention can fuse together two three-dimensional point cloud maps with greatly different viewing angles.

Description of drawings

FIG. 1 is a schematic flowchart of a three-dimensional point cloud map fusion method based on vision correction.

FIG. 2 is a schematic diagram of extracting key points using 3D-SIFT in a 3D point cloud map fusion method based on vision correction.

FIG. 3 is a schematic diagram of feature matching using FPFH in a 3D point cloud map fusion method based on vision correction.

FIG. 4 is an effect diagram of point cloud fusion performed by the 3D point cloud map fusion method based on vision correction.

Detailed ways

The technical solutions of the present invention will be described in further detail below in conjunction with specific embodiments.

Please refer to Figure 1, a 3D point cloud map fusion method based on vision correction, including the following steps:

1) Preprocess the 3D point cloud map. First, input two 3D point cloud map files in pcd format to obtain two point cloud maps to be fused. These two point cloud maps are the target point cloud map point _tgt and The point cloud map point _src to be fused, and then preprocess the two point cloud maps, including denoising and sampling;

2) For the two point cloud maps in step 1), extract the 3D-SIFT key points of the target point cloud map point _tgt and the point cloud map point _src to be fused, respectively;

The specific steps of 3D-SIFT key point extraction are:

1. Detect extreme points in the point cloud scale space, downsample the point cloud map to different degrees to construct a pyramid model, and construct a Gaussian scale space for each set of point cloud maps. The functions used are as follows:

Scale space: L(x,y,z,σ)=G(x,y,z,kσ)*P(x,y,z);

Difference of Gaussian function:

The parameter k refers to the number of groups in the pyramid, and σ is the scale;

Construct Gaussian difference space: D(x,y,z, ^ki σ)=L(x,y,z,ki ⁺¹ σ)-L(x,y,z, ^ki σ);

After the DoG space is constructed, the extreme point is found by the method of comparison: for a point, when its value is greater than or less than all points in its neighborhood, it is regarded as an extreme point;

Ⅱ. Determine the direction of key points, and calculate the azimuth and elevation angles of each point in the neighborhood where the extreme point is located. The calculation formula is as follows:

In the formula, d represents the distance between the extreme point and the center point of the neighborhood, θ represents the azimuth angle, φ represents the elevation angle, and x ₀ , y ₀ , and z ₀ refer to the center point in the neighborhood where the extreme point is located;

Count the directions of the azimuth angle θ and the elevation angle φ of each point in the neighborhood: use two histograms to count the directions of the two angles of each point, and the value range of the direction of θ is [0°, 360°]. For the statistical direction of the graph, each column represents 45 degrees; while the value of φ is [-90°, 90°], a 4-column histogram is used to count the direction, and each column represents 45 degrees;

After statistics, the peaks of the two histograms are taken as the azimuth and elevation angles of the extreme points. After calculation, each key point contains x, y, z coordinate information, scale information and direction information, namely (x, y, z ,σ,θ,φ);

Ⅲ. Rotate the coordinate system where a key point p is located to the main direction of the key point. Since the main direction has been calculated before, the following rotation formula is used for calculation

where _pi is the point before rotation, and _pi ′ represents the point after rotation;

Use the rotated point p _i ' to recalculate the aforementioned parameters, including d, θ, φ, and then calculate the geometric relationship between the rotated key point p' and the neighboring point p _i ', the The geometric relationship refers to the angle δ between the vector _p'pi ' and the normal vector n at the key point p before the rotation. The calculation of the angle is as follows, where the numerator represents the dot product of the two vectors, and the denominator Represents the length of the two vectors. For a key point and all neighbor points in the neighborhood, triple data information (θ, φ, δ) will be generated

Divide the three angles of several triples to generate feature descriptors;

3) Extract IPFH features from the two 3D-SIFT key points of the point cloud map in step 2), respectively, to obtain the feature vector P of the point cloud map point _src to be fused and the feature vector Q of the target point cloud map point _tgt , where the feature vector P has m feature points, and feature vector Q has n feature points;

The specific steps for extracting IPFH features from the area around 3D-SIFT key points are:

Ⅰ. At the 3D-SIFT key point p(x, y, z, σ, θ, φ), take this point as the center, determine the k neighborhood, and connect the points in the neighborhood to construct point pairs, and calculate each group. the normal vector of the point pair;

Ⅱ. A local u-v-w coordinate system is constructed between each group of point pairs, and the three coordinate axes are defined as follows:

Four eigenvalues are calculated according to the local coordinate system and normal vector. The calculation method is shown in the following formula. The four eigenvalues are called SPFH.

Ⅲ. In the two point cloud maps, re-select the k neighborhood, and use the SPFH adjacent to each 3D point to synthesize IPFH. The synthesis method is as follows:

ω ₀ refers to the average value of the distance between the query point p and each neighborhood point, and ω _i represents the distance between the query point p and a neighboring point p _i in the neighborhood space;

Ⅳ. Count the values of f1, f3, and f4 of each point, each value is divided into 11 intervals, and IPFH uses 33-dimensional data information to describe;

4) Calculate the Euclidean distance of the feature points in the feature vector P and the feature vector Q, and find the feature matching point according to the minimum distance;

The IPFH feature is matched to determine the corresponding relationship. The judgment criterion is the distance between the feature vectors. The following matching strategy is adopted: take the IPFH of a point A in the point cloud map point _src , and find the two closest points to the A vector in the point cloud map point _tgt . Points B and C are calculated by the following formula. If the conditions of the formula are met, select B as the matching point

5) Calculate the rotation matrix through the feature matching points, and use the matrix to rotate the point cloud map point _tgt , change the perspective of the point cloud map, and reduce the perspective difference between the two point cloud maps;

After finding the feature matching points, the singular value decomposition method is used to calculate the transformation matrix, and the point cloud map point _src is rotated according to the transformation matrix, thereby reducing the viewing angle difference between the two point cloud maps and completing the viewing angle correction;

6) The ICP precise fusion algorithm is used to fuse the two 3D point cloud maps after reducing the difference of viewing angles.

In Figure 2, the effect of using 3D-SIFT to extract key points in the 3D point cloud map fusion method based on visual correction is shown. SIFT features are local features of the image, which remain invariant to rotation, scaling, brightness changes, and maintain a certain degree of stability to viewing angle changes, affine transformations, and noise.

In Fig. 3, a schematic diagram of further extracting FPFH features on the key points extracted by 3D-SIFT and performing feature matching in the 3D point cloud map fusion method based on visual correction is shown. FPFH is an improvement on the PFH feature. It is a point feature representation method that describes the local geometric feature information around a sample point in the form of a statistical histogram. It calculates the Euclidean distance of the feature points in the source and target point clouds, according to the minimum Find the feature matching points by distance, and calculate the rotation matrix through the feature matching points.

In Figure 4, the test and experimental results of the 3D point cloud map fusion method based on vision correction are shown. This experiment uses three sets of data sets, of which the first two sets of data are from indoor point clouds collected using Xtion Pro stereo sensors. Data; another set of point clouds from outdoor parts of the city collected by a 3D laser scanner provided by PCL. The first and second columns of the picture represent the source and target point clouds respectively, the third and fourth columns are the fused point clouds, and the fourth column uses different colors to represent the result of the fusion of the two point clouds .

The invention uses 3D-SIFT to extract key points, calculates FPFH features on the key points, and finally calculates an initial rough estimate, provides an initial transformation matrix for the traditional point cloud registration method, and can reduce the number of iterations of the registration algorithm. Improve the registration success rate and accuracy of the algorithm, and reduce the computational cost.

The invention extends the SIFT feature into the three-dimensional point cloud, and extracts the 3D-SIFT key points, thereby ensuring the robustness of the feature to the rotation and transformation of the viewing angle; by extracting the IPFH feature, the problem of the wrong weight coefficient of the original FPFH feature is overcome, and at the same time This feature integrates the geometric characteristics of neighboring points to characterize the characteristics of a three-dimensional point, which greatly improves the stability of the algorithm. Through such processing, the present invention can fuse together two three-dimensional point cloud maps with greatly different viewing angles.

The preferred embodiments of the present invention have been described in detail above, but the present invention is not limited to the above-mentioned embodiments. Within the scope of knowledge possessed by those of ordinary skill in the art, various aspects can also be made without departing from the purpose of the present invention. kind of change.

Claims (5)

1. a three-dimensional point cloud map fusion method based on vision correction, is characterized in that, comprises the following steps: 1) Preprocess the 3D point cloud map. First, input two 3D point cloud map files in pcd format to obtain two point cloud maps to be fused. These two point cloud maps are the target point cloud map point _tgt and The point cloud map point _src to be fused, and then preprocess the two point cloud maps; 2) For the two point cloud maps in step 1), extract the 3D-SIFT key points of the target point cloud map point _tgt and the point cloud map point _src to be fused, respectively; 3) Extract IPFH features from the two 3D-SIFT key points of the point cloud map in step 2), respectively, to obtain the feature vector P of the point cloud map point _src to be fused and the feature vector Q of the target point cloud map point _tgt , where the feature vector P has m feature points, and feature vector Q has n feature points; 4) Calculate the Euclidean distance of the feature points in the feature vector P and the feature vector Q, and find the feature matching point according to the minimum distance; 5) Calculate the rotation matrix through the feature matching points, and use the matrix to rotate the point cloud map point _tgt , change the perspective of the point cloud map, and reduce the perspective difference between the two point cloud maps; 6) Using the ICP precise fusion algorithm to fuse the two 3D point cloud maps after reducing the difference in perspective; In step 3), the specific steps for extracting IPFH features from the area around the 3D-SIFT key point are: Ⅰ. At the 3D-SIFT key point p(x, y, z, σ, θ, φ), take this point as the center, determine the k neighborhood, and connect the points in the neighborhood to construct point pairs, and calculate each group. the normal vector of the point pair; Ⅱ. A local u-v-w coordinate system is constructed between each group of point pairs, and the three coordinate axes are defined as follows:

Four eigenvalues are calculated according to the local coordinate system and normal vector. The calculation method is shown in the following formula. The four eigenvalues are called SPFH.

Ⅲ. In the two point cloud maps, re-select the k neighborhood, and use the SPFH adjacent to each 3D point to synthesize IPFH. The synthesis method is as follows:

ω ₀ refers to the average value of the distance between the query point p and each neighborhood point, and ω _i represents the distance between the query point p and a neighboring point p _i in the neighborhood space; Ⅳ. Count the values of f1, f3, and f4 of each point. Each value is divided into 11 intervals. IPFH uses 33-dimensional data information to describe. 2 . The three-dimensional point cloud map fusion method based on vision correction according to claim 1 , wherein, in step 1), the preprocessing includes denoising and sampling. 3 . 3. the three-dimensional point cloud map fusion method based on vision correction according to claim 1, is characterized in that, in step 2), the concrete steps of 3D-SIFT key point extraction are: Ⅰ. Detect extreme points in the point cloud scale space, downsample the point cloud map to different degrees to construct a pyramid model, and construct a Gaussian scale space for each set of point cloud maps. The functions used are as follows Scale space: L(x,y,z,σ)=G(x,y,z,kσ)*P(x,y,z); Difference of Gaussian function:

The parameter k refers to the number of groups in the pyramid, and σ is the scale; Construct Gaussian difference space: D(x,y,z, ^ki σ)=L(x,y,z,ki ⁺¹ σ)-L(x,y,z, ^ki σ); After the DoG space is constructed, the extreme point is found by the method of comparison: for a point, when its value is greater than or less than all points in its neighborhood, it is regarded as an extreme point; Ⅱ. Determine the direction of key points, and calculate the azimuth and elevation angles of each point in the neighborhood where the extreme point is located. The calculation formula is as follows:

In the formula, d represents the distance between the extreme point and the center point of the neighborhood, θ represents the azimuth angle, φ represents the elevation angle, and x ₀ , y ₀ , and z ₀ refer to the center point in the neighborhood where the extreme point is located; Count the directions of the azimuth angle θ and the elevation angle φ of each point in the neighborhood: use two histograms to count the directions of the two angles of each point, and the value range of the direction of θ is [0°, 360°]. For the statistical direction of the graph, each column represents 45 degrees; while the value of φ is [-90°, 90°], a 4-column histogram is used to count the direction, and each column represents 45 degrees; After statistics, the peaks of the two histograms are taken as the azimuth and elevation angles of the extreme points. After calculation, each key point contains x, y, z coordinate information, scale information and direction information, namely (x, y, z ,σ,θ,φ); Ⅲ. Rotate the coordinate system where a key point p is located to the main direction of the key point. Since the main direction has been calculated before, the following rotation formula is used for calculation

where _{pi is the point before rotation, and p′ i represents} the point after rotation; Use the rotated point p′ _i to recalculate the aforementioned parameters, including d, θ, φ, and then calculate the geometric relationship between the rotated key point p and the neighboring point p′ _i . The relationship refers to the angle δ between the vector p'p _i ' and the normal vector n at the key point p before the rotation. The calculation of this angle uses the following formula, where the numerator represents the dot product of the two vectors, and the denominator represents the The length of the two vectors, for a key point and all neighbor points in the neighborhood, triple data information (θ, φ, δ) will be generated

Divide the three angles of several triples to generate feature descriptors. 4. the three-dimensional point cloud map fusion method based on vision correction according to claim 1, is characterized in that, in step 4), IPFH feature is matched, confirms corresponding relation, and criterion is the distance between feature vectors, adopts following Matching strategy: Take the IPFH of a point A in the point cloud map point _src , find the two points B and C closest to the A vector in the point cloud map point _tgt , and calculate by the following formula. If the formula conditions are met, select B as the match point

5. the three-dimensional point cloud map fusion method based on vision correction according to claim 4, is characterized in that, in step 5), after finding the feature matching point, adopt the method for singular value decomposition to calculate the transformation matrix, according to the transformation matrix will be. The point cloud map point _src is rotated, thereby reducing the viewing angle difference between the two point cloud maps and completing the viewing angle correction.

Images