place_profile_utils.cpp

Go to the documentation of this file.

#include <lama_common/place_profile_utils.h>

namespace lama_common
{

void normalizeExcludeSegments(PlaceProfile& profile)
{
  if (profile.exclude_segments.empty())
  {
    return;
  }

  if (is_sorted(profile.exclude_segments.begin(), profile.exclude_segments.end()))
  {
    bool need_erase = false;
    for (int i = 0; i < ((int) profile.exclude_segments.size()) - 1; ++i)
    {
      // Check for doubles in exclude_segments.
      if (profile.exclude_segments[i] == profile.exclude_segments[i + 1])
      {
        need_erase = true;
        break;
      }
      // Check for out-of-bound elements.
      if ((profile.exclude_segments[i] < 0) || (profile.exclude_segments[i] >= profile.exclude_segments.size()))
      {
        need_erase = true;
        break;
      }
    }
    need_erase |= (profile.exclude_segments.back() < 0);
    need_erase |= (profile.exclude_segments.back() >= profile.exclude_segments.size());
    if (!need_erase)
    {
      return;
    }
  }

  std::sort(profile.exclude_segments.begin(), profile.exclude_segments.end());

  for (vector<int32_t>::iterator s = profile.exclude_segments.begin(); s != profile.exclude_segments.end();)
  {
    // Ignore negative elements and too large elements.
    if ((*s < 0) || (*s >= profile.polygon.points.size()))
    {
      s = profile.exclude_segments.erase(s);
      continue;
    }

    // Remove doubles.
    std::vector<int32_t>::iterator last = --profile.exclude_segments.end();
    if (s != last)
    {
      std::vector<int>::iterator next = s + 1;
      if (*s == *next)
      {
        s = profile.exclude_segments.erase(s);
        continue;
      }
    }
    ++s;
  }
}

inline bool pointInCCWOrder(const vector<AngularPoint>& angular_points)
{
  double sum_angle_increments = 0;
  for (size_t i = 0; i < std::min((int)angular_points.size() - 1, 3); ++i)
  {
    const double dtheta = angular_points[i + 1].angle - angular_points[i].angle;
    if (std::abs(dtheta) < 0.99 * M_PI)
    {
      sum_angle_increments += dtheta;
    }
  }
  return sum_angle_increments > 0;
}

void normalizePlaceProfile(PlaceProfile& profile, bool sort)
{
  if (sort)
  {
    vector<AngularPoint> angular_points = toAngularPoints(profile);
    size_t point_count = profile.polygon.points.size();

    if (!is_sorted(angular_points.begin(), angular_points.end()))
    {
      // If points are clock-wise, exclude_segments needs to be changed because
      // after with clock-wise direction means before with counterclock-wise direction.
      const bool counterclockwise = pointInCCWOrder(angular_points);

      // TODO: Improve sort performance.
      // Appart from the position of the step from +pi to -pi (CCW) which can be in
      // the middle of the array instead of being absent, angular_points
      // should be sorted. Use a circular_iterator, if this exists.
      std::sort(angular_points.begin(), angular_points.end());

      PlaceProfile old_profile = profile;
      for (size_t i = 0; i < point_count; ++i)
      {
        profile.polygon.points[i] = old_profile.polygon.points[angular_points[i].index];
      }

      vector<int32_t> inverse_permutation;
      if (!profile.exclude_segments.empty())
      {
        inverse_permutation.resize(point_count);
        for (size_t i = 0; i < point_count; ++i)
        {
          inverse_permutation[angular_points[i].index] = i;
        }
      }
      if (!counterclockwise)
      {
        // v[i] = v[i]+1, does the trick to get the correct renumbering.
        for (size_t i = 0; i < old_profile.exclude_segments.size(); ++i)
        {
          old_profile.exclude_segments[i] = (old_profile.exclude_segments[i] + 1) % point_count;
        }
      }
      for (size_t i = 0; i < profile.exclude_segments.size(); ++i)
      {
        profile.exclude_segments[i] = inverse_permutation[old_profile.exclude_segments[i]];
      }
    }
  }

  normalizeExcludeSegments(profile);
}

PlaceProfile normalizedPlaceProfile(const PlaceProfile& profile, bool sort)
{
  PlaceProfile new_profile = profile;
  normalizePlaceProfile(new_profile, sort);
  return new_profile;
}

bool isClosed(const PlaceProfile& profile, double max_frontier_width)
{
  const size_t size = profile.polygon.points.size();
  const double width2 = max_frontier_width * max_frontier_width;

  for(size_t i = 0; i < size; ++i)
  {
    Point32 a = profile.polygon.points[i];
    Point32 b = profile.polygon.points[(i + 1) % size];

    const double dx = b.x - a.x;
    const double dy = b.y - a.y;

    if (dx * dx + dy * dy > width2)
    {
      return false;
    }
  }
  return true;
}


void closePlaceProfile(PlaceProfile& profile, double max_frontier_width)
{
  if (isClosed(profile, max_frontier_width))
  {
    return;
  }

  const size_t size = profile.polygon.points.size();
  const double width2 = max_frontier_width * max_frontier_width;

  PlaceProfile old_profile = profile;
  profile.polygon.points.clear();
  profile.polygon.points.reserve(size);
  profile.exclude_segments.clear();

  for(size_t i = 0; i < size; ++i)
  {
    Point32 a = old_profile.polygon.points[i];
    Point32 b = old_profile.polygon.points[(i + 1) % size];

    const double dx = b.x - a.x;
    const double dy = b.y - a.y;

    if (dx * dx + dy * dy > width2)
    {
      Point32 point;
      const double norm = std::sqrt(dx * dx + dy * dy);
      // Unit vector from point[i] to point[j].
      const double ux = dx / norm;
      const double uy = dy / norm;
      for (double s = 0; s <= norm; s += max_frontier_width) 
      {
        point.x = a.x + s * ux;
        point.y = a.y + s * uy;
        profile.polygon.points.push_back(point);
      }
    }
    else
    {
      profile.polygon.points.push_back(a);
    }
  }
}

PlaceProfile closedPlaceProfile(const PlaceProfile& profile, double max_frontier_width)
{
  if (isClosed(profile, max_frontier_width))
  {
    return profile;
  }

  // closePlaceProfile is not used here to avoid an extra copy of the polygon points.

  PlaceProfile new_profile;
  const size_t size = profile.polygon.points.size();
  const double width2 = max_frontier_width * max_frontier_width;

  new_profile.polygon.points.reserve(size);

  for(size_t i = 0; i < size; ++i)
  {
    Point32 a(profile.polygon.points[i]);
    Point32 b(profile.polygon.points[(i + 1) % size]);

    const double dx = b.x - a.x;
    const double dy = b.y - a.y;

    if (dx * dx + dy * dy > width2)
    {
      Point32 point;
      const double norm = std::sqrt(dx * dx + dy * dy);
      // Unit vector from point[i] to point[j].
      const double ux = dx / norm;
      const double uy = dy / norm;
      for (double s = 0; s <= norm; s += max_frontier_width) 
      {
        point.x = a.x + s * ux;
        point.y = a.y + s * uy;
        new_profile.polygon.points.push_back(point);
      }
    }
    else
    {
      new_profile.polygon.points.push_back(a);
    }
  }

  return new_profile;
}

struct IndexedDouble
{
  size_t index;
  double value;
  IndexedDouble(size_t i, double v) : index(i), value(v) {}

