New implementation of furniture tracking for both localization and path planning