cleaning code fixed npe
added pose stuff

Added method for getting pose have to fix it up and add to map class