Error handling

[aGotelli]

In the SE3Pose class there is no verifications that the vectors passed to the constructor are of the correct dimension and filled correctly. For example that the quaternion is unitary and that begins with the scalar components