himmelsbach.hpp

Go to the documentation of this file.

#pragma once
#include "ground_removal/ground_removal.hpp"
 
int constexpr NUM_RINGS = 40;
 
struct Ring {
  std::vector<int> indices;
 
  Ring() = default;
};
 
struct Slice {
  std::vector<Ring> rings{::NUM_RINGS};
 
  Slice() = default;
};
 
class Himmelsbach : public GroundRemoval {
public:
  Himmelsbach(const double grid_angle, const double max_slope, const double initial_alpha,
              const double alpha_augmentation_m, const double start_augmentation,
              TrimmingParameters trim_params);
 
  Himmelsbach() = default;
 
  void ground_removal(const sensor_msgs::msg::PointCloud2::SharedPtr& trimmed_point_cloud,
                      sensor_msgs::msg::PointCloud2::SharedPtr& ground_removed_point_cloud,
                      GroundGrid& ground_grid) const override;
 
private:
  double grid_angle_;     // Angle increment for slicing the point cloud
  double max_slope_;      // Maximum slope to consider a point as ground candidate
  double initial_alpha_;  // Initial distance threshold for ground classification of candidates
  double alpha_augmentation_m_;  // Increase of alpha by meter
  double start_augmentation_;    // Range at which distance threshold starts to increase
  TrimmingParameters trim_params_;
  std::shared_ptr<std::vector<Slice>>
      slices_;  // Vector containing all slices, initialized in constructor
 
  void process_slice(const sensor_msgs::msg::PointCloud2::SharedPtr& trimmed_point_cloud,
                     sensor_msgs::msg::PointCloud2::SharedPtr& ground_removed_point_cloud,
                     size_t slice_idx, GroundGrid& ground_grid) const;
 
  void split_point_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr& input_cloud) const;
};