C H A P T E R  6

Voxelization

In this chapter, we’ll discuss voxelization and develop a people tracking system. We’ll cover the following:

• What a voxel is
• Why you would want to voxelize your data
• How to make, manipulate, and use voxels
• Why people are rectangles

What Is a Voxel?

A voxel is the three-dimensional equivalent of a pixel—a box, rather than a point, in space that has a volume. Imagine taking an object and then decomposing into cubes, all of the same size. Or, if you’d prefer, building an object out of LEGOs or in Minecraft, much like a cubist painting. See Figure 6-1 for an example of a voxelized scene.

Figure 6-1. A voxelized scene

Why Voxelize Data?

There are two different reasons to voxelize a dataset. First, it reduces the size of the dataset, up to 75%—essential if you’re going to be doing a large amount of work on your data in real time. This size reduction can create an outsized response in performance, given the computational complexity of some of the algorithms involved. Instead of simply destroying data, as if you decimated it, voxelization creates a smoothly moving average over the space, something that maintains as much data as possible while simultaneously lowering the state space size. For example, given a scene that’s 3 meters by 3 meters by 3 meters and has 500,000 points evenly distributed, you can realize over a 50% reduction in points (216,000 vs. 500,000) with a 5-centimeter voxel size.

Second, voxelizing various datasets make them very easy to combine. This may not seem like a large benefit, but consider the multisensor fusion problem. Instead of relying on complex algorithms to handle fusing multiple point clouds, you simply need to address each potential voxel space of interest and calculate the probability that it is occupied given multiple sensors.

Voxels make your datasets easier to work with, but what’s the next step? There are many possibilities for voxelized data, but we’re going to cover background subtraction, people tracking, and collision detection. Background subtraction is cutting out the background of a scene (prerecorded) and isolating the new data. People tracking is using that new data (foreground) to track people and people shaped groups of voxels.

Voxelizing Data

Voxelizing in PCL is simply accomplished: the voxelizer is a filter, and it takes a point cloud as an input. It then outputs the voxelized point cloud, calculated for the centroid of the set of points voxelized. See Listing 6-1 for an example.

Let’s break down the preceding listing:

The preceding two commands declare the filter (VoxelGrid) and set the input to the filter to be point_cloud_ptr.

The preceding command is the key part of the declaration: it sets the size of the window over which voxels are calculated or the voxel size itself. Given that our measurements from the Kinect are in millimeters, this corresponds to a voxel that is 5mm per side.

While this calculation seems quite easy, there’s a problem: voxelizing in this manner disorders a point cloud. Instead of a set of points that’s easy to access and manipulate set of points that’s 640 high by 480 wide, you have a single line that’s 307,200 points long. This makes accessing a particular point or even getting the proper set of indices for a group of points difficult.

A better method is to use octrees. Octrees divide space into recursive sets of eight octants and organize it as a tree, as shown in Figure 6-2. Only the leaves hold data, and you can easily access them. Also, there are several very fast functions for calculating useful metrics over an octree; for example, nearest neighbor searches are lightning fast. See Figures 6-3 and Figure 6-4 for two examples of octree divided objects.

Again, PCL provides functions to make this work simple; the octree module handles everything. Listing 6-2 will do the same work as Listing 6-1. It voxelizes the incoming point cloud about the centroids of points, dividing the world up neatly into 50-mm–per–side squares.

The preceding code is very straightforward. Resolution defines the voxel size, in our case, 50 mm. We create the octree with standard octree and leaf types and set the points inside to be XYZRGB. We then set the input cloud to the octree as our cloud and call addPointsFromInputCloud to push them into the structure.

Figure 6-2. How octrees are organized

Figure 6-3. Octree bunny

Manipulating Voxels

In the end, we’re really only interested in the leaf nodes of the octree that contain the data we’re looking for. Normally, to reach all of the leaves, we’d have to traverse the entire tree end to end. However, PCL handily provides an iterator for leaf nodes, LeafNodeIterator:

Let’s work through a simple example. We’re going to take an input cloud and display it voxelized with boxes drawn where there are filled voxels in space, as shown in Listing 6-3.

