voronoi_point.cpp

Go to the documentation of this file.

#include <bwi_mapper/structures/voronoi_point.h>
#include <bwi_mapper/directed_dfs.h>
#include <bwi_mapper/point_utils.h>

namespace bwi_mapper {

  void VoronoiPoint::addBasisCandidate(const Point2d& candidate, 
      uint32_t threshold, const nav_msgs::OccupancyGrid& map, bool is_naive) {

    if (is_naive) {
      if (basis_points.size() == 0) {
        basis_points.push_back(candidate);
        basis_distance = candidate.distance_from_ref;
        return;
      }
      if (candidate.distance_from_ref > basis_distance + 0.01) {
        return;
      }
      if (candidate.distance_from_ref <= basis_distance - 0.01) {
        basis_points.clear();
        basis_points.push_back(candidate);
        basis_distance = candidate.distance_from_ref;
        return;
      }
      
      // Check if the point we are adding is next to an existing basis point 
      if (basis_points.size() == 1) {
        Point2d basis = basis_points[0];
        int diff_x = abs(basis.x - candidate.x);
        int diff_y = abs(basis.y - candidate.y);
        if (diff_x <= 1 && diff_y <= 1) {
          // Point is from the same site
          return;
        }
      }
      basis_points.push_back(candidate);
      return;
    }
    
    if (basis_points.size() == 0) {
      basis_points.push_back(candidate);
      basis_distance = candidate.distance_from_ref;
      return;
    }
    if (candidate.distance_from_ref > basis_distance + 1) {
      return;
    }

    basis_points.push_back(candidate);
    std::sort(basis_points.begin(), basis_points.end(), 
        Point2dDistanceComp());
    basis_distance = basis_points[0].distance_from_ref;

    // Get the directed DFS searcher
    DirectedDFS dfs(map);

    // Mark elements that are too close to be erased
    std::vector<size_t> elements_to_erase;
    for (size_t i = 0; i < basis_points.size(); ++i) {

      // Check if the clearance for this point is much further away from the 
      // minimum clearance
      if (basis_points[i].distance_from_ref > basis_distance + 1) {
        elements_to_erase.push_back(i);
        break;
      }

      std::vector<size_t>::iterator erase_iterator = 
        elements_to_erase.begin();

      for (size_t j = 0; j < i; ++j) {

        while (erase_iterator != elements_to_erase.end() && 
            *erase_iterator < j) {
          erase_iterator++;
        }

        if (erase_iterator != elements_to_erase.end() && 
            *erase_iterator == j) { 
          continue;
        }

        // See if basis point i is too close to basis point j. retain j
        float distance = bwi_mapper::getMagnitude(basis_points[i] - basis_points[j]);
        if (distance < 2 * threshold) {
          elements_to_erase.push_back(i); // does not affect erase_iterator
          break;
        }

        // See if these basis points are really close by searching for a
        // short path in the walls
        if (dfs.searchForPath(basis_points[i], basis_points[j], 
              2 * basis_distance)) {
          elements_to_erase.push_back(i);
          break;
        }
      }
    }

    // Actually remove elements from the basis point array
    for (size_t i = elements_to_erase.size() - 1; 
        i <= elements_to_erase.size(); --i) {
      basis_points.erase(basis_points.begin() + elements_to_erase[i]);
    }
  }

} /* bwi_mapper */