Assignment 2: Point Cloud Registration

Advanced Computer Graphics (COS 526)


In this assignment, I implement the Iterative Closest Points (ICP) algorithm for registration of point clouds. Using a provided set of 3D point cloud scans and a given set of initial guesses for their transformations, the algorithm pairwise aligns the point clouds by computing new transformations that minimize pairwise point cloud distances. If the initial transformation guesses are sufficiently "close enough", the algorithm will converge to produce a single, cohesively aligned point cloud representing the entire, original 3D surface.


User-defined Libraries and Classes

To implement the Iterative Closest Points algorithm, I first designed and implemented several different libraries and classes:

  • Point (point.h, point.cpp): A point in 3D Cartesian space. Stores point coordinates as well as normal vector. Supports transformations.
  • PointCloud (pointcloud.h, pointcloud.cpp): A point cloud of points in 3D Cartesian space. Supports nearest neighbor search via brute-force linear search or kd-tree search and can return a random subset of its points.
  • KDTree (kdtree.h, kdtree.cpp): A k-dimensional tree for storing points in 3D Cartesian space. Supports simple insertion and nearest neighbor methods.
  • Matrix4x4 (matrix4x4.h, matrix4x4.cpp): A non-generalized implementation of a 4x4 matrix and some simple operations, including addition, subtraction, multiplication by vector and matrix, division, determinant, inverse, and constructors for specific translation and rotation matrices used in rigid-body transformations. Used to represent a point cloud's affine transformation from its own coordinate space into world space.
  • Matrix6x6 (matrix6x6.h, matrix6x6.cpp): A non-generalized implementation of a 6x6 matrix and some simple operations, including addition, subtraction, multiplication by vector and matrix, and division. Does not support other operations, and is only used for formulating and solving the 6x6 linear system of equations in the ICP algorithm.
  • Vector3D (vector3D.h, vector3D.cpp): A 3-dimensional vector supporting simple operations, including addition, subtraction, multiplication, division, dot product, and cross product. Used to represent a Point's coordinates as well as its normal vector, and perform math at a higher level of abstraction.
  • Vector4D (vector4D.h, vector4D.cpp): A 4-dimensional vector supporting simple operations, including addition, subtraction, multiplication, division, and dot product. Used to represent a Point in homogenous coordinates so that an affine transformation can be performed by a simple matrix-vector multiplication.
  • Vector6D (vector6D.h, vector6D.cpp): A 6-dimensional vector supporting simple operations, including addition, subtraction, multiplication, division, and dot product. It is used only alongside Matrix6x6 for formulating and solving a 6x6 linear system of equations in the ICP algorithm.

The Iterative Closest Points Program

Using the above libraries, I then composed icp.cpp, which is the program's entry point. This file contains code for solving a linear system of equations using LU decomposition from the GNU Scientific Library. In its main function, it performs the 13 steps outlined in the provided assignment specification. During execution, the program's progress is sent to standard output, and overall time consumed is reported at the end.


The initial implementation of the algorithm used a brute-force linear search through the entire point cloud to find the nearest neighbor. For the provided point clouds, this process was not terribly slow. However, after I added an additional kd-tree data structure for logarithmic nearest neighbor search, performance improved significantly. On average, I noted a 3-5x improvement in time for the provided point clouds, in exchange for some pre-processing to create the kd-tree and the space to store it.

Problems I ran into...

For a while, I could not figure out why the mean point-to-plane distance would immediately increase after a single iteration (and consequently terminate the algorithm). I ran the program several times on the same two point clouds, and noticed that my two point clouds were slowly moving further and further apart from each other with each run. After some debugging and discussion with other students, I found that the formulation of the Y-rotation matrix from the reference slides had a typo: the angle was not negated properly! A quick change in negative signs quickly resolved the issue.


I performed the pairwise alignment of point cloud scans as suggested in the assignment spec:

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

Then, pts_view *.pts resulted in a beautifully aligned point cloud of a bunny! Here are a few different angles of the final result.

Future Work & Learnings

Being a somewhat involved assignment with the chance to implement some of my own libraries, I was able to continue to hone my skills in good C++ programming and style. Although I spent some time adhering to Google's C++ Style Guide, I am sure that there is still a lot that can be done to improve the overall modularity, design, and readability of this project.

In the future, I would like to revisit the project to continue cleaning up the code and improve its overall design and clarity as much as possible. In addition, I would most likely also remove my own matrix and vector libraries in favor of using more robust ones, such as those written in the GNU scientific library.



  • GSL (GNU Scientific Library)

Building with Makefile

From the root directory, use the provided Makefile to build the program.

$ make


The program is then run from the command line with 2 additional arguments:

$ ./icp src.pts dst.pts

The program expects that the point cloud (.pts) files and their corresponding transformation (.xf) files are in the same directory. It will not recursively search for the files in a nested subdirectory. Given src.pts and dst.pts, the program loads up the transformation files src.xf and dst.xf from the same local directory.

After aligning src.pts and dst.pts, the provided pts_view program can be run to view the two point clouds, which should now be aligned.

$ ./pts_view src.pts dst.pts