Method and apparatus for representing environmental elements, system, and vehicle/robot
A computer-implemented method for representing environmental elements includes receiving scan data comprising at least a point cloud representing at least an environmental element from a sensor, segmenting the point cloud into point clusters, and partitioning the point clusters into hierarchical grids. The method also includes establishing a Gaussian distribution for points in each cell of each of the hierarchical grids, and constructing a Gaussian Mixture Model based on the Gaussian distribution for representing the environmental element.
1. A computer-implemented method for representing environmental elements, comprising:
receiving scan data comprising at least a point cloud representing at least an environmental element from a sensor;
segmenting the point cloud into point clusters using a region growing algorithm with a predetermined criterion of smooth;
partitioning the point clusters into hierarchical grids;
establishing a Gaussian distribution for points in each cell of each of the hierarchical grids; and
constructing a Gaussian Mixture Model based on the Gaussian distribution for representing the environmental element,
wherein the predetermined criterion of smooth is derived by:
for each point of the point cloud, getting its neighboring points;
transforming the neighboring points into a local operation plane;
for each point of the point cloud, calculating principal curvatures of the local surface;
calculating surface curvature at one point of the point cloud to the direction of another point of the point cloud, surface curvature at the another point to the direction of the one point, and torsion of surface from the one point to the direction of the another point; and
establishing the predetermined criterion of smooth as the absolute value of the surface curvature at the one point to the direction of the another point being smaller than a threshold, the absolute value of the surface curvature at the another point to the direction of the one point being smaller than a threshold, and the absolute value of the torsion of surface from the one point to the direction of the another point being smaller than a threshold, and
wherein the surface curvature K ij at the one point to the direction of the another point G p j is denoted as:
K
ij
=
d
ij
T
d
ij
(
v
i
1
v
i
2
)
H
i
(
v
i
1
v
i
2
)
T
d
ij
d
ij
,
the surface curvature K ji at the another point G p j to the direction of the one point G p i is denoted as
K
ji
=
d
ij
T
d
ij
(
v
j
1
v
j
2
)
H
j
(
v
j
1
v
j
2
)
T
d
ij
d
ij
,
and
the torsion of surface T ij from the one point G p i to the direction of the another point G p j is denoted as
T
ij
=
d
ij
(
d
ij
×
v
i
3
)
d
ij
d
ij
×
v
i
3
,
where d ij represents the vector at the one point G p i to the another point G p j , v i1 , v i2 , v i3 are the column vectors of the rotation matrix from the original point cloud coordinate system to the local operation plane coordinate system related to the one point, G p i , v j1 , v j2 , v j3 are the column vectors of the rotation matrix from the original point cloud coordinate system to the local operation plane coordinate system related to the another point G p j , H i is the Hessian Matrix related to the one point G p i , and H j is the Hessian Matrix related to the another point G p j .
2. The method according to claim 1 , wherein transforming the neighboring points into a local operation plane further comprising fitting a local operation plane at a point of the neighboring points by using an Eigenvalue Decomposition of a matrix, and transforming the neighboring points into the local operation plane.
3. The method according to claim 1 , wherein for each point of the point cloud, calculating principal curvatures of the local surface further comprising:
for each point, calculating local quadratic surface parameters in the local operation plane;
constructing a Hessian Matrix for local quadratic surface based on the local quadratic surface parameters; and
calculating eigenvalues of the Hessian Matrix as the principal curvatures of the local surface.
4. The method according to claim 1 , wherein said partitioning the point clusters into hierarchical grids further comprising partitioning the point clusters into hierarchical grids using a Quadtree.
5. The method according to claim 1 , wherein establishing the Gaussian distribution for points in each cell of each of the hierarchical grids further comprising establishing the Gaussian distribution for points in a cell of a higher layer based on the Gaussian distribution for points in each child cell of the cell in a lower layer adjacent to the higher layer.
6. The method according to claim 5 , wherein at the lowest layer of the hierarchical grids, for points in a cell of the lowest layer {p 1 , p 2 , . . . , p n } with p i =(x i , y i , z i ) T , the mean μ and covariance Σ of the Gaussian distribution are
μ
=
1
n
(
p
1
+
p
2
+
…
+
p
n
)
,
∑
=
1
n
-
1
∑
i
=
1
n
(
p
i
-
μ
)
(
p
i
-
μ
)
T
for each higher layer of the hierarchical grids, calculate the Gaussian distribution for points in a cell of the higher layer by minimizing the sum of L2 distances between the Gaussian distribution for points in the cell of the higher layer with the Gaussian distribution for points in each child cell of the cell in the lower layer adjacent to the higher layer, where L2=N(0|μ F −μ ci , Σ F +Σ ci ) wherein μ F and Σ F are the mean and the covariance of the Gaussian distribution for the cell of the higher layer, respectively, μ ci and Σ ci are the mean and the covariance of the Gaussian distribution for the ith child cell of the cell.
7. An apparatus for representing environmental elements, comprising:
a scan data receive module configured to receive scan data comprising at least a point cloud representing at least an environmental element from a sensor;
a point cloud segment module configured to segment the point cloud into point clusters using a region growing algorithm with a predetermined criterion of smooth;
a hierarchical grids partition module configured to partition the point clusters into hierarchical grids;
a Gaussian distribution establish module configured to establish a Gaussian distribution for points in each cell of each of the hierarchical grids; and
a Gaussian Mixture Model construct module configured to construct a Gaussian Mixture Model based on the Gaussian distribution for representing the environmental element,
wherein the predetermined criterion of smooth is derived by:
for each point of the point cloud, getting its neighboring points;
transforming the neighboring points into a local operation plane;
for each point of the point cloud, calculating principal curvatures of the local surface;
calculating surface curvature at one point of the point cloud to the direction of another point of the point cloud, surface curvature at the another point to the direction of the one point, and torsion of surface from the one point to the direction of the another point; and
establishing the predetermined criterion of smooth as the absolute value of the surface curvature at the one point to the direction of the another point being smaller than a threshold, the absolute value of the surface curvature at the another point to the direction of the one point being smaller than a threshold, and the absolute value of the torsion of surface from the one point to the direction of the another point being smaller than a threshold, and
wherein the surface curvature K ij at the one point G p i to the direction of the another point G p j is denoted as:
K
ij
=
d
ij
T
d
ij
(
v
i
1
v
i
2
)
H
i
(
v
i
1
v
i
2
)
T
d
ij
d
ij
,
the surface curvature K ji at the another point to the direction of the one point G p i is denoted as
K
ji
=
d
ij
T
d
ij
(
v
j
1
v
j
2
)
H
j
(
v
j
1
v
j
2
)
T
d
ij
d
ij
,
and
the torsion of surface T ij from the one point G p i to the direction of the another point G p j is denoted as
T
ij
=
d
ij
(
d
ij
×
v
i
3
)
d
ij
d
ij
×
v
i
3
,
where d ij represents the vector at the one point G p i to the another point G p j , v i1 , v i2 , v i3 are the column vectors of the rotation matrix from the original point cloud coordinate system to the local operation plane coordinate system related to the one point, G p i , v j1 , v j2 , v j3 are the column vectors of the rotation matrix from the original point cloud coordinate system to the local operation plane coordinate system related to the another point G p j , H i is the Hessian Matrix related to the one point G p i , and H j is the Hessian Matrix related to the another point G p j .
8. The apparatus according to claim 7 , wherein transforming the neighboring points into a local operation plane further comprising fitting a local operation plane at a point of the neighboring points by using an Eigenvalue Decomposition of a matrix, and transforming the neighboring points into the local operation plane.
9. The apparatus according to claim 7 , wherein for each point of the point cloud, calculating principal curvatures of the local surface further comprising:
for each point, calculating local quadratic surface parameters in the local operation plane,
constructing a Hessian Matrix for local quadratic surface based on the local quadratic surface parameters, and
calculating eigenvalues of the Hessian Matrix as the principal curvatures of the local surface.
10. The apparatus according to claim 7 , wherein the hierarchical grids partition module is further configured to partition the point clusters into hierarchical grids by using a Quadtree.
11. The apparatus according to claim 7 , wherein the Gaussian distribution establish module is further configured to establish the Gaussian distribution for points in a cell of a higher layer based on the Gaussian distribution for points in each child cell of the cell in a lower layer adjacent to the higher layer.
12. The apparatus according to claim 11 , wherein at the lowest layer of the hierarchical grids, for points in a cell {p 1 , p 2 , . . . , p n } with p i =(x i , y i , z i ) T , calculate the mean μ and covariance Σ of the Gaussian distribution by
μ
=
1
n
(
p
1
+
p
2
+
…
+
p
n
)
,
∑
=
1
n
-
1
∑
i
=
1
n
(
p
i
-
μ
)
(
p
i
-
μ
)
T
for each higher layer of the hierarchical grids, calculate the Gaussian distribution for points in a cell of the higher layer by minimizing the sum of L2 distances between the Gaussian distribution for points in the cell of the higher layer with the Gaussian distribution for points in each child cell of the cell in the lower layer adjacent to the higher layer, where L2=N(0|μ F −μ ci , Σ F +Σ ci ) wherein μ F and Σ F are the mean and the covariance of the Gaussian distribution for the cell of the higher layer, respectively, μ ci and Σ ci are the mean and the covariance of the Gaussian distribution for the ith child cell of the cell.
13. A system for representing environmental elements, comprising:
a sensor configured to detect at least an environmental element and to output scan data comprising at least a point cloud; and
an apparatus configured to represent environmental elements, the apparatus comprising:
a scan data receive module configured to receive scan data comprising at least a point cloud representing at least an environmental element from a sensor,
a point cloud segment module configured to segment the point cloud into point clusters using a region growing algorithm with a predetermined criterion of smooth,
a hierarchical grids partition module configured to partition the point clusters into hierarchical grids,
a Gaussian distribution establish module configured to establish a Gaussian distribution for points in each cell of each of the hierarchical grids, and
a Gaussian Mixture Model construct module configured to construct a Gaussian Mixture Model based on the Gaussian distribution for representing the environmental element,
wherein the predetermined criterion of smooth is derived by:
for each point of the point cloud, getting its neighboring points;
transforming the neighboring points into a local operation plane;
for each point of the point cloud, calculating principal curvatures of the local surface;
calculating surface curvature at one point of the point cloud to the direction of another point of the point cloud, surface curvature at the another point to the direction of the one point, and torsion of surface from the one point to the direction of the another point; and
establishing the predetermined criterion of smooth as the absolute value of the surface curvature at the one point to the direction of the another point being smaller than a threshold, the absolute value of the surface curvature at the another point to the direction of the one point being smaller than a threshold, and the absolute value of the torsion of surface from the one point to the direction of the another point being smaller than a threshold, and
wherein the surface curvature K ij at the one point G p i to the direction of the another point G p j is denoted as:
K
i
j
=
d
i
j
T
d
i
j
(
v
i
1
v
i
2
)
H
i
(
v
i
1
v
i
2
)
T
d
i
j
d
i
j
,
the surface curvature K ji at the another point G p j to the direction of the one point G P i is denoted as
K
j
i
=
d
i
j
T
d
i
j
(
v
j
1
v
j
2
)
H
j
(
v
j
1
v
j
2
)
T
d
i
j
d
i
j
,
and
the torsion of surface T ij from the one point G p i to the direction of the another point G p j is denoted as
T
j
i
=
d
i
j
(
d
i
j
×
v
i
3
)
d
i
j
d
i
j
×
v
i
3
,
where d ij represents the vector at the one point G P i to the another point G p j , v i1 , v i2 , v i3 are the column vectors of the rotation matrix from the original point cloud coordinate system to the local operation plane coordinate system related to the one point, G p i , v j1 , v j2 , v j3 are the column vectors of the rotation matrix from the original point cloud coordinate system to the local operation plane coordinate system related to the another point G p j , H i is the Hessian Matrix related to the one point G p i , and H j is the Hessian Matrix related to the another point G p j .