ground_segmentation::PointCloudGrid< PointT > Class Template Reference

Core grid-based ground segmentation algorithm. More...

#include <ground_detection.hpp>

Collaboration diagram for ground_segmentation::PointCloudGrid< PointT >:

Public Types
typedef GridCell< PointT >	CellType
typedef std::unordered_map< Index3D, CellType, Index3D::HashFunction >	GridCellsType
typedef nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor< double, PCLPointCloudAdaptor< PointT > >, PCLPointCloudAdaptor< PointT >, 3, size_t >	KDTree

Public Member Functions
	PointCloudGrid (const GridConfig &config)
void	clear ()
void	setInputCloud (typename pcl::PointCloud< PointT >::Ptr input, const Eigen::Quaterniond &R_body2World)
GridCellsType &	getGridCells ()
std::pair< typename pcl::PointCloud< PointT >::Ptr, typename pcl::PointCloud< PointT >::Ptr >	segmentPoints ()
	Final segmentation step.
bool	checkIndex3DInGrid (const Index3D &index) const
void	setDistToGround (double z)

Private Member Functions
std::vector< Index3D >	generateIndices (const uint16_t &z_threshold)
	Generate neighbor offsets for region growing.
void	cleanUp ()
void	addPoint (const PointT &point)
void	getGroundCells ()
	Classify grid cells into ground or obstacle.
std::vector< Index3D >	getNeighbors (const GridCell< PointT > &cell, const TerrainType &type, const std::vector< Index3D > &neighbor_offsets)
double	computeSlope (const Eigen::Hyperplane< double, int(3)> &plane) const
	Compute slope angle between surface normal and global Z-axis.
double	computeSlope (const Eigen::Vector3d &normal)
bool	fitGroundPlane (GridCell< PointT > &cell, const double &inlier_threshold)
	Fit a planar model to cell points using PROSAC.
void	expandGrid (std::queue< Index3D > q)
	Region growing from robot cell using centroid KD-tree.
std::string	classifySparsityBoundingBox (const GridCell< PointT > &cell, typename pcl::PointCloud< PointT >::Ptr cloud)
bool	classifySparsityNormalDist (const GridCell< PointT > &cell)
bool	findNearestGroundNeighbor (const Index3D &cid, Index3D &out_gid) const
bool	pointIsGroundWrtCell (const PointT &p, const GridCell< PointT > &gcell) const

Private Attributes
std::vector< Index3D >	neighbor_offsets
GridCellsType	gridCells
GridConfig	grid_config
std::vector< Index3D >	ground_cells
std::vector< Index3D >	non_ground_cells
Eigen::Quaterniond	orientation
pcl::PointCloud< PointT >::Ptr	centroid_cloud
std::vector< Index3D >	centroid_indices
std::unordered_map< Index3D, size_t, Index3D::HashFunction >	index_to_centroid_idx
pcl::PointCloud< PointT >::Ptr	ground_points
pcl::PointCloud< PointT >::Ptr	non_ground_points
pcl::PointCloud< PointT >::Ptr	ground_inliers
pcl::PointCloud< PointT >::Ptr	non_ground_inliers
pcl::SACSegmentation< PointT >	seg

Detailed Description

template<typename PointT>
class ground_segmentation::PointCloudGrid< PointT >

Core grid-based ground segmentation algorithm.

Pipeline: 1) Voxelize point cloud into 3D grid 2) Compute PCA per cell 3) Classify primitives (line/plane/noise) 4) Fit plane model (RANSAC) 5) Apply slope threshold 6) Seed region at robot cell 7) Expand via centroid KD-tree

Outputs separated ground and non-ground clouds.

Template Parameters

PointT

PCL point type

Definition at line 79 of file ground_detection.hpp.

Member Typedef Documentation

CellType

template<typename PointT >

typedef GridCell<PointT> ground_segmentation::PointCloudGrid< PointT >::CellType

Definition at line 84 of file ground_detection.hpp.

GridCellsType

template<typename PointT >

typedef std::unordered_map<Index3D, CellType, Index3D::HashFunction> ground_segmentation::PointCloudGrid< PointT >::GridCellsType

Definition at line 85 of file ground_detection.hpp.

KDTree

template<typename PointT >

typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<double, PCLPointCloudAdaptor<PointT> >, PCLPointCloudAdaptor<PointT>, 3, size_t> ground_segmentation::PointCloudGrid< PointT >::KDTree

Definition at line 114 of file ground_detection.hpp.

Constructor & Destructor Documentation

PointCloudGrid()