Given these capabilities, you may ask, “What’s the purpose of drawing voxel boxes?” While our ultimate goal is voxel-on-voxel collisions, our first goal is to subtract the background of our scene from our new data, as shown in Listing 6-4. First comes the code in block, and then we ll break down each step.

As you may notice, the first quarter of the code is just like the bridge code we built in Chapter 3. The background subtraction code is all new, and we’ll be focusing on the functional part directly, in Listing 6-5.

First, we create a new foreground cloud (fgcloud). We also clear the octree’s buffer from the previous iteration. Next, we add the background cloud (bgcloud) to the octree – this is created by stacking successive point clouds (10 in this case), then removing duplicates. This is captured on the first set of iterations after turning on background subtraction. Then, we flip the buffer, keeping the new octree we just created in memory but freeing the input buffer. After that, we insert our new cloud data (cloud) into the octree. PCL has another handy working function called getPointIndicesFromNewVoxels, which provides a vector of indices within the second point cloud (cloud) that are not in the first (bgcloud). Next, we push back the points that are referred to by those indices into our new cloud (fgcloud) and display it. This results in the three output clouds shown in Figures 6-5 (background), 6-6 (full scene), and 6-7 (foreground).

Figure 6-5. The background of the scene, precaptured

Figure 6-6. The full sceneas a point cloud

Figure 6-7. Foreground, after subtraction

Clustering Voxels

Now that we’re able to subtract the background and get the foreground only in these images, we’re going to need to identify people in the scene. And, in order to do that, we’re need to intelligently cluster our voxels together, measure the results, and judge whether or not each result is a person.

There are many clustering algorithms, but we’re going to discuss one of the most simple, yet effective, ones—Euclidean clustering. This method is similar to the 2-D flood fill technique. Here’s how it works:

1. Create octree O from point cloud P.
2. Create a list of clusters, C, and a cluster, L.
3. For every voxel v in octree O, do the following:
   1. Add v_i to L.
   2. For every voxel v_i in cluster L, do the following:
      • Search in the set O^i_k for neighbors in a sphere with radius r.
      • For every neighbor v^k_i in O^k_i, see if the voxel has already been added to a cluster, and if not, add it to L.
4. When all of the voxels in L have been processed, add L to C, and reset L.
5. Terminate when all voxels v_i in octree O have been processed into Ls in C.

Again, PCL provides a function called EuclideanClusterExtraction to save us the pain of having to implement this algorithm ourselves. It is powered by a different tree implementation—a k-d tree. A k-dimensional (k-d) tree is another space-partitioning tree; this one is based around splitting along dimensions (octrees are split around a point). K-d trees are also always binary, a property that makes searching for nearest neighbors (or neighbors in a radius) extremely fast.

Clustering in PCL is easy to set up, as shown in Listing 6-6.

The KdTree line creates the k-d tree from the input data. A vector called cluster_indices is created to store the indices. setClusterTolerance sets the clustering reach, that is, how far we’ll look from each point (the radius in the equation in Listing 6-6). setMinClusterSize and setMaxClusterSize put a floor and ceiling on how many points we’ll try to categorize into a single cluster. All three of these values need to be tuned: if they’re too small, you’ll miss some objects; too large, and you’ll categorize multiple objects as one. Our search method is the k-d tree. The extract function fills the vector of PointIndices. When we run this code over our foreground, we end up with the image in Figure 6-8.

Figure 6-8. Clustered voxels

Tracking People and Fitting a Rectangular Prism

Now that we’ve clustered our data, we can proceed to tracking people and fitting a prism to the data. We’re going to use a very simple metric for tracking a person: adults are generally within a range of height and width, as well as being of a certain size. First, we’ll judge the height and width of our cluster, and then we’ll fit a convex hull to the points to learn what the area is. If a cluster’s area is within the metrics, it will be identified as a person, as shown in Listing 6-7.

Given these metrics—that a person is larger than 4 feet by 4 inches in the Kinect’s view—we get output that looks like Figure 6-9 and Figure 6-10.

Figure 6-9. Person tracking

Figure 6-10. Person tracking

Summary

In this chapter, we delved deep into voxels—why and how to use them—and voxelization. We discussed octrees, background subtraction, clustering, and people tracking. You even had the opportunity to build your very own person tracker.