Mapping with Odometry + others minor changes