template<typename PointT >

ground_segmentation::PointCloudGrid< PointT >::PointCloudGrid

(

const GridConfig &

config

)

Definition at line 221 of file ground_detection.hpp.

{
 
  centroid_cloud.reset(new pcl::PointCloud<PointT>());
  grid_config = config;
 
  if (grid_config.processing_phase == 2) {
    neighbor_offsets = generateIndices(1);
  } else {
    neighbor_offsets = generateIndices(0);
  }
 
  ground_points.reset(new pcl::PointCloud<PointT>());
  non_ground_points.reset(new pcl::PointCloud<PointT>());
  ground_inliers.reset(new pcl::PointCloud<PointT>());
  non_ground_inliers.reset(new pcl::PointCloud<PointT>());
 
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_PROSAC);
  seg.setMaxIterations(1000);
}

Member Function Documentation

addPoint()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::addPoint ( const PointT &  point )

private

Definition at line 354 of file ground_detection.hpp.

{
  double cell_x = point.x / grid_config.cellSizeX;
  double cell_y = point.y / grid_config.cellSizeY;
  double cell_z = point.z / grid_config.cellSizeZ;
 
  int x = static_cast<int>(std::floor(cell_x));
  int y = static_cast<int>(std::floor(cell_y));
  int z = static_cast<int>(std::floor(cell_z));
 
  CellType & cell = gridCells[{x, y, z}];
  // information is redundant:
  cell.x = x;
  cell.y = y;
  cell.z = z;
  cell.points->push_back(point);
}

References ground_segmentation::GridCell< PointT >::points, ground_segmentation::GridCell< PointT >::x, ground_segmentation::GridCell< PointT >::y, and ground_segmentation::GridCell< PointT >::z.

checkIndex3DInGrid()

template<typename PointT >

bool ground_segmentation::PointCloudGrid< PointT >::checkIndex3DInGrid

(

const Index3D &

index

)

const

Definition at line 763 of file ground_detection.hpp.

{
  if (auto search = gridCells.find(index); search != gridCells.end()) {
    return true;
  } else {
    return false;
  }
}

classifySparsityBoundingBox()

template<typename PointT >

std::string ground_segmentation::PointCloudGrid< PointT >::classifySparsityBoundingBox ( const GridCell< PointT > &  cell, typename pcl::PointCloud< PointT >::Ptr  cloud  )

private

Definition at line 393 of file ground_detection.hpp.

{
  if (cell.points->empty() || cloud->empty()) {return "Empty";}
 
  PointT min_pt, max_pt;
  pcl::getMinMax3D(*cell.points, min_pt, max_pt);
 
  double volume = (max_pt.x - min_pt.x) *
    (max_pt.y - min_pt.y) *
    (max_pt.z - min_pt.z);
 
  if (volume <= 0.0) {return "Degenerate";}
 
  double sparsity = volume / static_cast<double>(cloud->size());
 
  if (sparsity < 0.001) {
    return "Low sparsity";
  } else if (sparsity < 0.01) {
    return "Medium sparsity";
  } else {
    return "High sparsity";
  }
}

References ground_segmentation::GridCell< PointT >::points.

classifySparsityNormalDist()

template<typename PointT >

bool ground_segmentation::PointCloudGrid< PointT >::classifySparsityNormalDist ( const GridCell< PointT > &  cell )

private

Definition at line 420 of file ground_detection.hpp.

{
  if (cell.points->empty()) {return false;}
 
  // Assume cell.centroid and cell.normal are Eigen::Vector3d and cell.normal is normalized
  double sum_abs_proj = 0.0;
 
  for (const auto & pt : cell.points->points) {
    Eigen::Vector3d diff(pt.x - cell.centroid[0], pt.y - cell.centroid[1], pt.z - cell.centroid[2]);
    double proj = diff.dot(cell.normal);     // projection along normal
    sum_abs_proj += std::abs(proj);
  }
 
  double mean_abs_proj = sum_abs_proj / cell.points->size();
 
  // Threshold: tune for your use case (0.1 = flat/ground)
  return mean_abs_proj < 0.1;
}

References ground_segmentation::GridCell< PointT >::centroid, ground_segmentation::GridCell< PointT >::normal, and ground_segmentation::GridCell< PointT >::points.

cleanUp()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::cleanUp ( )

private

Definition at line 344 of file ground_detection.hpp.

{
  ground_cells.clear();
  non_ground_cells.clear();
  centroid_cloud->clear();
  centroid_indices.clear();
  index_to_centroid_idx.clear();
}

