COS 526 - Advanced Computer Graphics	Fall 2018

Assignment 2 - Point cloud registration

The goal of this assignment is to implement the ICP (Iterative Closest Points) algorithm for registration of point clouds. You will use this algorithm to align the range scans of the bunny, given a set of rough initial guesses for their transformations.

I. Download

1. Download bunny.zip, which contains point clouds for 10 scans of the bunny. Also download pts_view for Mac, Linux, or Windows, which is a viewer for .pts files.

2. Run "pts_view bun000.pts bun045.pts" to view the bun000 and bun045 scans with their rough transformations. The transformations are automatically read from files that have the extension .xf, and each such file specifies the rigid-body transformation from model coordinates to world coordinates as a $4 \times 4$ matrix.

II. Preliminary Code Infrastructure

1. In a programming language of your choosing, write code that will read a point cloud from a .pts file. The format is plain ASCII, and looks like the following:

   coord1_x coord1_y coord1_z norm1_x norm1_y norm1_z
   coord2_x coord2_y coord2_z norm2_x norm2_y norm2_z
   ...
   coordn_x coordn_y coordn_z normn_x normn_y normn_z
   

   That is, each line just specifies the $(x,y,z)$ coordinates of a point together with its normal.

2. Write code that will read a rigid-body transformation, in a simple ASCII format that specifies it as a $4 \times 4$ matrix in homogeneous coordinates. For example, a transformation that rotates by 90 degrees counterclockwise around the Z axis, and translates by $(2,3,4)$, would be written as

   0 -1 0 2
   1 0 0 3
   0 0 1 4
   0 0 0 1
   

   Look at these notes for a reminder about how transformations in homogeneous coordinates work.

3. Write code that, given an arbitrary point in 3D space, will find the closest point in a point set. I suggest that you start with a brute-force implementation, and eventually implement an accelerated version based on a grid or a k-d tree. Feel free to use the notes and k-d tree assignment from COS 226 as references.

   Debugging hint: The pts_view program can display a set of line segments if you pass it a file with the .lines extension. This file just specifies the two endpoints of each segment to be drawn:

   start1_x start1_y start1_z end1_x end1_y end1_z
   start2_x start2_y start2_z end2_x end2_y end2_z
   ...
   

   For example, if you run "pts_view bun000.pts bun045.pts bun045-bun000.lines", you'll see blue lines connecting points that were sampled from bun045 (in green) and the closest corresponding points on bun000 (in red).

4. Write code that applies a rigid-body transformation to a point. As a reminder, applying a transformation to a point is just a matrix multiplication by a column vector: $$ \begin{pmatrix} a & b & c & d \\ e & f & g & h \\ i & j & k & l \\ 0 & 0 & 0 & 1 \\ \end{pmatrix} \begin{pmatrix} x \\ y \\ z \\ 1 \end{pmatrix} $$

5. Write code that will compose (multiply) two transformations, and code to invert a given rigid-body transform. As a reminder, the inverse of a rotation matrix is just its transpose (but you still have to deal with computing the correct translation for the inverse).

   Debugging hint: Try verifying that the product of a transformation matrix and its inverse is (very close to) the identity.

6. Find (or write) code that can solve a $6 \times 6$ system of linear equations.

III. ICP

Write code that will implement Iterative Closest Points registration, as follows:

1. Read in two point clouds specified on the command line, together with their associated rigid-body transforms (which are in separate files with the extension .xf). For example, running

   % icp file1.pts file2.pts
   

   should read in file1.pts, file1.xf, file2.pts, and file2.xf. If either .xf file is missing, assume the identity transformation. On output, your program will write file1.xf with the new transformation that aligns file1.pts to file2.pts.

2. Randomly pick 1000 points on file1.

3. For each point chosen in (2), apply $M_1$, the current transformation of file1, and then the inverse of $M_2$, the transformation of file2. This way, you have the location of the point in the coordinate system of file2. Call these points $p_i$.

4. Find the closest point in file2 to each point computed in (3). Call these points $q_i$. The normal associated with each $q_i$ is $n_i$.

   Debugging hint: Write out a .lines file containing your $p_i$ and $q_i$, and use pts_view to load it up together with file1.pts and file2.pts.

