Partial fix to Jacobs (now it converges)
More SE(3) methods & unit tests