clear()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::clear

(

)

Definition at line 338 of file ground_detection.hpp.

{
  gridCells.clear();
}

computeSlope() [1/2]

template<typename PointT >

double ground_segmentation::PointCloudGrid< PointT >::computeSlope ( const Eigen::Hyperplane< double, int(3)> &  plane ) const

private

Compute slope angle between surface normal and global Z-axis.

Parameters

normal

Estimated plane normal

Returns

slope angle in radians

Uses body-to-world orientation to ensure gravity alignment.

Definition at line 383 of file ground_detection.hpp.

{
  const Eigen::Vector3d zNormal(Eigen::Vector3d::UnitZ());
  Eigen::Vector3d planeNormal = plane.normal();
  planeNormal = orientation * planeNormal;
  planeNormal.normalize();
  return acos(std::abs(planeNormal.dot(zNormal)));
}

computeSlope() [2/2]

template<typename PointT >

double ground_segmentation::PointCloudGrid< PointT >::computeSlope ( const Eigen::Vector3d &  normal )

private

Definition at line 373 of file ground_detection.hpp.

{
  const Eigen::Vector3d zNormal(Eigen::Vector3d::UnitZ());
  Eigen::Vector3d planeNormal = normal;
  planeNormal = orientation * planeNormal;
  planeNormal.normalize();
  return acos(std::abs(planeNormal.dot(zNormal)));
}

expandGrid()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::expandGrid ( std::queue< Index3D >  q )

private

Region growing from robot cell using centroid KD-tree.

Expands only to:

• Cells classified as GROUND
• Within centroidSearchRadius
• Within vertical threshold (Phase 2)

Ensures ground continuity and prevents floating ground artifacts.

Definition at line 666 of file ground_detection.hpp.

{
  // Wrap the PCL point cloud with nanoflann adaptor
  PCLPointCloudAdaptor<PointT> pclAdaptor(*centroid_cloud);
 
#if NANOFLANN_VERSION >= 0x150
  nanoflann::SearchParameters search_params;
#else
  nanoflann::SearchParams search_params;
#endif
 
  search_params.eps = 0.0;      // Larger tolerance for faster results
  search_params.sorted = false;   // No need to sort
 
  nanoflann::KDTreeSingleIndexAdaptorParams build_params(10);   // leaf size
  KDTree tree(3, pclAdaptor, build_params);
  tree.buildIndex();
 
  const double radius = grid_config.centroidSearchRadius * grid_config.centroidSearchRadius;   // nanoflann uses squared radius
 
  while (!q.empty()) {
    Index3D idx = q.front();
    q.pop();
 
    GridCell<PointT> & current_cell = gridCells[idx];
    current_cell.in_queue = false;     // Mark as not in queue now that we're processing it
 
    if (current_cell.expanded) {continue;}
    current_cell.expanded = true;
 
    // Find current centroid index
    size_t curr_centroid_idx = index_to_centroid_idx[idx];
    const PointT & curr_centroid = centroid_cloud->points.at(curr_centroid_idx);
 
    // Prepare radius search
#if NANOFLANN_VERSION >= 0x150
using Neighbor = nanoflann::ResultItem<size_t, double>;
#else
using Neighbor = std::pair<size_t, double>;
#endif
 
    std::vector<Neighbor> neighbors;
 
 
    double query_pt[3] = {static_cast<double>(curr_centroid.x),
      static_cast<double>(curr_centroid.y),
      static_cast<double>(curr_centroid.z)};
 
    tree.radiusSearch(query_pt, radius, neighbors, search_params);
 
    for (const auto & nb : neighbors) {
      size_t ni = nb.first;
      if (ni == curr_centroid_idx) {
        continue;                                  // skip self
 
      }
      Index3D neighbor_id = centroid_indices[ni];
      if (neighbor_id == idx) {
        continue;                             // redundant but safe
      }
 
      GridCell<PointT> & neighbor = gridCells[neighbor_id];
      if (neighbor.points->empty() || neighbor.expanded || neighbor.in_queue || neighbor.terrain_type != TerrainType::GROUND) {continue;}
 
      if (grid_config.processing_phase == 2) {
        // Reject neighbor if centroid height difference is too large
        // Height continuity constraint
        double dz =
            std::abs(curr_centroid.z - neighbor.centroid[2]);
 
        if (dz > grid_config.maxGroundHeightDeviation)
          continue;
      }
 
      if (neighbor.terrain_type == TerrainType::GROUND) {
        q.push(neighbor_id);
        neighbor.in_queue = true;
      }
    }
    ground_cells.emplace_back(idx);
  }
}

