Port Add height of cluster with the most points method #323 to master

grid_map_pcl/README.md

@@ -65,6 +65,11 @@ Resulting grid map parameters.
 * **pcl_grid_map_extraction/grid_map/min_num_points_per_cell** Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN.
 * **pcl_grid_map_extraction/grid_map/max_num_points_per_cell** Maximum number of points in the point cloud that are allowed to fall within any of the grid map cells. If there is more points, the value will be set to NaN. This number can be used to ignore cells with a lot of points which speeds up the processing (but results with some holes in the map).
 * **pcl_grid_map_extraction/grid_map/resolution** Resolution of the grid map. Width and lengths are computed automatically.
+* **pcl_grid_map_extraction/grid_map/height_type** How to calculate elevation value.
+0: Smallest value among the average values ​​of each cluster
+1: Largest value among the average values ​​of each cluster
+2: Mean value of the cluster with the most points
+* **pcl_grid_map_extraction/grid_map/height_thresh** Height range from the smallest cluster.(Only for height_type 2)
 
 ### Point Cloud Pre-processing Parameters
 
@@ -79,7 +84,6 @@ Cluster extraction is based on pcl algorithms. See: http://pointclouds.org/docum
 * **pcl_grid_map_extraction/cluster_extraction/cluster_tolerance** Distance between points below which they will still be considered part of one cluster. 
 * **pcl_grid_map_extraction/cluster_extraction/min_num_points** Min number of points that a cluster needs to have (otherwise it will be discarded). 
 * **pcl_grid_map_extraction/cluster_extraction/max_num_points** Max number of points that a cluster can have (otherwise it will be discarded). 
-* **pcl_grid_map_extraction/cluster_extraction/use_max_height_as_cell_elevation** Use the maximum cluster to extract the cell height. If false (default), the lowest cluster is taken. 
 
 ##### Outlier removal parameters
 See http://pointclouds.org/documentation/tutorials/statistical_outlier.php for more explanation on outlier removal.

grid_map_pcl/config/parameters.yaml

@@ -28,6 +28,12 @@ pcl_grid_map_extraction:
     min_num_points_per_cell: 4
     max_num_points_per_cell: 100000
     resolution: 0.1
+    # 0: Smallest value among the average values ​​of each cluster
+    # 1: Largest value among the average values ​​of each cluster
+    # 2: Mean value of the cluster with the most points
+    height_type: 0
+    # For height_type 2
+    height_thresh: 1.0
 
 
 

grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp

