Consider different CPD matrix solver for better tracking accuracy/numerical stability

[dcolli23]

When poking around, I discovered that my reimplimentation of the routine was more numerically stable and lead to less vertex "scrunching" when inputting many many points or points in the point cloud that are quite far away from the tracked template. See image for visual description of "scrunching".

I checked for differences in CPD routines and parameters and I'm fairly sure the reason is due to the use of the less numerically stable, but faster, Householder QR decomposition in the CPD inner loop. Eigen docs say that this is less numerically stable than FullPivHouseholderQr or ColPivHouseholderQR. Seems that these methods check for matrix rank in the decomposition.