I implemented a histogram of normals, and used it to identify planes, using both radius
search and search of k closest neighbors. Dirty code in ge_icp.cpp
There are also plots created with the points, the normals and the features. .