  bool operator<(const IndexedDouble& other) {return value < other.value;}
};

inline double getRelevance(const Point32& pi, const Point32& pj, const Point32& pk)
{
  double relevance;
  double dx;
  double dy;
  dx = pi.x - pj.x;
  dy = pi.y - pj.y;
  relevance = sqrt(dx * dx + dy * dy);
  dx = pj.x - pk.x;
  dy = pj.y - pk.y;
  relevance += sqrt(dx * dx + dy * dy);
  dx = pi.x - pk.x;
  dy = pi.y - pk.y;
  relevance -= sqrt(dx * dx+ dy * dy);
  return std::abs(relevance);
}

double getRelevance(const std::list<IndexedDouble>& l, std::list<IndexedDouble>::const_iterator it,
    const vector<Point32>& points)
{
  std::list<IndexedDouble>::const_iterator pred = it;
  std::list<IndexedDouble>::const_iterator succ = it;
  pred--;
  succ++;

  double relevance = -1;
  if (it != l.begin() && succ != l.end())
  {
    relevance = getRelevance(points[pred->index], points[it->index], points[succ->index]);
  } 
  return relevance;
}

vector<Point32> simplifyPath(const vector<Point32>& points, size_t begin, size_t end, double min_relevance)
{
  if(begin >= points.size())
  {
    std::ostringstream msg;
    msg << "begin index (" << begin << ") should but strictly smaller than length of argument 1 (" <<
      points.size() << ")";
    throw std::runtime_error(msg.str());
  }
  if(end > points.size())
  {
    std::ostringstream msg;
    msg << "end index (" << end << ") should but smaller than or equal to length of argument 1 (" <<
      points.size() << ")";
    throw std::runtime_error(msg.str());
  }
  if(begin > end)
  {
    std::ostringstream msg;
    msg << "end index (" << end << ") should but larger than or equal to begin index (" <<
      begin << ")";
    throw std::runtime_error(msg.str());
  }

  if (end - begin < 3)
  {
    // Two points or less, just copy the points.
    vector<Point32> filtered_points;
    if ((end - begin) > 0)
    {
      filtered_points.reserve((end - begin) - 1);
    }
    for (size_t i = begin; i < end; ++i)
    {
      filtered_points.push_back(points[i]);
    }
    return filtered_points;
  }

  std::list<IndexedDouble> relevances;
  double relevance = std::numeric_limits<double>::min();
  for (size_t j = begin; j < end; ++j)
  {
    if (j != begin && j != end - 1)
    {
      relevance = getRelevance(points[j - 1], points[j], points[j + 1]);
    }
    relevances.push_back(IndexedDouble(j, relevance));
  }
  relevances.front().value = 10 * min_relevance;
  relevances.back().value = 10 * min_relevance;

  do
  {
    if (relevances.size() < 3)
    {
      break;
    }

    std::list<IndexedDouble>::iterator me = std::min_element(relevances.begin(), relevances.end());

    if (me == relevances.end())
    {
      // relevances is empty, finish.
      break;
    }
    if (me->value >= min_relevance)
    {
      // All points are relevant, finish.
      break;
    }
    // Because of the last test, the smallest relevance cannot be on the first
    // nor the last point, so the following is not risky.
    std::list<IndexedDouble>::iterator pred = me;
    --pred;
    std::list<IndexedDouble>::iterator succ = me;
    ++succ;
    relevances.erase(me);
    if (pred != --relevances.end())
    {
      // The point removed was not the before-the-last point.
      relevance = getRelevance(relevances, pred, points);
      if (relevance >= 0)
      {
        pred->value = relevance;
      }
    }
    if (succ != relevances.begin())
    {
      // The point removed was not the 2nd point.
      relevance = getRelevance(relevances, succ, points);
      if (relevance >= 0)
      {
        succ->value = relevance;
      }
    }
  } while (true);

  vector<Point32> filtered_points;
  filtered_points.reserve(relevances.size());
  for(std::list<IndexedDouble>::const_iterator it = relevances.begin(); it != relevances.end(); ++it)
  {
    filtered_points.push_back(points[it->index]);
  }
  return filtered_points;
}

void simplifyPlaceProfile(PlaceProfile& profile, double min_relevance)
{
  // Note: we do not simplify the segment between the last point and the first one.

  if (profile.polygon.points.size() < 3)
  {
    return;
  }

  if (profile.exclude_segments.empty())
  {
    profile.polygon.points = simplifyPath(profile.polygon.points, 0, profile.polygon.points.size(), min_relevance);
    return;
  }

  PlaceProfile old_profile = profile;

  // We need sorted excluded segments (points do not need to be sorted).
  normalizeExcludeSegments(old_profile);

  profile.polygon.points.clear();
  profile.polygon.points.reserve(old_profile.polygon.points.size());
  profile.exclude_segments.clear();
  profile.exclude_segments.reserve(old_profile.exclude_segments.size());

  size_t path_start = 0;
  for (size_t i = 0; i < old_profile.exclude_segments.size(); ++i)
  {
    const vector<Point32> filtered_points = simplifyPath(old_profile.polygon.points, path_start, old_profile.exclude_segments[i] + 1, min_relevance);
    std::copy(filtered_points.begin(), filtered_points.end(), std::back_inserter(profile.polygon.points));
    profile.exclude_segments.push_back(((int)profile.polygon.points.size()) - 1);
    path_start = old_profile.exclude_segments[i] + 1;
  }
  if (path_start < old_profile.polygon.points.size())
  {
    // If path_start is not after the last point, what happens if last point is excluded.
    const vector<Point32> filtered_points = simplifyPath(old_profile.polygon.points, path_start, old_profile.polygon.points.size(), min_relevance);
    std::copy(filtered_points.begin(), filtered_points.end(), std::back_inserter(profile.polygon.points));
  }
}

PlaceProfile simplifiedPlaceProfile(const PlaceProfile& profile, double min_relevance)
{
  PlaceProfile new_profile = profile;
  simplifyPlaceProfile(new_profile, min_relevance);
  return new_profile;
}

void curtailPlaceProfile(PlaceProfile& profile, double max_distance)
{
  const size_t size = profile.polygon.points.size();
  const double dist2 = max_distance * max_distance;
  vector<bool> in_range(size, false);
  for (size_t i = 0; i < size; ++i)
  {
    if ((profile.polygon.points[i].x * profile.polygon.points[i].x +
          profile.polygon.points[i].y * profile.polygon.points[i].y) < dist2)
    {
      in_range[i] = true;
    }
  }

  PlaceProfile old_profile = profile;
  profile.polygon.points.clear();
  profile.exclude_segments.clear();
  for (size_t i = 0; i < size; ++i)
  {
    if (in_range[i] && in_range[(i + 1) % size])
    {
      // point i and next point are included.
      profile.polygon.points.push_back(old_profile.polygon.points[i]);
    }
    else if (in_range[i] && !in_range[(i + 1) % size])
    {
      // point i is included, next point (which will not be part of
      // profile.polygon) is excluded.
      profile.polygon.points.push_back(old_profile.polygon.points[i]);
      profile.exclude_segments.push_back(profile.polygon.points.size() - 1);
    }
  }

  if (profile.polygon.points.size() < 3)
  {
    profile.exclude_segments.clear();
  }
  else if (!profile.exclude_segments.empty() && (profile.exclude_segments.front() == -1))
  {
    // If segment between last point and point 0 is excluded.
    // This happens if the first range(s) is(are) out of range.
    profile.exclude_segments[0] = profile.polygon.points.size() - 1;
  }
}

PlaceProfile curtailedPlaceProfile(const PlaceProfile& profile, double max_distance)
{
  PlaceProfile new_profile = profile;
  curtailPlaceProfile(new_profile, max_distance);
  return new_profile;
}

} // namespace lama_common