inline Vector2<> ObstacleCombinator::gridToWorld(const Vector2<>& p) const
{
  Vector2<> pWorld(p);
  const float move(USObstacleGrid::GRID_LENGTH / 2);
  pWorld -= Vector2<>(move, move);
  pWorld *= USObstacleGrid::CELL_SIZE;
  Pose2D odoRotation(-theOdometryData.rotation, 0.0f, 0.0f);
  return odoRotation * pWorld;
}
inline Vector2<> USObstacleGridProvider::gridToWorld(const Vector2<>& p,const OdometryData& theOdometryData) const
{
    Vector2<> pWorld(p);
    const float move(GRID_LENGTH / 2);
    pWorld -= Vector2<>(move, move);
    pWorld *= CELL_SIZE;
    Pose2D odoRotation(-theOdometryData.rotation, 0.0f, 0.0f);
    return odoRotation * pWorld;
}