CN102945551A - Graph theory based three-dimensional point cloud data plane extracting method - Google Patents

Abstract

The invention relates to a graph theory based three-dimensional point cloud data plane extracting method, which comprises the following steps: firstly constructing a graph, corresponding each vertex in the graph to a data point in the three-dimensional point cloud, and calculating out the borders of the graph through a k-nearest neighbor method; at the same time, using a weighted partial plane fitting method to calculate plane normal vectors of each point; then, calculating the weighted value corresponding to each border, namely a difference value of the normal vectors of two vertexes corresponding to each border, and assigning an initial threshold value to each vertex in the graph; as for each border in the graph, if the difference value of the normal vectors is not larger than any one of the two regional threshold values, combining the two regions, wherein the threshold value of the new region is equal to the difference value of the normal vectors plus the initial value and dividing quantity of data points in the new region; and at last, as for each region, if the quantity of the data points is larger than the set threshold value, then determining that the region is a plane. Compared with the prior art, the method has the advantages of high plane extracting precision and strong noise interference prevention performance.

Description

A kind of three dimensional point cloud plane extracting method based on graph theory

Technical field

The present invention relates to a kind of Processing Method of Point-clouds, especially relate to a kind of three dimensional point cloud plane extracting method based on graph theory.

Background technology

Semantic map is the long-term objective that the mobile robot understands environment, and indoor environment comprises a lot of planes usually simultaneously, so it is the condition precedent that makes up semantic map, understands environment that plane extraction or plane are cut apart.As a rule, the plane extraction is the problem from one group of three-dimensional point centralized detecting plane.At present, the common method that extract on the plane is based on the method for plane mathematical model, such as stochastic sampling consistance (RANdom SAmple Consensus, RANSAC) method and hough transform method (Hough Transform Method), these methods can be extracted the plane that meets the plane mathematical model from three dimensional point cloud, but can't determine whether this plane is present in the actual scene really.In addition, also have region growing method (Region Growing Method), data clustering method etc., these methods can not well be processed the noise in the three dimensional point cloud.

Furniture in the indoor environment, such as chair, desk etc., it is indispensable understanding environment for the mobile robot, and these objects are often comprising the less plane of area that is not easy to be detected.During larger plane in detecting scene, such as wall, ceiling etc., these local facets often are left in the basket.So in order to understand better environment, need to extract local facet.Simultaneously, because inevitable noise in the inherent physical limitation of video camera and the data acquisition, usually contain noise, exterior point and cavity in the three-dimensional point cloud, because the existence of noise and have the limitation of areal model method in the data, has certain challenge so that extract than facet and high precision plane.

Summary of the invention

Purpose of the present invention is exactly to provide a kind of three dimensional point cloud plane extracting method based on graph theory for the defective that overcomes above-mentioned prior art existence, the method utilization figure (Graph) expression three dimensional point cloud and between relation, merge adjacent zone according to the limit on the figure, all planes in the three-dimensional point cloud can be extracted, comprise the plane that area is less; The plane of extracting simultaneously has very high precision.

Purpose of the present invention can be achieved through the following technical solutions:

A kind of three dimensional point cloud plane extracting method based on graph theory, the method comprises:

1) make up a non-directed graph G=(V, E), the summit in the V presentation graphs wherein, each summit represents a data point in the three-dimensional point cloud, and E represents the limit between adjacent two points, calculates by the k nearest neighbor algorithm;

2) adopt in the weight part plan approximating method Calculation of Three Dimensional point cloud each to put namely normal vector corresponding to each summit among the figure;

3) every weighted value that the limit is corresponding in the calculating chart, i.e. the normal vector difference on two summits corresponding to every limit, and the weighted value ascending order is pressed on all limits arrange; Simultaneously, compose an initial threshold for each summit among the figure;

4) to each the bar limit on the figure, if corresponding two summits are not or not same zone, calculate so the normal vector difference in two zones, if this normal vector difference is not more than in two region threshold any one, merge two zones, and the threshold value of new region equals the normal vector difference and adds that initial threshold is divided by the size of new region;

5) for each zone, if the data point number thinks then that greater than the threshold value of setting this zone is a plane.

Described step 2) be specially:

If one group of cloud data P={p ₁, p ₂..., p _nContaining n point, each puts p _i=(p _Ix, p _Iy, p _Iz) ^TThe k neighborhood be Q _i={ q _I1, q _I2, q _Ik, use T _iExpression point p _iTangent plane, then have:

Wherein, n _i=(n _Ix, n _Iy, n _Iz) be at a p _iTangent plane T _iNormal vector, d _iTo tangent plane T from true origin _iDistance,

Weighting function, and

By introducing Lagrange multiplier, obtain normal vector n _iBe matrix

Minimal eigenvalue characteristic of correspondence vector, wherein,

Generate according to Gaussian function:

σ is zooming parameter, σ=ρ r _Max, r _MaxP _iMaximum radius in the some k neighborhood scope, ρ is the convergent-divergent constant.

Described step 3) formula that calculates weighted value corresponding to every limit in is

w(v _i，v _j)＝α(1-|n _i·n _j|)

Wherein, w ((v _i, v _j)) be every limit (v among the non-directed graph G=(V, E) _i, v _j) corresponding weights, v _iAnd v _jBe two summits on this limit, these weights represent the similarity on two summits; n _i, n _jBe respectively vertex v _i, v _jNormal vector, α is amplification coefficient.

Described step 4) be specially:

