Commit the position reconstruction work
Merge remote-tracking branch 'origin/floor_plan' into floor_plan