References ground_segmentation::GridCell< PointT >::centroid, ground_segmentation::GridCell< PointT >::expanded, ground_segmentation::GROUND, ground_segmentation::GridCell< PointT >::in_queue, ground_segmentation::GridCell< PointT >::points, and ground_segmentation::GridCell< PointT >::terrain_type.

findNearestGroundNeighbor()

template<typename PointT >

bool ground_segmentation::PointCloudGrid< PointT >::findNearestGroundNeighbor ( const Index3D &  cid, Index3D &  out_gid  ) const

private

Definition at line 267 of file ground_detection.hpp.

{
  bool found = false;
  double best_d2 = std::numeric_limits<double>::infinity();
 
  for (const auto & off : neighbor_offsets) {
 
    Index3D nid = cid + off;
 
    if (!checkIndex3DInGrid(nid)) {
      continue;
    }
 
    const auto & ncell = gridCells.at(nid);
 
    if (ncell.terrain_type != TerrainType::GROUND ||
        ncell.points->empty() ||
        !ncell.expanded)
    {
      continue;
    }
 
    double dx = double(nid.x - cid.x);
    double dy = double(nid.y - cid.y);
    double dz = double(nid.z - cid.z);
 
    double d2 = dx*dx + dy*dy + 0.25*dz*dz;
 
    if (d2 < best_d2) {
      best_d2 = d2;
      out_gid = nid;
      found = true;
    }
  }
 
  return found;
}

References ground_segmentation::GROUND, ground_segmentation::Index3D::x, ground_segmentation::Index3D::y, and ground_segmentation::Index3D::z.

fitGroundPlane()

template<typename PointT >

bool ground_segmentation::PointCloudGrid< PointT >::fitGroundPlane ( GridCell< PointT > &  cell, const double &  inlier_threshold  )

private

Fit a planar model to cell points using PROSAC.

Parameters

cell	Grid cell
threshold	RANSAC inlier threshold (meters)

Returns

true if plane successfully estimated

Sets:

• cell.inliers
• cell.slope

Definition at line 474 of file ground_detection.hpp.

{
 
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  seg.setInputCloud(cell.points);
  seg.setDistanceThreshold(threshold);   // Adjust this threshold based on your needs
  seg.segment(*inliers, *coefficients);
  cell.inliers = inliers;
  if (cell.inliers->indices.size() == 0) {
    return false;
  }
 
  Eigen::Vector3d plane_normal(coefficients->values[0], coefficients->values[1],
    coefficients->values[2]);
  double distToOrigin = coefficients->values[3];
  auto plane = Eigen::Hyperplane<double, 3>(plane_normal, distToOrigin);
  cell.slope = computeSlope(plane);
  return true;
}

References ground_segmentation::GridCell< PointT >::inliers, ground_segmentation::GridCell< PointT >::points, and ground_segmentation::GridCell< PointT >::slope.

generateIndices()

template<typename PointT >

std::vector< Index3D > ground_segmentation::PointCloudGrid< PointT >::generateIndices ( const uint16_t &  z_threshold )

private

Generate neighbor offsets for region growing.

Parameters

z_threshold

Defines vertical expansion range.

Phase 1 → z_threshold = 0 (horizontal only) Phase 2 → z_threshold = 1 (vertical connectivity allowed)

Definition at line 245 of file ground_detection.hpp.

{
  std::vector<Index3D> idxs;
 
  for (int dx = -1; dx <= 1; ++dx) {
    for (int dy = -1; dy <= 1; ++dy) {
      for (int dz = -1; dz <= z_threshold; ++dz) {
        if (dx == 0 && dy == 0 && dz == 0) {
          continue;
        }
        Index3D idx;
        idx.x = dx;
        idx.y = dy;
        idx.z = dz;
        idxs.push_back(idx);
      }
    }
  }
  return idxs;
}

References ground_segmentation::Index3D::x, ground_segmentation::Index3D::y, and ground_segmentation::Index3D::z.

getGridCells()

template<typename PointT >

GridCellsType & ground_segmentation::PointCloudGrid< PointT >::getGridCells ( )

inline

Definition at line 92 of file ground_detection.hpp.

{return gridCells;}

References ground_segmentation::PointCloudGrid< PointT >::gridCells.

getGroundCells()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::getGroundCells ( )

private

Classify grid cells into ground or obstacle.

Steps:

• Reject sparse cells
• PCA eigenvalue analysis
• Primitive classification
• Plane fitting
• Slope thresholding
• Robot-seeded region growing

