Fixed visualization and PoseSE2 bug.