Add random polygonal features

Author: Saurav Agarwal

cppsrc/core/CMakeLists.txt

@@ -118,6 +118,7 @@ set(sources_list
 	polygon_utils.cpp
 	parameters.cpp
 	voronoi.cpp
+  world_idf.cpp
 	coverage_system.cpp
 	plotter.cpp
 	cuda_utils.cpp

cppsrc/core/include/CoverageControl/cgal/config.h

@@ -36,8 +36,12 @@
 #include <CGAL/Exact_predicates_exact_constructions_kernel.h>
 #include <CGAL/Partition_traits_2.h>
 #include <CGAL/Polygon_with_holes_2.h>
+#include <CGAL/Random.h>
+#include <CGAL/algorithm.h>
 #include <CGAL/centroid.h>
 #include <CGAL/partition_2.h>
+#include <CGAL/point_generators_2.h>
+#include <CGAL/random_polygon_2.h>
 
 #include <iterator>
 
@@ -58,4 +62,7 @@ typedef CGAL::Arr_walk_along_line_point_location<Arrangement_2> CGAL_pl;
 
 typedef CGAL::Partition_traits_2<K> Partition_traits_2;
 
+typedef CGAL::Creator_uniform_2<double, CGAL_Point2> Creator;
+typedef CGAL::Random_points_in_square_2<CGAL_Point2, Creator> Point_generator;
+
 #endif  // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_CGAL_CONFIG_H_

cppsrc/core/include/CoverageControl/cgal/polygon_utils.h

@@ -43,6 +43,20 @@ namespace CoverageControl {
 void PolygonYMonotonePartition(PointVector const &polygon,
                                std::vector<PointVector> &y_monotone_polygons);
 
+/*! \brief Generate random polygons
+ *
+ * @param[in] num_polygons The number of polygons to generate
+ * @param[in] max_vertices The maximum number of vertices in each polygon
+ * @param[bounding_box_lower] The lower left corner of the bounding box
+ * @param[bounding_box_upper] The upper right corner of the bounding box
+ * @param[radius] The radius of the circle in which the polygons are generated
+ * @param[out] polygons The output polygons
+ */
+
+void GenerateRandomPolygons(int const num_polygons, int const max_vertices,
+                            double const half_width, double const world_size,
+                            std::vector<PointVector> &polygons);
+
 } /* namespace CoverageControl */
 
 #endif  // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_CGAL_POLYGON_UTILS_H_

cppsrc/core/include/CoverageControl/coverage_system.h

@@ -180,6 +180,9 @@ class CoverageSystem {
   CoverageSystem(Parameters const &params, int const num_gaussians,
                  int const num_robots);
 
+  CoverageSystem(Parameters const &params, int const num_gaussians,
+                 int const num_polygons, int const num_robots);
+
   /*! \brief Create with given world IDF and robot positions from file
    *
    *
@@ -441,10 +444,13 @@ class CoverageSystem {
   //
   //! @{
   const auto &GetWorldIDFObject() const { return world_idf_; }
-  const MapType &GetWorldMap() const { return world_idf_.GetWorldMap(); }
   const MapType &GetSystemMap() const { return system_map_; }
   const MapType &GetSystemExplorationMap() const { return exploration_map_; }
   const MapType &GetSystemExploredIDFMap() const { return explored_idf_map_; }
+  //! Get the world map
+  const MapType &GetWorldMap() const { return world_idf_.GetWorldMap(); }
+  //! Get the world map (mutable)
+  MapType &GetWorldMapMutable() { return world_idf_.GetWorldMapMutable(); }
 
   inline auto GetNumRobots() const { return num_robots_; }
   inline auto GetNumFeatures() const { return num_robots_; }

cppsrc/core/include/CoverageControl/cuda/geometry_utils.cuh

@@ -41,7 +41,8 @@ namespace CoverageControl {
  * left of the vector \return -1 if point r is in clockwise direction, i.e.,
  * right of the vector \return  0 otherwise
  */
-__device__ int Orientation(float2 const &p, float2 const &q, float2 const &r) {
+__host__ __device__ int Orientation(float2 const &p, float2 const &q,
+                                    float2 const &r) {
   float2 qp{q.x - p.x, q.y - p.y};
   float2 rp{r.x - p.x, r.y - p.y};
   float cross_prod1 = qp.x * rp.y;
@@ -55,8 +56,8 @@ __device__ int Orientation(float2 const &p, float2 const &q, float2 const &r) {
  * Check if the point r is inside the monotone polygon defined by the vertices x
  * and y \return true if the point is inside the polygon \return false otherwise
  */
-__device__ bool IsPointInMonotonePolygon(float *x, float *y, int sz,
-                                         float2 const &r) {
+__host__ __device__ bool IsPointInMonotonePolygon(float *x, float *y, int sz,
+                                                  float2 const &r) {
   bool left = false;
   bool right = false;
   for (int ct = 0; ct < sz; ++ct) {

cppsrc/core/include/CoverageControl/parameters.h

@@ -57,6 +57,9 @@ class Parameters {
    */
   int pNumRobots = 32;    //!< Number of robots
   int pNumFeatures = 32;  //!< Number of features
+  int pNumPolygons = 0;   //!< Number of polygonal features
+  int pMaxVertices = 10;  //!< Maximum number of vertices in a polygon
+  double pPolygonRadius = 64;
 
   /*! \name Map Parameters
    * @{

cppsrc/core/include/CoverageControl/world_idf.h

@@ -176,6 +176,20 @@ class WorldIDF {
     params_ = params;
   }
 
+  WorldIDF(Parameters const &params, MapType const &world_map)
+      : WorldIDF(params.pWorldMapSize) {
+    params_ = params;
+    if (world_map.rows() != params.pWorldMapSize ||
+        world_map.cols() != params.pWorldMapSize) {
+      std::cout << "Error: World map size does not match the specified size"
+                << std::endl;
+      throw std::invalid_argument(
+          "World map size does not match the specified size");
+      exit(1);
+    }
+    world_map_ = world_map;
+  }
+
   WorldIDF(Parameters const &params, std::string const &file_name)
       : WorldIDF(params.pWorldMapSize) {
     params_ = params;
@@ -201,14 +215,15 @@ class WorldIDF {
       } else if (type == "Uniform") {
         int num_vertices;
         file >> num_vertices;
+        double importance;
+        file >>
+            importance;  // importance moved here. Be careful with semantic file
         std::vector<Point2> vertices;
         for (int i = 0; i < num_vertices; ++i) {
           double x, y;
           file >> x >> y;
           vertices.push_back(Point2(x, y));
         }
-        double importance;
-        file >> importance;
         AddUniformDistributionPolygon(PolygonFeature(vertices, importance));
       } else {
         std::cout << "Error: Unknown feature type " << type << std::endl;
@@ -305,74 +320,15 @@ class WorldIDF {
 
   const MapType &GetWorldMap() const { return world_map_; }
 
-  inline int WriteDistributions(std::string const &file_name) const {
-    std::ofstream file(file_name);
-    if (!file.is_open()) {
-      std::cerr << "Could not open file: " << file_name << std::endl;
-      return -1;
-    }
-    file << std::setprecision(kMaxPrecision);
-    for (auto const &dist : normal_distributions_) {
-      Point2 sigma = dist.GetSigma();
-      if (sigma.x() == sigma.y()) {
-        file << "CircularBND" << std::endl;
-        file << dist.GetMean().x() << " " << dist.GetMean().y() << " "
-             << sigma.x() << " " << dist.GetScale() << std::endl;
-      } else {
-        file << "BND" << std::endl;
-        file << dist.GetMean().x() << " " << dist.GetMean().y() << " "
-             << dist.GetSigma().x() << " " << dist.GetSigma().y() << " "
-             << dist.GetRho() << " " << dist.GetScale() << std::endl;
-      }
-    }
-    file.close();
-    return 0;
-  }
+  MapType &GetWorldMapMutable() { return world_map_; }
+
+  int WriteDistributions(std::string const &file_name) const;
 
   auto GetNumFeatures() const {
     return normal_distributions_.size() + polygon_features_.size();
   }
 
-  /*! Fills in values of the world_map_ with the total importance for each cell
-   */
-  void GenerateMapCPU() {
-    /* std::cout << "Generating map using CPU" << std::endl; */
-    float max_importance = 0;
-    float res = static_cast<float>(params_.pResolution);
-    for (int i = 0; i < params_.pWorldMapSize; ++i) {  // Row (x index)
-      float x1 = res * i;   // Left x-coordinate of pixel
-      float x2 = x1 + res;  // Right x-coordinate of pixel
-      for (int j = 0; j < params_.pWorldMapSize; ++j) {  // Column (y index)
-        float y1 = res * j;   // Lower y-coordinate of pixel
-        float y2 = y1 + res;  // Upper y-coordinate of pixel
-        float importance = ComputeImportanceBND<Point2f, float>(
-            Point2f(x1, y1), Point2f(x2, y2));
-        /* auto importance = ComputeImportanceBND(pt1, pt2); */
-        /* if (std::abs(importance) < kEps) { */
-        /*   importance = 0; */
-        /* } */
-        world_map_(i, j) = importance;
-        if (importance > max_importance) {
-          max_importance = importance;
-        }
-      }
-    }
-
-    if (max_importance < kEps) {
-      normalization_factor_ = static_cast<float>(params_.pNorm);
-    } else {
-      normalization_factor_ =
-          static_cast<float>(params_.pNorm) / max_importance;
-    }
-
-    // Normalize the world map
-#pragma omp parallel for
-    for (int i = 0; i < params_.pWorldMapSize; ++i) {
-      for (int j = 0; j < params_.pWorldMapSize; ++j) {
-        world_map_(i, j) *= normalization_factor_;
-      }
-    }
-  }
+  void GenerateMapCPU();
 
 #ifdef COVERAGECONTROL_WITH_CUDA
   void GenerateMapCuda() {

cppsrc/core/src/coverage_system.cpp

@@ -32,9 +32,15 @@
 namespace CoverageControl {
 
 CoverageSystem::CoverageSystem(Parameters const &params)
-    : CoverageSystem(params, params.pNumFeatures, params.pNumRobots) {}
+    : CoverageSystem(params, params.pNumFeatures, params.pNumPolygons,
+                     params.pNumRobots) {}
 
-CoverageSystem::CoverageSystem(Parameters const &params, int const num_features,
+CoverageSystem::CoverageSystem(Parameters const &params,
+                               int const num_gaussians, int const num_robots)
+    : CoverageSystem(params, num_gaussians, 0, num_robots) {}
+
+CoverageSystem::CoverageSystem(Parameters const &params,
+                               int const num_gaussians, int const num_polygons,
                                int const num_robots)
     : params_{params}, world_idf_{WorldIDF(params_)} {
   // Generate Bivariate Normal Distribution from random numbers
@@ -48,22 +54,34 @@ CoverageSystem::CoverageSystem(Parameters const &params, int const num_features,
                                                params_.pMaxSigma);
   std::uniform_real_distribution<> distrib_peak(params_.pMinPeak,
                                                 params_.pMaxPeak);
-  for (int i = 0; i < num_features; ++i) {
+  for (int i = 0; i < num_gaussians; ++i) {
     Point2 mean(distrib_pts_(gen_), distrib_pts_(gen_));
     double sigma(distrib_var(gen_));
     double scale(distrib_peak(gen_));
+    scale = 2 * M_PI * sigma * sigma * scale;
     BivariateNormalDistribution dist(mean, sigma, scale);
     world_idf_.AddNormalDistribution(dist);
   }
 
+  std::vector<PointVector> polygons;
+  GenerateRandomPolygons(num_polygons, params_.pMaxVertices,
+                         params_.pPolygonRadius,
+                         params_.pWorldMapSize * params_.pResolution, polygons);
+
+  for (auto &poly : polygons) {
+    double importance = distrib_peak(gen_) * 0.5;
+    PolygonFeature poly_feature(poly, importance);
+    world_idf_.AddUniformDistributionPolygon(poly_feature);
+  }
+
   world_idf_.GenerateMap();
   normalization_factor_ = world_idf_.GetNormalizationFactor();
 
-  std::uniform_real_distribution<> robot_pos_dist(
+  std::uniform_real_distribution<> env_point_dist(
       kLargeEps, params_.pRobotInitDist - kLargeEps);
   robots_.reserve(num_robots);
   for (int i = 0; i < num_robots; ++i) {
-    Point2 start_pos(robot_pos_dist(gen_), robot_pos_dist(gen_));
+    Point2 start_pos(env_point_dist(gen_), env_point_dist(gen_));
     robots_.push_back(RobotModel(params_, start_pos, world_idf_));
   }
   InitSetup();