* Added function for getting pose marker
* Added a few comments
* Need to remove visualizePose functions and combine them