With two vertex v on every limit _i, v _jRespectively as two regional S _i, S _j, and two normal vectors corresponding to zone are respectively

If there is w (S _i, S _j)≤T (S _i, S _j), then same zone is merged in these two zones, wherein, w (S _i, S _j) two adjacent area S _iAnd S _jThe normal vector difference, $w(S_i,S_j)=\mathrm{\α}(1-|n_{s_i}\·n_{s_j}\left|\right),$ T (S _i, S _j) be threshold function table,

T(S _i，S _j)＝max(H(S _i)+θ(S _i)，H(S _j)+θ(S _j))

H (S) is respectively the normal vector difference in two zones that merge regional S, the adjustment function of θ (S) expression and the inversely proportional relation of area size,

β is the initial threshold on each summit, also is the maximum similarity that can merge two summits, | S| is the size of regional S.

Compared with prior art, the present invention has the following advantages:

1) the inventive method is utilized graph theory to merge adjacent zone and is extracted plane in the three dimensional point cloud, can improve the precision that extract on the plane, and avoiding extracting only has mathematical meaning and non-existent plane in the actual scene;

2) the inventive method can be extracted all planes in the scene, and particularly the less plane of some areas can not be left in the basket because of the noise of three dimensional point cloud.

Embodiment

The present invention is described in detail below in conjunction with specific embodiment.

Embodiment

A kind of three dimensional point cloud plane extracting method based on graph theory, the method at first makes up a figure, a data point in each summit corresponding three-dimensional points cloud among the figure, the limit on the figure calculates by k neighbour (K-Nearest Neighbor, KNN) algorithm; Simultaneously, the planar process of each some vector in the Calculation of Three Dimensional point cloud, the present embodiment use a kind of weight part plan approximating method to estimate the planar process vector of each point.

Then, every weighted value that the limit is corresponding in the calculating chart, i.e. the normal vector difference on two summits corresponding to every limit, and the weighted value ascending order is pressed on all limits arrange, the limit that the normal vector difference is less like this will come the front; Simultaneously, compose an initial threshold for each summit among the figure.

Then, to each the bar limit on the figure, if corresponding two summits are not or not same zone, calculate so the normal vector difference in two zones, if this normal vector difference is not more than in two region threshold any one, merge two zones, and the threshold value of new region equals the normal vector difference and adds that initial threshold is divided by the size (number of new region data point) of new region.

At last, for each zone, if number of data points thinks then that greater than the threshold value of setting this zone is a plane.

The concrete steps of the present embodiment method comprise:

1) make up a non-directed graph G=(V, E), the summit in the V presentation graphs wherein, each summit represents a data point in the three-dimensional point cloud, and E represents the limit between adjacent two points.

2) adopt normal vector corresponding to each point in the weight part plan approximating method Calculation of Three Dimensional point cloud:

One group of cloud data P={p ₁, p ₂..., p _nContaining n point, each puts p _i=(p _Ix, p _Iy, p _Iz) ^TThe k neighborhood be Q _i={ q _I1, q _I2..., q _Ik, use T _iExpression point p _iTangent plane, then have:

Wherein, n _i=(n _Ix, n _Iy, n _Iz) be at a p _iTangent plane T _iNormal vector, d _iTo tangent plane T from true origin _iDistance,

Weighting function, and

With function f (n _i, d _i) represent T (n _i, d _i) polynomial expression, the normal vector problem can be expressed as:

$s.t.n_i^T{n}_i=1$

Wherein,

Define a Lagrangian function l (n _i, d _i, λ), represent Lagrange multiplier with λ, then have:

$l(n_i,d_i,\mathrm{\λ})=f(n_i,d_i)+\mathrm{\λ}(1-n_i^T{n}_i)$

Pass through partial differential equation: $\frac{\∂}{{\∂d}_i}l(n_i,d_i,\mathrm{\λ})=0$ With $\frac{\∂}{{\∂n}_i}l(n_i,d_i,\mathrm{\λ})=0,$ Can obtain

f(n _i，d _i)＝λ

Normal vector n _iBe matrix

Minimal eigenvalue characteristic of correspondence vector,

Wherein,

Generate according to Gaussian function: σ is zooming parameter, σ=ρ r _Max, r _MaxP _iMaximum radius in the some k neighborhood scope, ρ is the convergent-divergent constant.

3) every limit (v among the calculating non-directed graph G=(V, E) _i, v _j) corresponding weight w ((v _i, v _j)), v _iAnd v _jBe two summits on this limit, these weights represent the normal vector difference on two summits:

w(v _i，v _j)＝α(1-|n _i·n _j|)

n _i, n _jBe respectively vertex v _i, v _jNormal vector, α is amplification coefficient.

4) arrange by ascending order on all limits among the large young pathbreaker E according to corresponding weights.

