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