This week I focused on the ICP algorithm as well as helping Alex with his code. One of our issues with the monkey was that its ears could not be scanned, so I started working on the ICP algorithm using open3d functions. We were still working on fixing the laser detection algorithm and other parts of the code that was broken this week, so I used some example point clouds to develop my code.
The ICP algorithm determines the transformation between two point clouds from different angles of the object by using least squares to match duplicate points – these point clouds would be constructed by mapping the scanned pixel and depth to their corresponding 3D cartesian coordinates as shown from the math above. Similar to gradient descent, ICP works best when starting at a good starting point to avoid being stuck at local minima and also save computation time.
Thus, there are actually two steps for ICP. The first step is called global registration, which uses downsampling with a voxel size in order to find an approximate transformation matrix to be fed into the local registration step. The global registration step can allow for any sort of transformations, and as an example, this is the starting point between the two meshes:
Next, the meshes are downsampled with a specified voxel size, which in this case is 5cm, but will be tuned for our use case later. Then, global registration will find a transformation matrix that is close to the real one which gives a good approximation in combining the meshes:
Next, there are two different types of ICP registration – point to point, and point to plane. Point to plane tends to produce more accurate results, and here is the result from point to plane ICP registration. Again, note that local registration requires a transformation matrix that is almost at the right place, that is why the previous step with global registration is important. Otherwise, the resulting transformation matrix will not move much at all. 
This is an example of when point to point registration is slightly off, where the resulting meshes are a bit separate vertically:
Next week I will work on refining the ICP algorithm as well as testing it on our go-to meshes like the monkey and the vase.