Clustering
A point cloud is of no help if, for example, you want to navigate a robot in an unknown environment. To get a better picture of the environment, adjacent points must be grouped into clusters. After that, one can try to calculate objects from the clusters that have a distance, an angle, and a dimension.
Building Clusters
First, we want to group adjacent points into a cluster.
For us humans, this is very easy if we can visualize the points, but for an algorithm, it is much more difficult.
The first step is to convert the frame data (angle, distance, quality) into x and y coordinates.
def convert_frame_to_XY(frame):
points_xy = []
for angle, distance, _ in frame:
r = distance / 1000.0
rad = math.radians(angle)
x = r * math.cos(rad)
y = r * math.sin(rad)
points_xy.append((x, y))
return points_xy
Now we need an algorithm that can form clusters from x and y coordinates. There are many ready-made solutions for this; I used DBSCAN from scikit-learn.
DBSCAN
The DBSCAN algorithm views clusters as areas of high density separated by areas of low density. Due to this rather generic view, clusters found by DBSCAN can be any shape, as opposed to k-means which assumes that clusters are convex shaped. The central component to the DBSCAN is the concept of core samples, which are samples that are in areas of high density. A cluster is therefore a set of core samples, each close to each other (measured by some distance measure) and a set of non-core samples that are close to a core sample (but are not themselves core samples).
X = np.array(points_xy) db = DBSCAN(eps=0.15, min_samples=6).fit(X)
As you can see, 1000 points have become 24 clusters.
Unfortunately, the x y coordinates are not helpful if, for example, you want to know what is in front of or behind a robot. Ideally, one would now have to convert the summarized values back into data with an angle, a distance and - if possible - a dimension.
From Clusters To Objects
Therefore, I implemented another method that delivers the objects as JSON data with the following structure:
{
"id": 1,
"angle": 37.2, # degree
"distance": 1.05, # meter
"size": 0.32 # meter
}
The result of the calculations now looks like this:
{'id': 1, 'angle': 67.14, 'distance': 3.2, 'size': 0.47}
{'id': 2, 'angle': 90.29, 'distance': 0.68, 'size': 0.54}
{'id': 3, 'angle': 113.26, 'distance': 1.94, 'size': 0.4}
{'id': 4, 'angle': 133.29, 'distance': 1.29, 'size': 0.63}
{'id': 5, 'angle': 152.49, 'distance': 1.7, 'size': 0.31}
.................
Code
This is the complete class with all the methods I used for clustering. To be able to use this code you must first install "scikit-learn".
import numpy as np
from sklearn.cluster import DBSCAN
import math
class Cluster:
# --- convert Frame data (angle, distance, quality) into x,y coordinates
def convert_frame_to_XY(self, frame):
points_xy = []
for angle, distance, _ in frame:
r = distance / 1000.0
rad = math.radians(angle)
x = r * math.cos(rad)
y = r * math.sin(rad)
points_xy.append((x, y))
return points_xy
# --- Clustering ---
def cluster_points(self, points_xy):
X = np.array(points_xy)
db = DBSCAN(eps=0.15, min_samples=6).fit(X)
labels = db.labels_
clusters = {} # <-- dict
for i, label in enumerate(labels):
if label == -1:
continue
if label not in clusters:
clusters[label] = []
clusters[label].append(X[i])
return clusters, labels
# -- clusters to objects ----
def clusters_to_objects(self, clusters):
objects = []
obj_id = 1
for label, pts in clusters.items():
pts = np.array(pts)
# center
cx = pts[:,0].mean()
cy = pts[:,1].mean()
# Polar-coordinates
distance = math.sqrt(cx**2 + cy**2)
angle = math.degrees(math.atan2(cy, cx))
if angle < 0:
angle += 360
# size (diameter)
size = np.max(np.linalg.norm(pts - [cx, cy], axis=1)) * 2
obj = {
"id": obj_id,
"angle": round(angle, 2),
"distance": round(distance, 2),
"size": round(size, 2)
}
objects.append(obj)
obj_id += 1
return objects