Our paper has been accepted by IEEE Transactions on Robotics (T-RO)! Check out the open-source code here and read the full paper here.