Step 5, Part 2: Saving Points in a Voxel Using Voxel Grid Filter

To figure out which points belong to each plane, we need to break down the combined point cloud into numerous simplified, yet detailed structures, The best way to do this is to apply a voxel grid to the combined point cloud. Since applying a voxel grid will create a set of cubes of a certain size called voxels, we can determine which points from the voxel belong to which points belong to which plane through a simple comparison of coordinates.

To extract the data from the voxels after the application of the voxel grid on the point cloud, we needed to use combination of methods already offered by the PCL VoxelGrid class. Here are the steps to getting the correct points:

1. Apply the Voxel Grid to the cloud
2. Get the grid coordinates of each point in the cloud (this will return the coordinates of a centroid for that point)
3. Using the centroid's coordinates, get the index of centroid in the filtered cloud
4. Using the centroid, we are able to determine which points are in each voxel

To save the information from created by the voxel grid, I created another data structure called voxel, which saves the centroid of that particular voxel, the points located in that centroid, and the corresponding planes of each point in that voxel.

*** Update: I was able to accomplish this, but it did not turn out like I hoped.
Here's the situation:

Files:

• Registration.h - voxel function is in this header file; also computes normals for each plane
• Segmentation - header file where planes are being extracted from point clouds
• trans.cpp - main file where the point clouds are being loaded from files, planes are being divided into planes, and points in the planes are being divided into voxels

Commands:

cmake . && make && cd bin && ./transform

Output:

PointCloud (no filtering): 242540 data points.

PointCloud representing the planar component: 149589 data points.

Planar id: 1

PointCloud representing the planar component: 56901 data points.

Planar id: 2

PointCloud representing the planar component: 20883 data points.

Planar id: 3

PointCloud representing the planar component: 7365 data points.

Planar id: 4

PointCloud representing the planar component: 4875 data points.

Planar id: 5

PointCloud representing the planar component: 1755 data points.

Planar id: 6

PointCloud (no filtering): 241031 data points.

PointCloud representing the planar component: 112620 data points.

Planar id: 7

PointCloud representing the planar component: 93356 data points.

Planar id: 8

PointCloud representing the planar component: 21105 data points.

Planar id: 9

PointCloud representing the planar component: 7954 data points.

Planar id: 10

PointCloud representing the planar component: 3748 data points.

Planar id: 11

PointCloud (no filtering): 240900 data points.

PointCloud representing the planar component: 171566 data points.

Planar id: 12

PointCloud representing the planar component: 34734 data points.

Planar id: 13

PointCloud representing the planar component: 19348 data points.

Planar id: 14

PointCloud representing the planar component: 9069 data points.

Planar id: 15

PointCloud representing the planar component: 3957 data points.

Planar id: 16

PointCloud (no filtering): 242560 data points.

PointCloud representing the planar component: 213759 data points.

Planar id: 17

PointCloud representing the planar component: 17257 data points.

Planar id: 18

PointCloud representing the planar component: 7237 data points.

Planar id: 19

PointCloud representing the planar component: 3570 data points.

Planar id: 20

PointCloud (no filtering): 238562 data points.

PointCloud representing the planar component: 192368 data points.

Planar id: 21

PointCloud representing the planar component: 45577 data points.

Planar id: 22

PointCloud (no filtering): 216241 data points.

PointCloud representing the planar component: 147356 data points.

Planar id: 23

PointCloud representing the planar component: 68600 data points.

Planar id: 24

PointCloud (no filtering): 219348 data points.

PointCloud representing the planar component: 143190 data points.

Planar id: 25

PointCloud representing the planar component: 76039 data points.

Planar id: 26

Computing normals of planes...

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

...Done.

Combining Point Clouds...

...Done.

Cloud: 1641182

Creating Voxels...

...Done.

Assigning Points to Voxels...

...Done.

Voxel Size:550008 voxels.

Voxel Size:100359 voxels.

224

1 : 1 7 12 