Definition at line 496 of file ground_detection.hpp.

{
 
  if (gridCells.empty()) {
    return;
  }
 
  this->cleanUp();
 
  centroid_cloud->points.reserve(gridCells.size());
  centroid_indices.reserve(gridCells.size());
  ground_cells.reserve(gridCells.size());
  non_ground_cells.reserve(gridCells.size());
 
  for (auto & cellPair : gridCells) {
    GridCell<PointT> & cell = cellPair.second;
 
    //Too few points
    if ((cell.points->size() < 3)) {continue;}
 
    Index3D cell_id = cellPair.first;
    pcl::compute3DCentroid(*(cell.points), cell.centroid);
 
    if (cell.points->size() <= 5) {
      Eigen::Vector4f squared_diff_sum(0, 0, 0, 0);
 
      for (typename pcl::PointCloud<PointT>::iterator it = cell.points->begin();
        it != cell.points->end(); ++it)
      {
        Eigen::Vector4f diff = (*it).getVector4fMap() - cell.centroid.template cast<float>();
        squared_diff_sum += diff.array().square().matrix();
      }
 
      Eigen::Vector4f variance = squared_diff_sum / cell.points->size();
 
      if (variance[0] < variance[2] && variance[1] < variance[2]) {
        cell.terrain_type = TerrainType::OBSTACLE;
        non_ground_cells.push_back(cell_id);
      } else {
        cell.terrain_type = TerrainType::GROUND;
        PointT centroid3d;
 
        centroid3d.x = cell.centroid[0];
        centroid3d.y = cell.centroid[1];
        centroid3d.z = cell.centroid[2];
 
        centroid_cloud->points.push_back(centroid3d);
        centroid_indices.push_back(cell_id);
        index_to_centroid_idx[cell_id] = centroid_cloud->size() - 1;
      }
      continue;
    }
 
    Eigen::Matrix3d covariance_matrix;
    pcl::computeCovarianceMatrixNormalized(*cell.points, cell.centroid, covariance_matrix);
 
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(covariance_matrix,
      Eigen::ComputeEigenvectors);
    cell.eigenvectors = eigen_solver.eigenvectors();
    cell.eigenvalues = eigen_solver.eigenvalues();
 
    Eigen::Vector3d normal = cell.eigenvectors.col(0);
 
    // Ensure all normals point upward
    if (normal(2) < 0) {
      normal *= -1;       // flip the normal direction
    }
 
    normal.normalize();
    cell.normal = normal;
 
    double ratio = cell.eigenvalues[2] / cell.eigenvalues.sum();
    if (ratio > 0.950) {
      cell.primitive_type = PrimitiveType::LINE;
 
      Eigen::Vector3d v = cell.eigenvectors.col(2);
      if (v(2) < 0) {
        v *= -1;         // flip the normal direction
      }
      v = orientation * v;
      v.normalize();
 
      double angle_rad = acos(std::abs(v.dot(Eigen::Vector3d::UnitZ())));
 
      if (angle_rad > ((90 - grid_config.slopeThresholdDegrees) * (M_PI / 180))) {
        cell.terrain_type = TerrainType::GROUND;
        PointT centroid3d;
 
        centroid3d.x = cell.centroid[0];
        centroid3d.y = cell.centroid[1];
        centroid3d.z = cell.centroid[2];
 
        centroid_cloud->points.push_back(centroid3d);
        centroid_indices.push_back(cell_id);
        index_to_centroid_idx[cell_id] = centroid_cloud->size() - 1;
      } else {
        cell.terrain_type = TerrainType::OBSTACLE;
        non_ground_cells.push_back(cell_id);
      }
      continue;
    } else if (ratio > 0.4) {
      cell.primitive_type = PrimitiveType::PLANE;
      if (std::abs(computeSlope(cell.normal)) > (grid_config.slopeThresholdDegrees * (M_PI / 180.0))) {
        cell.terrain_type = TerrainType::OBSTACLE;
        non_ground_cells.push_back(cell_id);
        continue;
      }
    } else {
      cell.terrain_type = TerrainType::OBSTACLE;
      cell.primitive_type = PrimitiveType::NOISE;
      non_ground_cells.push_back(cell_id);
      continue;
    }
 
    if (!fitGroundPlane(cell, grid_config.groundInlierThreshold)) {
      cell.terrain_type = TerrainType::OBSTACLE;
      non_ground_cells.push_back(cell_id);
      continue;
    }
 
    if (cell.slope < (grid_config.slopeThresholdDegrees * (M_PI / 180)) ) {
      cell.terrain_type = TerrainType::GROUND;
      PointT centroid3d;
 
      centroid3d.x = cell.centroid[0];
      centroid3d.y = cell.centroid[1];
      centroid3d.z = cell.centroid[2];
 
      centroid_cloud->points.push_back(centroid3d);
      centroid_indices.push_back(cell_id);
      index_to_centroid_idx[cell_id] = centroid_cloud->size() - 1;
    } else {
      cell.terrain_type = TerrainType::OBSTACLE;
      non_ground_cells.push_back(cell_id);
    }
  }
 
  std::queue<Index3D> q;
 
  Index3D best_robot_cell;
 
  // Compute z index directly from distToGround
  int z = static_cast<int>(std::floor(grid_config.distToGround / grid_config.cellSizeZ));
  best_robot_cell = Index3D{0, 0, z};
 
  // Ensure the cell exists (even if empty)
  GridCell<PointT> & robot_cell = gridCells[best_robot_cell];
 
  // Mark semantics
  robot_cell.terrain_type = TerrainType::GROUND;
 
  // IMPORTANT: allow expansion even without points
  robot_cell.expanded = false;
  robot_cell.in_queue = true;
 
  // Add centroid ONLY for KD-tree seeding
  PointT centroid3d;
  centroid3d.x = 0.0;
  centroid3d.y = 0.0;
  centroid3d.z = grid_config.distToGround;
 
  centroid_cloud->points.push_back(centroid3d);
  centroid_indices.push_back(best_robot_cell);
  index_to_centroid_idx[best_robot_cell] = centroid_cloud->size() - 1;
 
  q.push(best_robot_cell);
  expandGrid(q);
}

