MATH: Fixed SVD and test cases
SFM: Switched to own SVD code (deprecates use of Eigen)