Disclaimer This implementation is heavily borrowed from Jur Van Den Berg's implementation that is available here Instead of using a custom matrix library, I used Eigen for my implementation