References ground_segmentation::GridCell< PointT >::centroid, ground_segmentation::GridCell< PointT >::eigenvalues, ground_segmentation::GridCell< PointT >::eigenvectors, ground_segmentation::GridCell< PointT >::expanded, ground_segmentation::GROUND, ground_segmentation::GridCell< PointT >::in_queue, ground_segmentation::LINE, ground_segmentation::NOISE, ground_segmentation::GridCell< PointT >::normal, ground_segmentation::OBSTACLE, ground_segmentation::PLANE, ground_segmentation::GridCell< PointT >::points, ground_segmentation::GridCell< PointT >::primitive_type, ground_segmentation::GridCell< PointT >::slope, and ground_segmentation::GridCell< PointT >::terrain_type.

getNeighbors()

template<typename PointT >

std::vector< Index3D > ground_segmentation::PointCloudGrid< PointT >::getNeighbors ( const GridCell< PointT > &  cell, const TerrainType &  type, const std::vector< Index3D > &  neighbor_offsets  )

private

Definition at line 440 of file ground_detection.hpp.

{
 
  std::vector<Index3D> neighbors;
 
  Index3D cell_id;
  cell_id.x = cell.x;
  cell_id.y = cell.y;
  cell_id.z = cell.z;
 
  for (uint i = 0; i < idx.size(); ++i) {
    Index3D neighbor_id = cell_id + idx[i];
 
    if (!checkIndex3DInGrid(neighbor_id)) {
      continue;
    }
 
    const GridCell<PointT> & neighbor = gridCells[neighbor_id];
 
    if (neighbor.points->size() > 0 && neighbor.terrain_type == type) {
      Index3D id;
      id.x = neighbor.x;
      id.y = neighbor.y;
      id.z = neighbor.z;
      neighbors.push_back(id);
    }
  }
  return neighbors;
}

References ground_segmentation::GridCell< PointT >::points, ground_segmentation::GridCell< PointT >::terrain_type, ground_segmentation::GridCell< PointT >::x, ground_segmentation::Index3D::x, ground_segmentation::GridCell< PointT >::y, ground_segmentation::Index3D::y, ground_segmentation::GridCell< PointT >::z, and ground_segmentation::Index3D::z.

pointIsGroundWrtCell()

template<typename PointT >

bool ground_segmentation::PointCloudGrid< PointT >::pointIsGroundWrtCell ( const PointT &  p, const GridCell< PointT > &  gcell  ) const

private

Definition at line 308 of file ground_detection.hpp.

