Map<SO3/SE3> unit tests and related bug fix