Jiahe Pan's paper accepted at RA-L and IROS.