{
  Eigen::Vector3d n = gcell.normal;
 
  if (!std::isfinite(n.x()) || !std::isfinite(n.y()) ||
      !std::isfinite(n.z()) || n.norm() < 1e-6)
  {
    return false;
  }
 
  n.normalize();
 
  Eigen::Vector3d c(
      double(gcell.centroid[0]),
      double(gcell.centroid[1]),
      double(gcell.centroid[2]));
 
  Eigen::Vector3d x(double(p.x), double(p.y), double(p.z));
 
  // Distance to plane
  double plane_dist = std::abs(n.dot(x - c));
  if (plane_dist > grid_config.groundInlierThreshold)
      return false;
      
  return true;
}

References ground_segmentation::GridCell< PointT >::centroid, and ground_segmentation::GridCell< PointT >::normal.

segmentPoints()

template<typename PointT >

std::pair< typename pcl::PointCloud< PointT >::Ptr, typename pcl::PointCloud< PointT >::Ptr > ground_segmentation::PointCloudGrid< PointT >::segmentPoints

(

)

Final segmentation step.

Splits:

• Ground points
• Non-ground points

Applies:

• Inlier extraction
• Sparsity checks
• Floating cell rejection

Returns

pair<ground_cloud, non_ground_cloud>

Definition at line 774 of file ground_detection.hpp.

{
  ground_points->clear();
  non_ground_points->clear();
  ground_inliers->clear();
  non_ground_inliers->clear();
 
  pcl::ExtractIndices<PointT> extract_ground;
 
  getGroundCells();
  for (auto & cell_id : ground_cells) {
 
    GridCell<PointT> & cell = gridCells[cell_id];
 
    if ((cell.points->size() <= 5 || cell.primitive_type == PrimitiveType::LINE) &&
      cell.terrain_type == TerrainType::GROUND)
    {
      *ground_points += *cell.points;
      continue;
    }
 
    extract_ground.setInputCloud(cell.points);
    extract_ground.setIndices(cell.inliers);
 
    extract_ground.setNegative(false);
    extract_ground.filter(*ground_inliers);
 
    extract_ground.setNegative(true);
    extract_ground.filter(*non_ground_inliers);
 
    if (ground_inliers->size() == 0) {
      continue;
    }
 
    auto score1 = classifySparsityBoundingBox(cell, ground_inliers);
    auto score2 = classifySparsityBoundingBox(cell, non_ground_inliers);
 
    if (score1 == score2) {
      Eigen::Vector4d centroid;
      pcl::compute3DCentroid(*(ground_inliers), centroid);
 
      // For each candidate ground cell:
      double cell_z = centroid[2];
      std::vector<double> neighbor_zs;
      for (const auto & offset : neighbor_offsets) {
        Index3D nidx = cell_id + offset;
        if (!checkIndex3DInGrid(nidx)) {continue;}
        const auto & ncell = gridCells[nidx];
        if (ncell.terrain_type == TerrainType::GROUND) {
          neighbor_zs.push_back(ncell.centroid[2]);
        }
      }
 
      if (neighbor_zs.empty()) {
        *non_ground_points += *cell.points;
        continue;
      }
 
      double local_ref = *std::min_element(neighbor_zs.begin(), neighbor_zs.end());
 
      if (std::abs(cell_z - local_ref) > grid_config.maxGroundHeightDeviation) {
        *non_ground_points += *cell.points;
        continue;
      }
 
      bool reject_as_floating = false;
      int bz = cell_id.z - 1;
      while (true) {
        Index3D below(cell_id.x, cell_id.y, bz);
        if (!checkIndex3DInGrid(below)) {
          break;                                        // Out of bound: stop looping
        }
        const auto & bcell = gridCells[below];
        if (!bcell.points->empty() && bcell.terrain_type != TerrainType::GROUND) {
          // Found an occupied non-ground cell below—reject as floating!
          reject_as_floating = true;
          break;
        }
        --bz;
      }
      if (reject_as_floating) {
        *non_ground_points += *cell.points;
        continue;
      }
    }
    *ground_points += *ground_inliers;
    *non_ground_points += *non_ground_inliers;
  }
 
  if (grid_config.processing_phase == 1)
  {
    for (const auto & cell_id : non_ground_cells)
    {
      const GridCell<PointT> & cell = gridCells[cell_id];
 
      Index3D nearest_ground_id;
 
      if (!findNearestGroundNeighbor(cell_id, nearest_ground_id)) {
          *non_ground_points += *cell.points;
        continue;
      }
 
      const auto & gcell = gridCells[nearest_ground_id];
 
      for (const auto & pt : cell.points->points)
      {
          if (pointIsGroundWrtCell(pt, gcell))
              ground_points->push_back(pt);
          else
              non_ground_points->push_back(pt);
      }
    }
  }
  else
  {
    for (const auto & cell_id : non_ground_cells) {
        *non_ground_points += *gridCells[cell_id].points;
    }
  }
  return std::make_pair(ground_points, non_ground_points);
}

