First cut at implementing invert() and inverted() for poses

DO NOT MERGE, this is for discussion only.

@HannesSommer, I need your help with this. 

This fails to compile because inverted() returns the wrong type. The compiler is right about this because the class hierarchy looks like:

PoseBase --> HomogeneousTransformation --> HomogeneousTransformationPosition3RotationQuaternion

How is HomogeneousTransformation supposed to know what type to return?

Thanks!