5. Compute the median point-to-plane distance between the 1000 point pairs. That is, compute the median value of $$ |(p_i - q_i) \cdot n_i| $$

   Debugging hint: For the initial transformations provided to you, you should be seeing initial median distances in the range of 1 through 10. (The mesh units are millimeters.)

6. To perform outlier rejection, eliminate (or mark as unused) any point pair whose point-to-plane distance is more than 3 times the median distance you found in (5).

7. Compute the mean point-to-plane distance between the remaining point pairs found in (6). This will be used later to see how much this ICP iteration reduced the misalignment.

8. Referring to the last slide of these notes, construct the matrix $C = A^T A$ and the vector $d = A^T b$. You never actually need to construct the full matrix $A$ or vector $b$, if you construct $C$ and $d$ by summing up the appropriate contributions for each point pair: $$ C = \sum_i A_i^T A_i \qquad d = \sum_i A_i^T b_i \qquad A_i^T = \begin{pmatrix} (p_i \times n_i)_x \\ (p_i \times n_i)_y \\ (p_i \times n_i)_z \\ (n_i)_x \\ (n_i)_y \\ (n_i)_z \end{pmatrix} \qquad b_i = -(p_i - q_i) \cdot n_i $$ That is, for each point $i$ you will construct a $6 \times 6$ matrix and $6 \times 1$ vector, and sum up all of those matrices and vectors as $C$ and $d$.

9. Solve the system $$ Cx = d $$ for the vector $x$, which consists of the rotations around the 3 axes as its first 3 components, and the translation as its last 3 components. Construct a rigid-body transformation $M_{ICP}$ out of those 6 values: $$ M_{ICP} = T R_z R_y R_x $$ where $$ T = \begin{pmatrix} 1 & 0 & 0 & t_x \\ 0 & 1 & 0 & t_y \\ 0 & 0 & 1 & t_z \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}, \quad R_z = \begin{pmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}, \\ R_y = \begin{pmatrix} \cos\theta & 0 & \sin\theta & 0 \\ 0 & 1 & 0 & 0 \\ -\sin\theta & 0 & \cos\theta & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}, \quad R_x = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta & 0 \\ 0 & \sin\theta & \cos\theta & 0 \\ 0 & 0 & 0 & 1 \\ \end{pmatrix}. $$ This is from slides 56 and 57 of these notes, but note that $R_y$ is wrong there.

10. Update the transformation of file1 to include the $M_{ICP}$ you just computed. The tricky part is that $M_{ICP}$ is expressed in the coordinate system of file2, so the update that you need to perform is: $$ M_1 \leftarrow M_2 M_{ICP} M_2^{-1} M_1 $$ Make sure you understand why this is correct.

11. Update the $p_i$ in the list of points from (6) by multiplying them by $M_{ICP}$. Re-compute the mean point-to-plane distance between point pairs, and compare to the distance you found in (7).

12. If the ratio of distances found in (11) and (7) is less than 0.9999 (meaning that this ICP iteration has improved things by more than 0.01%), continue with the next iteration of ICP by going back to step (2), else stop.

13. Write out the new $M_1$ in file1.xf

IV. Aligning the Bunny

1. Try running "icp bun045.pts bun000.pts", where icp is the program you have written, to align bun045 to bun000. If you re-run pts_view bun000.pts bun045.pts, you should see the point clouds on top of each other.

2. Now try aligning all the point clouds to each other. I suggest the following order:

   icp bun045.pts bun000.pts
   icp bun090.pts bun045.pts
   icp bun315.pts bun000.pts
   icp bun270.pts bun315.pts
   icp bun180.pts bun270.pts
   icp chin.pts bun315.pts
   icp ear_back.pts bun180.pts
   icp top2.pts bun180.pts
   icp top3.pts bun000.pts
   

3. Create a writeup (in HTML) briefly describing your implementation and showing a screen-cap from "pts_view *.pts" with an aligned bunny!

V. Submitting

This assignment is due Monday, November 12.

Please submit a single .zip file containing your code and writeup.

The Dropbox link to submit the assignment is here.

Note: Feel free to use external libraries for input/output and solving linear systems, but obviously you shouldn't use external code that implements ICP itself. Please acknowledge in your writeup all outside sources that you consulted.