Partial completion of #95

Not done genetic search or using more of point cloud, but it does use ICP and has been refactored. The most important change is the use of double precision for pose matrices/