Refactored pose utils

Change-Id: Id34bb7963c86409b65c19b4032c5ffc868e05c15