voronoi_point.cpp
Go to the documentation of this file.
00001 
00038 #include <bwi_mapper/structures/voronoi_point.h>
00039 #include <bwi_mapper/directed_dfs.h>
00040 #include <bwi_mapper/point_utils.h>
00041 
00042 namespace bwi_mapper {
00043 
00053   void VoronoiPoint::addBasisCandidate(const Point2d& candidate, 
00054       uint32_t threshold, const nav_msgs::OccupancyGrid& map, bool is_naive) {
00055 
00056     if (is_naive) {
00057       if (basis_points.size() == 0) {
00058         basis_points.push_back(candidate);
00059         basis_distance = candidate.distance_from_ref;
00060         return;
00061       }
00062       if (candidate.distance_from_ref > basis_distance + 0.01) {
00063         return;
00064       }
00065       if (candidate.distance_from_ref <= basis_distance - 0.01) {
00066         basis_points.clear();
00067         basis_points.push_back(candidate);
00068         basis_distance = candidate.distance_from_ref;
00069         return;
00070       }
00071       
00072       // Check if the point we are adding is next to an existing basis point 
00073       if (basis_points.size() == 1) {
00074         Point2d basis = basis_points[0];
00075         int diff_x = abs(basis.x - candidate.x);
00076         int diff_y = abs(basis.y - candidate.y);
00077         if (diff_x <= 1 && diff_y <= 1) {
00078           // Point is from the same site
00079           return;
00080         }
00081       }
00082       basis_points.push_back(candidate);
00083       return;
00084     }
00085     
00086     if (basis_points.size() == 0) {
00087       basis_points.push_back(candidate);
00088       basis_distance = candidate.distance_from_ref;
00089       return;
00090     }
00091     if (candidate.distance_from_ref > basis_distance + 1) {
00092       return;
00093     }
00094 
00095     basis_points.push_back(candidate);
00096     std::sort(basis_points.begin(), basis_points.end(), 
00097         Point2dDistanceComp());
00098     basis_distance = basis_points[0].distance_from_ref;
00099 
00100     // Get the directed DFS searcher
00101     DirectedDFS dfs(map);
00102 
00103     // Mark elements that are too close to be erased
00104     std::vector<size_t> elements_to_erase;
00105     for (size_t i = 0; i < basis_points.size(); ++i) {
00106 
00107       // Check if the clearance for this point is much further away from the 
00108       // minimum clearance
00109       if (basis_points[i].distance_from_ref > basis_distance + 1) {
00110         elements_to_erase.push_back(i);
00111         break;
00112       }
00113 
00114       std::vector<size_t>::iterator erase_iterator = 
00115         elements_to_erase.begin();
00116 
00117       for (size_t j = 0; j < i; ++j) {
00118 
00119         while (erase_iterator != elements_to_erase.end() && 
00120             *erase_iterator < j) {
00121           erase_iterator++;
00122         }
00123 
00124         if (erase_iterator != elements_to_erase.end() && 
00125             *erase_iterator == j) { 
00126           continue;
00127         }
00128 
00129         // See if basis point i is too close to basis point j. retain j
00130         float distance = bwi_mapper::getMagnitude(basis_points[i] - basis_points[j]);
00131         if (distance < 2 * threshold) {
00132           elements_to_erase.push_back(i); // does not affect erase_iterator
00133           break;
00134         }
00135 
00136         // See if these basis points are really close by searching for a
00137         // short path in the walls
00138         if (dfs.searchForPath(basis_points[i], basis_points[j], 
00139               2 * basis_distance)) {
00140           elements_to_erase.push_back(i);
00141           break;
00142         }
00143       }
00144     }
00145 
00146     // Actually remove elements from the basis point array
00147     for (size_t i = elements_to_erase.size() - 1; 
00148         i <= elements_to_erase.size(); --i) {
00149       basis_points.erase(basis_points.begin() + elements_to_erase[i]);
00150     }
00151   }
00152 
00153 } /* bwi_mapper */


bwi_mapper
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:21