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. **

Step 3: (Soution 1) C++ Pipes

Through hours of testing and experimentation, with the much need interjection of knowledge by my advising professor Dr. Gordon Erlebacher, I was finally able to get the libfreenect and PCL/OpenNI libraries to work together in the same program. This feat was achieved through the use of the pipe() and fork () functions in C++.

A pipe is used for one-way communication of a stream of bytes. The command to create a pipe is pipe(), which takes an array of two integers. It fills in the array with two file descriptors that can be used for low-level I/O. A common use of pipes is to send data to or receive data from a program being run as a subprocess. One way of doing this is by using a combination of pipe() (to create the pipe), fork() (to create the subprocess), dup2 (to force the subprocess to use the pipe as its standard input or output channel), and exec (to execute the new program). For more information and examples on pipes, you can visit this websites:

 http://www.unix.com/showthread.php?t=58138
http://www.cs.sunysb.edu/~cse533/asgn1/pipes.html
http://www.ecst.csuchico.edu/~hilzer/csci372/pdf/Chapter7.pdf
http://stackoverflow.com/questions/4812891/fork-and-pipes-in-c
http://www.gnu.org/software/libc/manual/html_node/Creating-a-Pipe.html#Creating-a-Pipe

In my program, I first executed the KinectMotor header file which uses the libfreenect library to move the Kinect Motor to the appropriate angle. The angle is entered through the command line by the user. Once the Kinect motor is moved to the appropriate angle, the angle is then "piped" through the child process to the parent process, where the OpenNISaveFrame header file is constructed and called to control the Kinect's camera and save point clouds from the camera into a .pcd file.Once the point cloud is saved into a file, the process is looped until the user pressed CTRL + C to end the program.


This method has been reliable so far in the tests, but i need to add conditional statements to handle bad data being entered into the program by the user such as the Kinect angle (can only be from -30 to 30 degrees) and filenames (as the angle is the filename of the point cloud .pcd file). Most importantly, for some reason, the OpenNISaveFrame function is executed multiple times per function call. I haven't been able to figure out why this is, but with the naming convention, this is not a major problem as I will be able to get one frame per angle as the new frames will have the same names effectively overwriting itself. Still, this is a major feat for my work thus far and I am proud of this work (for now).

Step 3: Moving the Kinect Motor & Saving Point Clouds to a file using the Microsoft Kinect

Through hours and testing and experimentation, I have come to find out that I can get the Kinect motor to move using the libfreenect library and save point clouds to a file using the OpenNI and PCL libraries. I also found out out that the way to achieve this is very brute force and is a bit inconsistent.

The steps are:

1. Move the Kinect motor to the appropriate angle

2. Save the point cloud to file (run this code 2-3 times to ensure that the connection is established between the OpenNI/PCL code and the Kinect)


The reason that the OpenNI/PCL code does not establish a connection with the Kinect has to do with the signal:

// make callback function from member function
        boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
        boost::bind (&OpenNISaveFrame::save, this, _1);
        // connect callback function for desired signal. In this case its a point cloud with color values
        boost::signals2::connection c = interface->registerCallback (f);

        // start receiving point clouds
        interface->start ();

        // stop the grabber
        interface->stop ();

If the Kinect already has an established signal (which is does with the libfreenect library) the connection never registers a callback to the function, which causes the save function in my code not to be executed. Through testing, I have found out that the execution of the save code multiple times (at least 2-3 times) guarantees that the save function is called at least once. I need to test this more to see why that is the case, but, for now,  I can take point clouds from the environment at different angles using the Kinect.

Step 2: Save a Point Cloud from a Kinect

Because the Point Cloud Library website provides an easy implementation for saving point clouds to a file, which can be found here, creating a class for to accomplish this was very trivial. In my project, I call this class OpenNISaveFrame, which saves the point cloud captured by the Kinect into a binary compressed format and as a .pcd (Point Cloud Data) file.

The part of this step that was not trivial was trying to combine these two classes into one workable program that can be implemented rather quickly. My first attempts at this have fail as the libreenect and OpenNI "fight" over control of the Kinect with their function calls. More technically, when the program wants to move the Kinect's motor, the code establishes a signal to the Kinect in the code via libfreenect functions and moves the Kinect accordingly. If the program wants to save point clouds from the Kinect, the code establishes a different connection via PCL and OpenNI functions and the Kinect responds to that signal accordingly. The problem occurs when both connections are being established in the same program. One of two different complications occurred while testing this problem:

  - The libfreenect connection is established, but the OpenNI/PCL connection throws an Exception as the OpenNI/PCL cannot establish a connection with the Kinect since the libfreenect connection is still sending signals to the Kinect, causing the program to end prematurely.

- The libfreenect connection is established and manually shutdown by the code, which causes a segmentation fault to occur in the program.

Since I believe there is a way to get this two libraries to work concurrently in the same program. I will continue to experiment with the classes and the program and will update this blog accordingly.

*** Update: through more testing, I have come to find out that freenect_sync.h and freenect_sync.c provide a disconnection in the thread between the signal and the Kinect. I need to create a class and immediately implement it. (12/1/13) ***

Step 1: Moving the Kinect Motor

The first step for my project is to get the Kinect Motor to tilt so that it is possible to take point clouds of an environment from different angles. Unfortunately, this is not possible using the Point Cloud Library and through a series of tests, it is also not very effective using the implementation provided by OpenNI in the Linux environment. Based on this observations, I have turn to a older C++ library called libfreenect developed by the OpenKinect community. With this library, I was able to create a C++ class called KinectMotor.h. For the purpose of my project, this class will deal with any adjustments needed to the hardware components of the Kinect such as moving and tilting the Kinect angle and changing the LED light on the Kinect.

Based on some experimentations, I have come to find out that the Kinect can only tilt between -31 and 31 degrees, with 0 as the reset angle. I should conduct some timing on the code to see how long it takes for the commands to take over the Kinect and move the device accordingly.

Next, I need to combine the C++ libraries OpenNI and libfreenect in the same program, as I will need the libfreenect library to move the Kinect to the appropriate angle and the OpenNI library to save the point clouds to a file to be processed later.

Research Overview

My name is Brandon Johnson. I am a first year graduate student in the Department of Scientific Computing at Florida State University. My purpose for creating this blog is to keep an up-to-date development log on my research involving the Microsoft Kinect and the Point Cloud Library. I plan on presenting this research at the campus-wide expo called DIGITECH and the department's Computational Expo. I also plan on publishing a paper on my work done from now to April or May 2014.