Multi-robot 6D graph SLAM connecting decoupled local reference filters
Simultaneous localization and mapping: part I