5) with two vertex v on every limit _i, v _jRespectively as two regional S _i, S _j, and two normal vectors corresponding to zone are respectively

For the normal vector more than the zone of a point, the normal vector that the present embodiment adopts this zone initially to merge time point represents the normal vector that this is regional, if there is w (S _i, S _j)≤T (S _i, S _j), then same zone is merged in these two zones, wherein, w (S _i, S _j) two adjacent area S _iAnd S _jThe normal vector difference, $w(S_i,S_j)=\mathrm{\α}(1-|n_{s_i}\·n_{s_j}\left|\right),$ T (S _i, S _j) be threshold function table,

T(S _i，S _j)＝max(H(S _i)+θ(S _i)，H(S _j)+θ(S _j))

H (S) is respectively the normal vector difference in two zones that merge regional S, the adjustment function of θ (S) expression and the inversely proportional relation of area size,

β is the initial threshold on each summit, also is the maximum similarity that can merge two summits, | S| is the size of regional S.Normal vector difference and area size when the threshold value that obtains like this can merge according to every sub-region are regulated automatically, and when the normal vector difference in two zones is less, the merging threshold value in the corresponding new zone that merges is also corresponding less; Simultaneously, region area is larger, and then the threshold value increment diminishes relatively.

6) through after the above-mentioned steps, most three dimensional point cloud all can be assigned to a zone number, to each zone, if the data point number in should the zone is greater than the threshold value of setting, be a plane with this extracted region then, filter out the plane that the data point number does not reach requirement.

Claims (4)

1. the three dimensional point cloud plane extracting method based on graph theory is characterized in that, the method comprises:

1) make up a non-directed graph G=(V.E), the summit in the V presentation graphs wherein, each summit represents a data point in the three-dimensional point cloud, and E represents the limit between adjacent two points, calculates by the k nearest neighbor algorithm;

2) adopt in the weight part plan approximating method Calculation of Three Dimensional point cloud each to put namely normal vector corresponding to each summit among the figure;

3) every weighted value that the limit is corresponding in the calculating chart, i.e. the normal vector difference on two summits corresponding to every limit, and the weighted value ascending order is pressed on all limits arrange; Simultaneously, compose an initial threshold for each summit among the figure;

4) to each the bar limit on the figure, if corresponding two summits are not or not same zone, calculate so the normal vector difference in two zones, if this normal vector difference is not more than in two region threshold any one, merge two zones, and the threshold value of new region equals the normal vector difference and adds that initial threshold is divided by the size of new region;

5) for each zone, if the data point number thinks then that greater than the threshold value of setting this zone is a plane.

2. a kind of three dimensional point cloud plane extracting method based on graph theory according to claim 1 is characterized in that described step 2) be specially:

If one group of cloud data P={p ₁, p ₂..., p _nContaining n point, each puts p _i=(p _Ix, p _Iy, p _Iz) ^TThe k neighborhood be Q _i={ q _I1, q _I2..., q _Ik, use T _iExpression point p ₁Tangent plane, then have:

Wherein, n _i=(n _Ix, n _Iy, n _Iz) be at a p _iTangent plane T _iNormal vector, d _iTo tangent plane T from true origin _iDistance,

Weighting function, and $\left|\right|P_j-P_i\left|\right|=\sqrt{{(P_j-P_i)}^T(p_j-p_i)};$

By introducing Lagrange multiplier, obtain normal vector n _iBe matrix

Minimal eigenvalue characteristic of correspondence vector, wherein,

Generate according to Gaussian function:

σ is zooming parameter, σ=ρ r _Max, r _MaxP _iMaximum radius in the some k neighborhood scope, ρ is the convergent-divergent constant.

3. a kind of three dimensional point cloud plane extracting method based on graph theory according to claim 2 is characterized in that described step 3) in calculate weighted value corresponding to every limit formula be

w(v _i，v _j)＝α(1-|n _i·n _j|)

Wherein, w ((v _i, v _j)) be every limit (v among the non-directed graph G=(V, E) _i, v _j) corresponding weights, v _iAnd v _jBe two summits on this limit, these weights represent the similarity on two summits; n _i, n _jBe respectively vertex v _i, v _jNormal vector, α is amplification coefficient.

4. a kind of three dimensional point cloud plane extracting method based on graph theory according to claim 3 is characterized in that described step 4) be specially:

With two vertex v on every limit _i, v _jRespectively as two regional S _i, S _j, and two normal vectors corresponding to zone are respectively

If there is w (S _i, S _j)≤T (S _i, S _j), then same zone is merged in these two zones, wherein, w (S _i, S _j) two adjacent area S _iAnd S _jThe normal vector difference, $w(S_i,S_j)=\mathrm{\α}(1-|n_{s_i}\·n_{s_j}\left|\right),$ T (S _i, S _j) be threshold function table,

T(S _i，S _j)＝max(H(S _i)+θ(S _j)，H(S _j)+θ(S _j))

H (S) is respectively the normal vector difference in two zones that merge regional S, the adjustment function of θ (S) expression and the inversely proportional relation of area size,

β is the initial threshold on each summit, also is the maximum similarity that can merge two summits, | S| is the size of regional S.