References ground_segmentation::GROUND, ground_segmentation::GridCell< PointT >::inliers, ground_segmentation::LINE, ground_segmentation::GridCell< PointT >::points, ground_segmentation::GridCell< PointT >::primitive_type, and ground_segmentation::GridCell< PointT >::terrain_type.

setDistToGround()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::setDistToGround ( double  z )

inline

Definition at line 117 of file ground_detection.hpp.

  {
    grid_config.distToGround = z;
  }

References ground_segmentation::GridConfig::distToGround, and ground_segmentation::PointCloudGrid< PointT >::grid_config.

setInputCloud()

template<typename PointT >

void ground_segmentation::PointCloudGrid< PointT >::setInputCloud	(	typename pcl::PointCloud< PointT >::Ptr	input,
		const Eigen::Quaterniond &	R_body2World
	)

Definition at line 750 of file ground_detection.hpp.

{
 
  this->clear();
  orientation = R_body2World;
  for (typename pcl::PointCloud<PointT>::iterator it = input->begin(); it != input->end(); ++it) {
    this->addPoint(*it);
  }
}

Member Data Documentation

centroid_cloud

template<typename PointT >

pcl::PointCloud<PointT>::Ptr ground_segmentation::PointCloudGrid< PointT >::centroid_cloud

private

Definition at line 209 of file ground_detection.hpp.

centroid_indices

template<typename PointT >

std::vector<Index3D> ground_segmentation::PointCloudGrid< PointT >::centroid_indices

private

Definition at line 210 of file ground_detection.hpp.

grid_config

template<typename PointT >

GridConfig ground_segmentation::PointCloudGrid< PointT >::grid_config

private

Definition at line 203 of file ground_detection.hpp.

Referenced by ground_segmentation::PointCloudGrid< PointT >::setDistToGround().

gridCells

template<typename PointT >

GridCellsType ground_segmentation::PointCloudGrid< PointT >::gridCells

private

Definition at line 202 of file ground_detection.hpp.

Referenced by ground_segmentation::PointCloudGrid< PointT >::getGridCells().

ground_cells

template<typename PointT >

std::vector<Index3D> ground_segmentation::PointCloudGrid< PointT >::ground_cells

private

Definition at line 204 of file ground_detection.hpp.

ground_inliers

template<typename PointT >

pcl::PointCloud<PointT>::Ptr ground_segmentation::PointCloudGrid< PointT >::ground_inliers

private

Definition at line 215 of file ground_detection.hpp.

ground_points

template<typename PointT >

pcl::PointCloud<PointT>::Ptr ground_segmentation::PointCloudGrid< PointT >::ground_points

private

Definition at line 213 of file ground_detection.hpp.

index_to_centroid_idx

template<typename PointT >

std::unordered_map<Index3D, size_t, Index3D::HashFunction> ground_segmentation::PointCloudGrid< PointT >::index_to_centroid_idx

private

Definition at line 211 of file ground_detection.hpp.

neighbor_offsets

template<typename PointT >

std::vector<Index3D> ground_segmentation::PointCloudGrid< PointT >::neighbor_offsets

private

Definition at line 200 of file ground_detection.hpp.

non_ground_cells

template<typename PointT >

std::vector<Index3D> ground_segmentation::PointCloudGrid< PointT >::non_ground_cells

private

Definition at line 205 of file ground_detection.hpp.

non_ground_inliers

template<typename PointT >

pcl::PointCloud<PointT>::Ptr ground_segmentation::PointCloudGrid< PointT >::non_ground_inliers

private

Definition at line 216 of file ground_detection.hpp.

non_ground_points

template<typename PointT >

pcl::PointCloud<PointT>::Ptr ground_segmentation::PointCloudGrid< PointT >::non_ground_points

private

Definition at line 214 of file ground_detection.hpp.

orientation

template<typename PointT >

Eigen::Quaterniond ground_segmentation::PointCloudGrid< PointT >::orientation

private

Definition at line 206 of file ground_detection.hpp.

seg

template<typename PointT >

pcl::SACSegmentation<PointT> ground_segmentation::PointCloudGrid< PointT >::seg

private

Definition at line 218 of file ground_detection.hpp.

---

The documentation for this class was generated from the following file:

• /home/runner/work/ground_segmentation/ground_segmentation/include/ground_detection.hpp