Publish result of icp as pose stamped

![screenshot_from_2014-09-27 14 11 41](https://cloud.githubusercontent.com/assets/40454/4429800/97c27ce4-4604-11e4-9d3e-b6f9bf85e6fd.png)