3️⃣

Clustering Obstacles

You have a way to segment points and recognize which ones represent obstacles for your car. It would be great to break up and group those obstacle points, especially if you want to do multiple object tracking with cars, pedestrians, and bicyclists, for instance. One way to do that grouping and cluster point cloud data is called euclidean clustering.

Euclidean Clustering

The idea is you associate groups of points by how close together they are. To do a nearest neighbor search efficiently, you use a KD-Tree data structure which, on average, speeds up your look up time from O(n) to O(log(n)). This is because the tree allows you to better break up your search space. By grouping points into regions in a KD-Tree, you can avoid calculating distance for possibly thousands of points just because you know they are not even considered in a close enough region.

Euclidean Clustering Arguments

Implementing KD-Tree

Tree separating x and y regions.

Now let’s talk about how exactly the tree is created.

At the very beginning when the tree is empty, root is NULL. The point inserted becomes the root, and splits the x region. Here is what this visually looks like, after inserting the first point (-6.2, 7).

The next point is (-6.3, 8.4). Since we previously split in the x-dimension, and -6.3 is less than -6.2. This Node will be created and be a part of root's left node. The point (-6.3, 8.4) will split the region in the y dimension.

To recap, the root was at depth 0, and split the x region. The next point became the left child of root and had a depth of 1, and split the y region.

A point at depth 2 will split the x region again, so the split dimension number can actually be calculated as depth % 2, where 2 is the number of dimensions we are working with. The image below shows how the tree looks after inserting the second point.

Then here is what the tree looks like after inserting two more points (-5.2, 7.1) and (-5.7, 6.3), and having another x split division from point (-5.7, 6.3). The tree is now at depth 2.

The image below shows so far what the tree looks like after inserting those 4 points. The labeled nodes A, B, C, D, and E are all NULL.

💡
If the next point (7.2, 6.1) is inserted, which of those 5 nodes will the new point node be assigned to? Remember to traverse the tree starting at the root. The depth tells you which dimension (x or y) you should use for comparison.

Improving the Tree

Having a balanced tree that evenly splits regions improves the search time for finding points later. To improve the tree, insert points that alternate between splitting the x region and the y region evenly. To do this pick the median of sorted x and y points. For instance if you are inserting the first four points that we used above (-6.3, 8.4), (-6.2, 7), (-5.2, 7.1), (-5.7, 6.3) we would first insert (-5.2,7.1) since it is the median along the x axis. If there is an even number of elements the lower median is chosen. The next point to be inserted would be (-6.2, 7), the median of the three points for y. This would be followed by (-5.7,6.3) the lower median between the two for x, and then finally (-6.3,8.4). This ordering will allow the tree to more evenly split the region space and improve search time later.

Example of Insertion for Binary Tree

Implementing a recursive helper function to insert points can be a very nice way to update Nodes. The basic idea is that the tree is traversed until the Node it arrives at is NULL, in which case a new Node is created and replaces the NULL Node. For assigning a Node, one way is to use a double pointer. You could pass in a pointer to the node, starting at root, and then when you want to replace a node you can dereference the double pointer and assign it to the newly created Node. Another way of achieving this is by using a pointer reference as well.

Double Pointer

void insert(BinaryTreeNode **node, int data)
   {
      if(*node == NULL)
      {
        *node = getNewNode(data);
      }
      else if(data < (*node)->data)
      {
        insert(&(*node)->left, data);
      }
      else
      {
        insert(&(*node)->right, data);
      }
   }

Pointer Reference

void insert(BinaryTreeNode *&node, int data)
   {
      if(node == NULL)
      {
        node = getNewNode(data);
      }
      else if(data < node->data)
      {
        insert(node->left, data);
      }
      else
      {
        insert(node->right, data);
      }
   }

Searching Points in a KD-Tree

Euclidean Clustering

Pseudocode

Proximity(point,cluster):
    mark point as processed
    add point to cluster
    nearby points = tree(point)
    Iterate through each nearby point
        If point has not been processed
            Proximity(cluster)

EuclideanCluster():
    list of clusters 
    Iterate through each point
        If point has not been processed
            Create cluster
            Proximity(point, cluster)
            cluster add clusters
    return clusters
The image shows the expected output results. Each of the three nearby clusters is colored differently, red, blue, and green.