full text https://ras.papercept.net/conferences/conferences/IROS16/program/IROS16_ContentListWeb_3.html#wet22