2 : 2 3 4 5 6 8 12 17 21 

3 : 2 3 4 5 6 7 8 12 17 21 

4 : 2 3 4 5 6 8 12 17 21 

5 : 2 3 4 5 6 8 12 17 21 

6 : 2 3 4 5 6 12 17 21 

7 : 1 3 7 9 10 11 12 13 17 21 23 

8 : 2 3 4 5 8 17 

9 : 7 9 10 11 12 13 17 21 23 

10 : 7 9 10 11 12 13 17 21 23 

11 : 7 9 10 11 12 13 17 21 23 

12 : 1 2 3 4 5 6 7 9 10 11 12 13 14 15 17 21 23 

13 : 7 9 10 11 12 13 14 15 17 21 23 

14 : 12 13 14 17 21 23 

15 : 12 13 15 17 21 23 

16 : 16 

17 : 2 3 4 5 6 7 8 9 10 11 12 13 14 15 17 18 19 21 23 25 

18 : 17 18 19 21 23 25 

19 : 17 18 19 21 23 25 

20 : 20 

21 : 2 3 4 5 6 7 9 10 11 12 13 14 15 17 18 19 21 22 23 25 

22 : 21 22 23 24 25 

23 : 7 9 10 11 12 13 14 15 17 18 19 21 22 23 24 25 

24 : 22 23 24 25 26 

25 : 17 18 19 21 22 23 24 25 26 

26 : 24 25 26 

Elapsed time: 12456.968750 milliseconds

Problem:

• The results are inconclusive since the resulting pictures and table showed that ALL of the planes and in the same plane, which makes no sense.

***

Step 5, Part 1: Extracting Planes from Point Clouds

Now that we were able to reconstruct the scene using the point clouds taken from the Kinect. We need to obtain some crucial information from each point now that the clouds are concatenated.  Some of data needed from the points are the normals of the points, and which cloud each point belongs to. To obtain this, we must break down each cloud into planes. In this case, a plane is a two-dimensional representation of a subset in a point cloud. There is a way to extract the planes from a point cloud using a combination of  built-in PCL classes called SAC Segmentation and Extract Indices. The functions of SAC Segmentation and Extract Indices are to identify the plane present in the point cloud based on the RANSAC parameters and to extract the indices of the plane from the rest of the point cloud.

To hold the necessary data after the extraction of a plane. I created my own data structure called Plane. This data structure holds a vector of points in the plane or planes, the ids of the planes, and the normals of the planes. The vector are correspond to each other based on the indices of the vector. Here is an example of the Clouds before and after the extraction of the planes:

Images of the combined clouds and all of the planes in that cloud

Based on our parameters for the extraction of the planes, there were 17 total planes extracted from the six point clouds. Because the planes were extracted from one point cloud at a time, some of the planes may possibly be the same planes in the combined cloud. To figure this out, we need to extract more data from the planes using a voxel grid.

Step 4, Part 3: Reconstructing an Entire Scene using Point Cloud Registration

Now is the time to put the process decided in the previous two posts to the test. I used the Microsoft Kinect to capture point clouds from -30 degrees all the way up to 30 degrees in 10-degree increments resulting in seven point clouds at different angles. The process is a little different now because moving all of the point clouds up from -30 degrees to 30 degrees and apply the ICP to each of them will make the process inefficient as the process goes on as the number of point start to increase with each new point cloud being added. In this case, the point clouds from -30 to 0 degrees will be combined together and point clouds from 10 to 30 degrees will be combined together, resulting in these two point clouds being combined at the end to produce one single point cloud. Here are some pictures of the results of the process:

Before applying rotation

After applying rotation (1,664,196 points)

After applying ICP to top and bottom halves of scene (Top -  678,301 points / Bottom - 985,895 points / 1,664,196 points Total)

After applying final ICP and combining all of the point clouds (1,664,196 points)

