7.2 Graph-based heuristics
7.2.1 Point cloud segmentation and labeling
In this section we describe a real time point cloud P C segmentation and labeling, registered by an ICP-based SLAM [Pom+13]. The point cloud P C stores the geometric position (x, y, z) of each point p ∈ P C, approximating a surface M . The objective is to establish traversability of the surface for the path control, requiring real-time computation. To this end we define just four categories for traversability:
ground, walls, ramp or stairs, surmountable obstacle. These categories have been
specified to match the effective robot overcoming abilities: 30 cm steps with rounded edge; 40 cm gap; 20 cm stairs at 40◦ slope; spiral staircases; 45◦ slope / 15◦ roll; 80cm × 80cm cavities. Here we assume that segmentation is always performed on the incoming laser measures, and we consider the current one, in so avoiding to parametrize points with time.
We propose here a novel approach to simple categories segmentation based on geomet- ric features. The approach is specified by the following steps (1) patch construction and filtering; (2) estimation of the normals and of the principal curvatures of the surface at all points and (3) segmentation and labeling of the P C.
Patch construction and Filtering
An initial points filtering is already provided by the ICP algorithm, here we are mainly concerned with constructing a surface patch for each PC region, used for geometric features computation, and to discard ill-conditioned neighborhoods. A patch is defined by a mapping ψ : (x, y) 7→ M , with ψ(x, y) = p ∈ R3, and (x, y) ∈ R2, the parameters of the mapping. To simplify we denote ψ(x, y) by x intending also the coordinates of the point p on the surface M ; similarly, we identify the parameters with the x and y coordinates. The patch can be obtained by transforming the point representation of P C into a voxel grid, building a suitable neighborhood for points that can be reordered into surface patches of contiguous points, and ensuring that there is none or little overlapping between patches. First we decimate the
7.2 Graph-based heuristics 51
neighborhood of each point q in P Cs with k-means taking the q as seeds, using
only the points coordinates together with the Euclidean distance. Since k-means does not take care of ill-conditioned clusters we adjust each cluster by expanding it or reducing it, by splitting or merging, according to the cluster spatial frequency
ρ = |H|r3 , where | · | is the cardinality of the cluster H and r = mean(d(H)) is the
radius defined by the mean distance within the cluster. An optimal density ρ? is given by a radius rmin= 0.5√2m, (m, meters), and a neighborhood of 81 points. If
ρ < ρ2? then r is increased to catch more points joining with the cluster that ensures to keep the optimal dimension, among the closer ones. To control the radius growth and filter out noise, a noise function is specified according to the spatial frequency of points as follows. Let ϕ(H) = exp(−π(rmin− r)), then the probability that a
neighborhood H is affected by noise is:
p(noise(H)) = 1 − ϕ(H) (7.2)
The noise function is clearly zero when r = rmin. Neighborhoods with noise value
greater than 0.5 are removed from the P C. Once the clusters are stabilized then, each of them is labeled into a voxel grid, possibly resorting to interpolation, mostly required at the boundary of a patch, so that close points are contiguous. The grid representation leads to the surface patch representation of the three matrices X, for the x coordinates of the points in the cluster, Y for the y coordinates, and Z for
ψ(x, y). These matrices have size m × n each.
Normal and principal curvature estimation
Having transformed the original P C into a number of surface patches well ap- proximating the surface M , using the neighbors construction and the grid, a very precise estimation of the normals, and likewise of the principal curvatures, can be obtained quite simply, using first and second derivatives. This allows us to avoid the computation based on least mean square (see for example [Hop+92; Pau+03]), which can be quite sensitive to noise and it is computationally very expensive, since it requires to compute neighborhoods for each point of the cloud and to perform SVD decomposition.
Here we consider finite-size linear-phase separable kernels, introduced by [Far+04], for first and second order differentiation of discrete multidimensional signals. These kernels are used to convolve the matrices X, Y and Z to obtain the derivatives, by first expanding the matrices at the boundaries. Then, differentiation for each coordinate x and y is obtained by collecting the convolution of each matrix in each direction. Namely, let fx, gx, hx be the first-order derivatives of X, Y and
Z, respectively, with respect to x, obtained with the convolution kernel, these are matrices of size m × n too. Therefore xx is the tensor (fx, gx, hx)>and, analogously,
xyy, xxy. Having defined the patches in explicit form, it is now straightforward to
obtain the normals ns for all the points in a patch, by simply applying the cross product to the first derivative tensors, obtaining the tensor 3 × n × m:
± gx· hy− gy· hx fy· hx− fx· hy fx· gy− fy· gx (7.3)
and normalizing to get the tensor N of unit normals, noting that in the above equation · is the element-wise matrix multiplication. Note that the sign can be actually determined by simply computing the smallest angle between the obtained normals and the laser source position, say S. Namely, we look for the angle θ minimizing cos−1(s>n03×mn)/kskkn03×mnk, where
s = (S ⊗ 11×nm− x03×nm)/kS ⊗ 11×nm− x03×nmk
with S the source position, ⊗ the Kronecker product, x03×nm and n03×nm tensors of size (3 × n × m) reshaped into a 3 × nm matrix.
As for the principal curvatures, at each point p in a patch, we exploit the fact that with the first and second derivatives and the normals tensor N available, we can obtain the following elements, named according the I and II fundamental forms:
E = Σdim=3xx· xx F = Σdim=3xx· xy G = Σdim=3xy· xy;
L = Σdim=3xxx· N M = Σdim=3xxy· N N = Σdim=3xyy· N
(7.4)
In other words E, F, G, L, M, N are all matrices m × n. Thus, k is computed as the solution of the quadratic equation
(EG − F2)k2+ (2F M − EN − GL)k + (LN − M2) (7.5) And it is well-known that the discriminant of the above equation is greater or equal zero, therefore there are either two distinct real roots k1 and k2, namely the two principal curvatures, or a single one. While at umbilical points L/M =
M/F = N/G. Given the principal curvatures we can also obtain both the mean
curvature (1/2)(k1+ k2) and the Gaussian curvature k1k2. We observe that the only computational difficulty here is the construction of the patches. The main difficulties coming from far points, which are those mostly requiring merging and dropping, therefore it might be better for those noisy regions to delay the patch construction, according to the noise function defined in Equation 7.2.
Segmentation and Labeling
Segmentation still uses the matrices X, Y, Z yet all integrated into the voxel grid. We note that without hampering the information for the categories, the kernel con- volution included a significant smoothing of the matrix Z. Then we can obtain from
7.2 Graph-based heuristics 53
Figure 7.3. Left: point cloud segmentation and labeling. Right: weighted graph represen- tation of a fire escape stairs scenario..
the normals, the Gaussian curvature and the principal curvatures an initialization of the categories as follows. Let |n| = (|n1|, |n2|, |n3|)> be the unit normal absolute value at a point p, let K = k1k2 be the Gaussian curvature, with k1 and k2 the principal curvatures, let θ1 = 0.17 and θ2 = 0.95 then the following table illustrates the initialization values:
Parameters Classes |n1|, |n2|,|n3| K, k1, k2,Z ground |n1|, |n2|<0.01, |n3|≥0.8 |K|<0.1 walls |n2|, |n3|<0.01, |n1| ≥ 0.8 |K|<0.1 ramp θ1≤|n3|≤θ2 |n1|≤0.1, θ1≤|n2|≤θ2 |k2|<10−2, |k1| ≥ 1 |n2|≤0.1 θ1≤|n1|≤θ2 |k1|<10−2, |k2| ≥ 1 surmountable obstacles θ1≤|n3|≤θ2 |K| ≥ 1, z > 0.3m.
Because of noise these parameters produce scattered segmentation, to even the neighborhoods of the above defined categories we introduce an energy functional for each class as follows.
All points in a class are assigned kinetic energy Ek; a stop condition is given by a function g(p) which stops the front collecting points, at a given direction of growth, whenever there is a jump in energy level. Let the normal for the given classes be specified by ne with e = {ground, wall, ramp, surm.obstacle}. The energy level
for class e is E`e(p) = cos
−1(n>n
e), with n the normal at the current point p
considered, with p0 a starting point. Let κ denotes either the Gaussian curvature or the principal curvatures, according to the class and its constraints considered. Let:
η(p0, p) = s (|κ(p) − κ(p0) |) + s (|E`e(p0) − E`e(p)|) (7.6)
Here s is the logistic function y = c/(1 + a exp(−bx)), with b = 25 and a = π5. We have chosen for the limiting upper bound c = 100, these values ensures that when the difference in energy level/curvature increases beyond a threshold 0.08 then the
function fires high values up to c. Let
g(p) = Ek(p0) − η(p0, p) (7.7)
Points p, around p0, are collected as far as g(p) > 0: Therefore when g ≤ 0 then the front expansion stops for that point. To resume:
1. Input: the set of surface points, the normals, the curvatures. 2. Assign kinetic energy Ek to the chosen points p0 in the class.
3. Compute a path to a farthest point such that for every point in the path the difference in energy level and curvature is low, namely η(p0, p) ≤ 0.1.
4. Move the neighborhood front in all directions, checking for each direction the stopping criterion g(p), Equation 7.7.
5. Remove the points collected into the formed clusters, if the set of surface points is empty stop, else go to item 2).
6. Output: the list of clusters.
Note that we choose a furthest point together with the path, and define the kinetic energy in place of a cost, confront with [Sch+11]. The final obtained clusters are labeled according to the specified categories.