Clone of Sophus (https://github.com/strasdat/Sophus).