Four-point congruent sets algorithm (FPCS)

4PCS algorithm for alignment 3D models.
Check _Eigen_ manual, there are special operations to copy matrices block-wise. Using them will simplify the code of this function to a couple of lines.