Source: https://robot.gmc.ulaval.ca/fileadmin/documents/Events/ICRA2019/Papers/Schreiber-etal.pdf