Merge branch 'feature/grid_map_pcl_maxmin_elevation' into 'master'

[grid_map_pcl] Added a parameter to choose whether elevation takes min or max values as reference

GitOrigin-RevId: e10b0e41c9709a36cd256bb53d2dee520c43aa25

Author: mgaertneratanybotics

grid_map_pcl/README.md

@@ -8,7 +8,7 @@ This is a C++ package integrated with ROS for computing 2.5D elevation maps from
 The elevation is computed by raytracing lines coming from the grid map cells and computing their intersection with the mesh.
 
 #### Converting a raw pointcloud to a grid map
-The elevation is computed by slicing the point cloud in the x-y plane into columns. Column location and size correspond to the location and size of grid map cells in the point cloud coordinate frame. Subsequently, the clustering of points inside each column is performed and the elevation of the lowest cluster (min z coordinate) is assumed to be the terrain elevation. The elevation of the cluster is computed as a mean of all positions from the points that belong to a cluster. All calculations are performed in the pointcloud frame.
+The elevation is computed by slicing the point cloud in the x-y plane into columns. Column location and size correspond to the location and size of grid map cells in the point cloud coordinate frame. Subsequently, the clustering of points inside each column is performed and the elevation of the lowest or highest cluster (min/max z coordinate) is assumed to be the terrain elevation. The elevation of the cluster is computed as a mean of all positions from the points that belong to a cluster. All calculations are performed in the pointcloud frame.
 
 
 **Authors: Edo Jelavic, Dominic Jud<br />
@@ -65,6 +65,7 @@ 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

@@ -13,6 +13,7 @@ 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

grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp

@@ -32,6 +32,7 @@ class PclLoaderParameters {
 		double clusterTolerance_ = 0.3;
 		unsigned int minNumPoints_ = 2;
 		unsigned int maxNumPoints_ = 1000000;
+		bool useMaxHeightAsCellElevation_;
 	};
 	struct OutlierRemovalParameters {
 		bool isRemoveOutliers_ = false;

grid_map_pcl/src/GridMapPclLoader.cpp

@@ -174,9 +174,10 @@ double GridMapPclLoader::calculateElevationFromPointsInsideGridMapCell(
   std::transform(clusterClouds.begin(), clusterClouds.end(), clusterHeights.begin(),
                  [this](Pointcloud::ConstPtr cloud) -> double {return grid_map_pcl::calculateMeanOfPointPositions(cloud).z();});
 
-  double minClusterHeight = *(std::min_element(clusterHeights.begin(), clusterHeights.end()));
+  const double clusterHeight = params_->get().clusterExtraction_.useMaxHeightAsCellElevation_ ? *(std::max_element(clusterHeights.begin(), clusterHeights.end()))
+                                                            : *(std::min_element(clusterHeights.begin(), clusterHeights.end()));
 
-  return minClusterHeight;
+  return clusterHeight;
 }
 
 GridMapPclLoader::Pointcloud::Ptr GridMapPclLoader::getPointcloudInsideGridMapCellBorder(

grid_map_pcl/src/PclLoaderParameters.cpp

@@ -36,6 +36,9 @@ void PclLoaderParameters::handleYamlNode(const YAML::Node &yamlNode) {
 	parameters_.cloudTransformation_.rpyIntrinsic_.z() =
 			yamlNode[prefix]["cloud_transform"]["rotation"]["y"].as<double>();
 
+	parameters_.clusterExtraction_.useMaxHeightAsCellElevation_ =
+			yamlNode[prefix]["cluster_extraction"]["use_max_height_as_cell_elevation"].as<
+					bool>();
 	parameters_.clusterExtraction_.clusterTolerance_ =
 			yamlNode[prefix]["cluster_extraction"]["cluster_tolerance"].as<
 					double>();