@@ -153,10 +153,13 @@ class GridMapPclLoader {
    * maps. Function finds all the clusters and for each of them it computes a mean of point
    * positions. Then it looks at z value of the means and takes the lowest one.
    * @param[in] point cloud that is entirely contained inside a grid map cell
+   * @param[in] height_type which is the method of calculation height value
    * @return elevation value computed from the input point cloud. It will return NaN if no clusters
    * have been found or an empty cloud is passed in.
    */
-  void calculateElevationFromPointsInsideGridMapCell(Pointcloud::ConstPtr cloud, std::vector<float>& heights) const;
+  float calculateElevationFromPointsInsideGridMapCell(
+      Pointcloud::ConstPtr cloud, const int height_type,
+      std::vector<float> &heights) const;
 
   /*!
    * Allocates space for the point clouds and dispatches points to the

grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp

@@ -31,7 +31,6 @@ class PclLoaderParameters {
     double clusterTolerance_ = 0.3;
     unsigned int minNumPoints_ = 2;
     unsigned int maxNumPoints_ = 1000000;
-    bool useMaxHeightAsCellElevation_;
   };
   struct OutlierRemovalParameters {
     bool isRemoveOutliers_ = false;
@@ -47,6 +46,12 @@ class PclLoaderParameters {
     double resolution_ = 0.1;
     unsigned int minCloudPointsPerCell_ = 2;
     unsigned int maxCloudPointsPerCell_ = 100000;
+    // 0: Smallest value among the average values ​​of each cluster
+    // 1: Largest value among the average values ​​of each cluster
+    // 2: Mean value of the cluster with the most points
+    unsigned int height_type_ = 0;
+    // For height_type 2
+    double height_thresh_ = 1.0;
   };
 
   struct Parameters {

grid_map_pcl/src/GridMapPclLoader.cpp

@@ -134,31 +134,57 @@ void GridMapPclLoader::processGridMapCell(const unsigned int linearGridMapIndex,
     return;
   }
   auto& clusterHeights = clusterHeightsWithingGridMapCell_[index(0)][index(1)];
-  calculateElevationFromPointsInsideGridMapCell(pointsInsideCellBorder, clusterHeights);
-  if (clusterHeights.empty()) {
-    (*gridMapData)(index(0), index(1)) = std::nan("1");
-  } else {
-    (*gridMapData)(index(0), index(1)) = params_.get().clusterExtraction_.useMaxHeightAsCellElevation_
-                                             ? *(std::max_element(clusterHeights.begin(), clusterHeights.end()))
-                                             : *(std::min_element(clusterHeights.begin(), clusterHeights.end()));
-  }
+  const int height_type = params_.get().gridMap_.height_type_;
+  (*gridMapData)(index(0), index(1)) =
+      calculateElevationFromPointsInsideGridMapCell(
+          pointsInsideCellBorder, height_type, clusterHeights);
 }
 
-void GridMapPclLoader::calculateElevationFromPointsInsideGridMapCell(Pointcloud::ConstPtr cloud, std::vector<float>& heights) const {
+float GridMapPclLoader::calculateElevationFromPointsInsideGridMapCell(
+    Pointcloud::ConstPtr cloud, const int height_type,
+    std::vector<float> &heights) const {
   heights.clear();
   // Extract point cloud cluster from point cloud and return if none is found.
   std::vector<Pointcloud::Ptr> clusterClouds = pointcloudProcessor_.extractClusterCloudsFromPointcloud(cloud);
   const bool isNoClustersFound = clusterClouds.empty();
   if (isNoClustersFound) {
     ROS_WARN_STREAM_THROTTLE(10.0, "No clusters found in the grid map cell");
-    return;
+    return std::nan("1");
   }
 
   // Extract mean z value of cluster vector and return smallest height value
   heights.reserve(clusterClouds.size());
 
   std::transform(clusterClouds.begin(), clusterClouds.end(), std::back_inserter(heights),
                  [this](Pointcloud::ConstPtr cloud) -> double { return grid_map_pcl::calculateMeanOfPointPositions(cloud).z(); });
+
+  double height;
+  // 0: smallest value among the mean values ​​of each cluster
+  if (height_type == 0) {
+    height = *(std::min_element(heights.begin(), heights.end()));
+  }
+  // 1: largest value among the mean values ​​of each cluster
+  else if (height_type == 1) {
+    height = *(std::max_element(heights.begin(), heights.end()));
+  }
+  // 2: Mean value of the cluster with the most points
+  else if (height_type == 2) {
+    const float min_height = *(std::min_element(heights.begin(), heights.end()));
+    std::vector<int> clusterSizes(clusterClouds.size());
+    for (size_t i = 0; i < clusterClouds.size(); i++) {
+      clusterSizes[i] = heights[i] - min_height < params_.get().gridMap_.height_thresh_ ?
+        clusterClouds[i]->size() :
+        -1;
+    }
+    const std::vector<int>::iterator maxIt =
+      std::max_element(clusterSizes.begin(), clusterSizes.end());
+    const size_t maxIndex = std::distance(clusterSizes.begin(), maxIt);
+    height = heights[maxIndex];
+  } else {
+    return std::nan("1");
+  }
+
+  return height;
 }
 
 GridMapPclLoader::Pointcloud::Ptr GridMapPclLoader::getPointcloudInsideGridMapCellBorder(const grid_map::Index& index) const {
@@ -167,6 +193,17 @@ GridMapPclLoader::Pointcloud::Ptr GridMapPclLoader::getPointcloudInsideGridMapCe
 
 void GridMapPclLoader::loadParameters(const std::string& filename) {
   params_.loadParameters(filename);
+
+  const int height_type = params_.get().gridMap_.height_type_;
+  if ((std::set<int>{0, 1, 2}).count(height_type) == 0) {
+    ROS_ERROR_STREAM(
+        "Invalid height type: " + std::to_string(height_type) +
+        "\nValid types are below" +
+        "\n0: Smallest value among the average values of each cluster" +
+        "\n1: Largest value among the average values of each cluster" +
+        "\n2: Mean value of the cluster with the most points");
+  }
+
   pointcloudProcessor_.loadParameters(filename);
 }
 

grid_map_pcl/src/PclLoaderParameters.cpp

@@ -25,8 +25,6 @@ void PclLoaderParameters::loadParameters(const YAML::Node& yamlNode) {
   parameters_.cloudTransformation_.rpyIntrinsic_.y() = yamlNode["cloud_transform"]["rotation"]["p"].as<double>();
   parameters_.cloudTransformation_.rpyIntrinsic_.z() = yamlNode["cloud_transform"]["rotation"]["y"].as<double>();
 
-  parameters_.clusterExtraction_.useMaxHeightAsCellElevation_ =
-      yamlNode["cluster_extraction"]["use_max_height_as_cell_elevation"].as<bool>();
   parameters_.clusterExtraction_.clusterTolerance_ = yamlNode["cluster_extraction"]["cluster_tolerance"].as<double>();
   parameters_.clusterExtraction_.minNumPoints_ = yamlNode["cluster_extraction"]["min_num_points"].as<int>();
   parameters_.clusterExtraction_.maxNumPoints_ = yamlNode["cluster_extraction"]["max_num_points"].as<int>();
@@ -38,6 +36,8 @@ void PclLoaderParameters::loadParameters(const YAML::Node& yamlNode) {
   parameters_.gridMap_.resolution_ = yamlNode["grid_map"]["resolution"].as<double>();
   parameters_.gridMap_.minCloudPointsPerCell_ = yamlNode["grid_map"]["min_num_points_per_cell"].as<int>();
   parameters_.gridMap_.maxCloudPointsPerCell_ = yamlNode["grid_map"]["max_num_points_per_cell"].as<int>();
+  parameters_.gridMap_.height_type_ = yamlNode["grid_map"]["height_type"].as<int>();
+  parameters_.gridMap_.height_thresh_ = yamlNode["grid_map"]["height_thresh"].as<float>();
 
   parameters_.downsampling_.isDownsampleCloud_ = yamlNode["downsampling"]["is_downsample_cloud"].as<bool>();
   parameters_.downsampling_.voxelSize_.x() = yamlNode["downsampling"]["voxel_size"]["x"].as<double>();

grid_map_pcl/test/GridMapPclLoaderTest.cpp

@@ -194,9 +194,10 @@ TEST(GridMapPclLoaderTest, CalculateElevation)  // NOLINT
     gridMapPclLoader.loadParameters(grid_map_pcl_test::getConfigFilePath());
     gridMapPclLoader.setInputCloud(cloud);
 
+    // 0: Smallest value among the average values ​​of each cluster
+    const int height_type = 0;
     std::vector<float> clusterHeights;
-    gridMapPclLoader.calculateElevationFromPointsInsideGridMapCell(cloud, clusterHeights);
-    const float elevation = *std::min_element(clusterHeights.begin(), clusterHeights.end());
+    const float elevation = gridMapPclLoader.calculateElevationFromPointsInsideGridMapCell(cloud, height_type, clusterHeights);
     EXPECT_NEAR(elevation, minZ, 3 * stdDevZ);
   }
 

grid_map_pcl/test/test_data/parameters.yaml

@@ -13,7 +13,6 @@ pcl_grid_map_extraction:
     cluster_tolerance: 0.05
     min_num_points: 4
     max_num_points: 1000000
-    use_max_height_as_cell_elevation: false
   outlier_removal:
     is_remove_outliers: true
     mean_K: 10
@@ -28,6 +27,12 @@ pcl_grid_map_extraction:
     min_num_points_per_cell: 4
     max_num_points_per_cell: 100000
     resolution: 0.1
+    # 0: Smallest value among the average values ​​of each cluster
+    # 1: Largest value among the average values ​​of each cluster
+    # 2: Mean value of the cluster with the most points
+    height_type: 0
+    # For height_type 2
+    height_thresh: 1.0