This code was used for the initial submission of the RA-L 2018 paper:
(for the videos in the accepted version the code has been further improved)