Source: https://www.cs.unc.edu/~jeffi/papers/ICRA2019_Ichnowski.pdf