Fix refinment of footstep by using relative transformation

Refine result of footstep planning in order to ignore recognition error

Blue: refined footstep
gray: input footstep (curved)
![screenshot_from_2014-10-30 02 37 47](https://cloud.githubusercontent.com/assets/40454/4831136/63b484cc-5f92-11e4-9e54-42c130a13ed9.png)