The final resulting point cloud has around 1.6 million points so applying the voxel grid is very useful for this point cloud. Now that we know that Point Cloud Registration is efficient and most importantly works, we can move on to using multiple Kinect and using the same process as described in the previous posts.

Step 4, Part 2: Registration of Point Clouds at Different Angles

Now that the point clouds have been rotated to a relative coordinate system, we can perform the Iterative Closest Point Algorithm efficiently and accurately. Like I stated in the Introduction of Registration post, the Iterative Closest Point Algorithm is a built-in class in the Point Cloud Library that method used to minimize the distance between two clouds of points.

Once the ICP is performed, the algorithm produces a transformation matrix, which contains a translation vector, rotation matrix, an axis of rotation vector, and an angle of rotation. For the ICP algorithm to work efficiently, some parameters have to be adjusted. Here are some of the parameters:
- Input Cloud - this is the point cloud that is being transformed
- Input Target Cloud - this is the point cloud that the input cloud is being set to look like
- Max Correspondence Distance - any corresponding points in the point clouds further than this value will be ignored by the algorithm (0.1 is the value for my project)
-  Max Iterations - once the algorithm runs for x amount of times, the algorithm will terminate (1000 is the value in my project)
- Transformation Epsilon and Euclidean Fitness Epsilon - tolerance parameters for estimating the minimum distance between points in two different point clouds. Algorithm will terminate if the computed epsilons are lower than these values (1E-10 ans 1E-2 are values for my project, respectively)

Here are some pictures of the process from two point clouds at different angles to applying a rotation matrix, performing the Iterative Closest Point Algorithm, and concatenating the point clouds to produce one point cloud taken at different angles. Note that the test were applied to point clouds taken at 0,10,20,30 degrees:

***

Before applying rotation

After applying rotation (924,903 points)

After applying ICP and voxel grid (44,209 points)

Note: the shadows of different point clouds start to mix during ICP and concatenation. This is not an error, but would prefer a evenly lighted room for a more accurate demonstration.

This process produces about 900,000 point clouds, making it inefficient for further processing. So for this case, it would be important to apply a filter to the final point cloud to down-sample the number of points in the cloud from around 900,000 points to about 45,000 points use another PCL built-in class called the Voxel Grid filter. The next step is to now obtain point clouds from the entire range of the Microsoft Kinect from -30 to 30 degrees in 10-degree increments, apply this process to the points clouds to  produce one combined point cloud and apply a Voxel Grid to the point cloud to decrease the number of points in the cloud.

Step 4, Part 1: Pre-Rotation of Point Clouds at Different Angles

Now that I have four point clouds from 0, 10, 20, and 30 degrees, I can now begin the process of combining the point clouds together in their proper positions. The immediate problem is simple: All of the point clouds have been taken at different angles so combining all of the point clouds through point cloud concatenation is not a good strategy as the point point will not be in there proper positions. Another problem through testing is the use of point cloud registration using the Iterative Closest Point Algorithm or ICP. This option, unfortunately, is not the best option either as the ICP will have to rotate all of the point in one point cloud (which is 640 x 480 = 307,200 points). Not only would rotating the point cloud via ICP be inefficient, the point cloud position is not very accurate. 

Based on this, it is necessary to apply a rotation to a point cloud to move the point cloud into a relative position of another point cloud. That way we can perform ICP, get the transformation matrix from the ICP and apply the transform to the point cloud, resulting in two point clouds in their proper positions. 

