Fix/frame in tolerated pose serialization

Fixes small error in serialization of Frames that are not children of the world frame + adds test case to make sure works.