One paper accepted at RA-L presented at IROS 2025, Hangzhou.