To apply the proper rotation to a point cloud, we first must use some trigonometric properties and set some conditions to the rotation. First, the rotation is for point clouds 10 degrees apart from each other. Secondly, the rotation is only applied to points in the y (up and down) and z (depth) directions (Note: the point's x direction is not changed through the rotation, but will be changed through the transformation matrix produced by the ICP). Below is code used to rotate the matrix to the proper degrees:

for(int i = 0; i < clouds[0]->points.size(); i++){

  clouds[0]->points[i].x = clouds[0]->points[i].x;

  clouds[0]->points[i].y = cos(angle)*clouds[0]->points[i].y + sin(angle)*clouds[0]->points[i].z;

  clouds[0]->points[i].z = -sin(angle)*clouds[0]->points[i].y + cos(angle)*clouds[0]->points[i].z;

}

Note that the angle must be converted from radians to degrees to properly apply the rotation.

This is the result of applying the rotation:

Before applying rotation

After applying rotation

Introduction to Point Cloud Registration

Image Registration is the process of transforming different data sets into one coordinate system. For this research this go any further, a specialized type of registration called Point Cloud Registration is required. Point Cloud Registration is just like Image Registration except for the fact that images are in two dimensions while point clouds are in a three-dimensional space (due to depth). There are a couple of methods that are used to perform registration on point clouds such as Iterative Closest Point (ICP) and Random Sample Consensus (RANSAC), just to name a few. The Point Cloud Library has built-in classes dedicated to Point Cloud Registration that will the foundation to the registration done in this research.

The method that will be used in my research will be the Iterative Closest Point Algorithm, or ICP. The Iterative Closest Point Algorithm is a method used to minimize the distance between two clouds of points. For ICP to be perform efficiently, there needs to be considerable (about 45%) overlap between both of the point clouds. The process of Iterative Closest Point can be described as follows:

Step 1. Search for correspondences.
Step 2. Reject bad correspondences.
Step 3. Estimate a transformation using the good correspondences.
Step 4. Iterate.

If you would like to learn more about how ICP works in the Point Cloud Library, you can take a look at the PCL documentation: http://docs.pointclouds.org/trunk/a02953.html

Step 3: (Solution 2) C++ System( ) Function

I have rewritten my code again to test the program's ability to utilize both the Libfreenect and OpenNI/PCL libraries. I have used the pipe( ) function as described in the last post, but I was told that this is unnecessarily complicated for what I need the code to do and, most importantly, I need the code to be automatized, that is, I need the code to be able to move the Kinect motor with pretested values and capture point clouds and save them according. After looking through some C++ documentation and books, I have found a function that will simplify my work: the system( ) function.

The system( )  function invokes the command processor to execute a command. Based on the programming environment (I am currently using Linux for this project), the commands can be different for different cases. For my project, I have created executable programs to move the Kinect motor to the appropriate angle and to save point clouds to a file with a specific file name. Theoretically, the libraries are not in the same program, but I am still able to utilize both libraries in the same program.

To achieve this, I had to change the overall structure of the OpenNISaveFrame header file. I had to edit the OpenNISaveFrame constructor in order for the file name to be changed from case to case. This was for simple trivial to do. Next, I had to set up the save.cpp program to read in a string from the command line, the string, of course, being the file name. Also, due to the unreliable connection between the OpenNI library and the Kinect, I have to initialize the signal twice per program execution.

Here are some pictures of the point clouds from the cloud_viewer:

Point Cloud taken at 10 degrees

Point Cloud taken at 20 degrees

The code does in fact work, but not perfectly. For some reason, when I tested the program, there were still times when the program would not initialize a point cloud would not be captured and saved into a file. I was able to compensate for this by saving point clouds for every angle twice per run. This step seems to fix that problem. Unfortunately, the run time of the program is affected as the time complexity per run increases. I will continue to work to make the program more efficient by saving point clouds to memory instead of saving to disk.

**UPDATE: I was able to get the code to work more efficiently. First, I was able to get the signal between the OpenNI and the Kinect to be more consistent by implementing the sleep ( ) function after the initialization of the interface. This allowed the signal enough time set up the connection with the Kinect before the rest of the program executed. Due to this, I only have to initialize the connection once per run instead of twice per run.  Secondly, I was able to set up the OpenNISaveFrame header file so that the point cloud captured from the Kinect saves to memory instead of saving to disk. This makes the code execute faster as it takes less time to save to memory than to save to disk. **