apollo::perception::algorithm::PlaneFitGroundDetector类 参考

#include <i_ground.h>

类 apollo::perception::algorithm::PlaneFitGroundDetector 继承关系图:

apollo::perception::algorithm::PlaneFitGroundDetector 的协作图:

Public 成员函数
	PlaneFitGroundDetector (PlaneFitGroundDetectorParam &param)
	~PlaneFitGroundDetector ()
bool	Init ()
bool	Detect (const float *point_cloud, const int *semantic_value, float *height_above_ground, unsigned int nr_points, unsigned int nr_point_elements)
void	ResetParams (float ori_z_lower, float ori_z_upper)
void	UpdateParams (float parsing_ground_z, float buffer, double timestamp)
const char *	GetLabel () const
const VoxelGridXY< float > *	GetGrid () const
const GroundPlaneLiDAR *	GetGroundPlane (int r, int c) const
unsigned int	GetGridDimX () const
unsigned int	GetGridDimY () const
float	GetUnknownHeight ()
void	Pc2Voxel (float x, float y, float z, int *row, int *col)
PlaneFitPointCandIndices **	GetCandis () const
Public 成员函数 继承自 apollo::perception::algorithm::BaseGroundDetector
	BaseGroundDetector (PlaneFitGroundDetectorParam &param)
virtual	~BaseGroundDetector ()

Protected 成员函数
void	CleanUp ()
void	InitOrderTable (const VoxelGridXY< float > *vg, std::pair< int, int > *order)
int	Fit ()
int	FitLine (unsigned int r)
int	FitGrid (const float *point_cloud, PlaneFitPointCandIndices *candi, GroundPlaneLiDAR *groundplane, unsigned int nr_points, unsigned int nr_point_element, float dist_thre)
int	FitInOrder ()
int	FilterCandidates (int r, int c, const float *point_cloud, PlaneFitPointCandIndices *candi, std::vector< std::pair< int, int > > *neighbors, unsigned int nr_point_element)
int	FitGridWithNeighbors (int r, int c, const float *point_cloud, GroundPlaneLiDAR *groundplane, unsigned int nr_points, unsigned int nr_point_element, float dist_thre)
void	GetNeighbors (int r, int c, int rows, int cols, std::vector< std::pair< int, int > > *neighbors)
float	CalculateAngleDist (const GroundPlaneLiDAR &plane, const std::vector< std::pair< int, int > > &neighbors)
void	SelectGroundCandidate (const float *point_cloud, const int *semantic_list, const std::vector< int > &indices, PlaneFitPointCandIndices *candi, GroundCandidateIndices *ground_candi, unsigned int nr_point_element)
int	NeighborGroundCandidate (int row, int col, const float *point_cloud, const int *semantic_list, const std::vector< int > &indices, PlaneFitPointCandIndices *candi, unsigned int nr_point_element)
void	SelectCandidates ()
int	Filter ()
int	FilterLine (unsigned int r)
int	FilterGrid (const Voxel< float > &vg, const float *point_cloud, PlaneFitPointCandIndices *candi, unsigned int nr_points, unsigned int nr_point_element)
int	Smooth ()
int	SmoothInOrder ()
void	GetInliers (int *inliers)
void	Optimization ()
void	PlaneRefine ()
void	SetGroundPlaneSpherParams (float *params)
void	GetGroundPlaneSpherParams (float *params)
void	SmoothInMath ()
int	SmoothLine (unsigned int up, unsigned int r, unsigned int dn)
int	CompleteGrid (const GroundPlaneSpherical &lt, const GroundPlaneSpherical &rt, const GroundPlaneSpherical &up, const GroundPlaneSpherical &dn, GroundPlaneSpherical *gp)
int	SmoothGrid (const GroundPlaneSpherical &g, const GroundPlaneSpherical &lt, const GroundPlaneSpherical &rt, const GroundPlaneSpherical &up, const GroundPlaneSpherical &dn, GroundPlaneSpherical *gp)
int	CompareZ (const float *point_cloud, const std::vector< int > &point_indices, const float *z_values, PlaneFitPointCandIndices *candi, unsigned int nr_points, unsigned int nr_point_element, unsigned int nr_compares)
void	ComputeAdaptiveThreshold ()
void	ComputeSignedGroundHeight (const float *point_cloud, float *height_above_ground, unsigned int nr_points, unsigned int nr_point_elements)
void	ComputeSignedGroundHeightLine (const float *point_cloud, const GroundPlaneLiDAR *up, const GroundPlaneLiDAR *cn, const GroundPlaneLiDAR *dn, float *height_above_ground, unsigned int r, unsigned int nr_points, unsigned int nr_point_elements)

Protected 属性
VoxelGridXY< float > *	vg_fine_
VoxelGridXY< float > *	vg_coarse_
GroundPlaneLiDAR **	ground_planes_
GroundPlaneSpherical **	ground_planes_sphe_
PlaneFitPointCandIndices **	local_candis_
GroundCandidateIndices **	ground_candidates_
std::pair< float, bool > **	ground_z_
float **	pf_thresholds_
unsigned int *	map_fine_to_coarse_
char *	labels_
float *	sampled_z_values_
float *	pf_threeds_
float *	spher_params_
int *	sampled_indices_
int	age_ = 0
std::pair< int, int > *	order_table_
double	frame_timestamp_ = 0.0
float **	weight_buffer_ = nullptr
int *	inliers_ = nullptr
float *	sphe_params_ = nullptr
std::vector< std::vector< unsigned int > >	neighbors_
Eigen::SparseMatrix< float >	a_array_
Eigen::SparseMatrix< float >	b_array_
Eigen::SimplicialLDLT< Eigen::SparseMatrix< float > >	solver_
Protected 属性 继承自 apollo::perception::algorithm::BaseGroundDetector
PlaneFitGroundDetectorParam &	param_

详细描述

在文件 i_ground.h 第 339 行定义.

构造及析构函数说明

PlaneFitGroundDetector()

apollo::perception::algorithm::PlaneFitGroundDetector::PlaneFitGroundDetector ( PlaneFitGroundDetectorParam &  param )

explicit

在文件 i_ground.cc 第 106 行定义.

                                      : BaseGroundDetector(param) {
  assert(Init());
}

~PlaneFitGroundDetector()

apollo::perception::algorithm::PlaneFitGroundDetector::~PlaneFitGroundDetector

(

)

在文件 i_ground.cc 第 111 行定义.

                                                { 
    CleanUp();
    if (weight_buffer_ != nullptr) {
      IFree2<float>(&weight_buffer_);
    }
    if (inliers_ != nullptr) {
      IFreeAligned<int>(&inliers_);
    }
    if (sphe_params_ != nullptr) {
      IFreeAligned<float>(&sphe_params_);
    }
}

成员函数说明

CalculateAngleDist()

float apollo::perception::algorithm::PlaneFitGroundDetector::CalculateAngleDist ( const GroundPlaneLiDAR &  plane, const std::vector< std::pair< int, int > > &  neighbors  )

protected

在文件 i_ground.cc 第 1367 行定义.

                                                   {
  float angle_dist = 0.0f;
  int count = 0;
  unsigned int j = 0;
  int r_n = 0;
  int c_n = 0;
  for (j = 0; j < neighbors.size(); ++j) {
    r_n = neighbors[j].first;
    c_n = neighbors[j].second;
    if (ground_planes_[r_n][c_n].IsValid()) {
      angle_dist += calculate_two_angles(ground_planes_[r_n][c_n], plane);
      count++;
    }
  }
  if (count == 0) {
    return -1;
  }
  return angle_dist / static_cast<float>(count);
}

CleanUp()

void apollo::perception::algorithm::PlaneFitGroundDetector::CleanUp ( )

protected

在文件 i_ground.cc 第 344 行定义.

                                     {
  if (vg_fine_) {
    delete vg_fine_;
  }
  if (vg_coarse_) {
    delete vg_coarse_;
  }
  IFree2<GroundPlaneLiDAR>(&ground_planes_);
  IFree2<GroundPlaneSpherical>(&ground_planes_sphe_);
  IFree2<std::pair<float, bool>>(&ground_z_);
  IFree2<PlaneFitPointCandIndices>(&local_candis_);
  IFree2<GroundCandidateIndices>(&ground_candidates_);
  IFreeAligned<float>(&pf_threeds_);
  IFreeAligned<char>(&labels_);
  IFreeAligned<unsigned int>(&map_fine_to_coarse_);
  IFreeAligned<float>(&sampled_z_values_);
  IFreeAligned<int>(&sampled_indices_);
  IFree2<float>(&pf_thresholds_);
  IFree<std::pair<int, int>>(&order_table_);
}

CompareZ()

int apollo::perception::algorithm::PlaneFitGroundDetector::CompareZ ( const float *  point_cloud, const std::vector< int > &  point_indices, const float *  z_values, PlaneFitPointCandIndices *  candi, unsigned int  nr_points, unsigned int  nr_point_element, unsigned int  nr_compares  )

protected

在文件 i_ground.cc 第 365 行定义.

                                                               {
  int pos = 0;
  int nr_candis = 0;
  unsigned int i = 0;
  unsigned int nr_contradi = 0;
  unsigned int nr_z_comp_fail_threshold =
      IMin(param_.nr_z_comp_fail_threshold, (unsigned int)(nr_compares >> 1));
  const float *ptr = nullptr;
  float z = 0.0f;
  float delta_z = 0.0f;
  std::vector<int>::const_iterator iter = indices.cbegin();
  while (iter < indices.cend()) {
    nr_contradi = 0;
    pos = *iter++;
    assert(pos < static_cast<int>(nr_points));
    nr_contradi = 0;
    //  requires the Z element to be in the third position, i.e., after X, Y
    ptr = point_cloud + (pos * nr_point_element);
    z = ptr[2];
    // for near range check valid height
    if (IAbs(ptr[0]) < param_.roi_near_rad &&
        IAbs(ptr[1]) < param_.roi_near_rad) {
      if (z < param_.sample_region_z_lower ||
          z > param_.sample_region_z_upper) {
        continue;
      }
    }
    if (nr_compares > nr_z_comp_fail_threshold) {
      for (i = 0; i < nr_compares; ++i) {
        delta_z = IAbs(z_values[i] - z);
        if (delta_z > param_.planefit_filter_threshold) {
          nr_contradi++;
          if (nr_contradi > nr_z_comp_fail_threshold) {
            break;
          }
        }
      }
    }
    if (nr_contradi <= nr_z_comp_fail_threshold) {
      labels_[pos] = 1;
      candi->PushIndex(pos);
      nr_candis++;
    }
  }
  return nr_candis;
}

CompleteGrid()

int apollo::perception::algorithm::PlaneFitGroundDetector::CompleteGrid ( const GroundPlaneSpherical &  lt, const GroundPlaneSpherical &  rt, const GroundPlaneSpherical &  up, const GroundPlaneSpherical &  dn, GroundPlaneSpherical *  gp  )

protected

在文件 i_ground.cc 第 1551 行定义.

                                                                   {
  int supports[] = {0, 0, 0, 0};
  float weights[] = {0.f, 0.f, 0.f, 0.f};
  gp->ForceInvalid();
  supports[0] = lt.GetNrSupport();
  supports[1] = rt.GetNrSupport();
  supports[2] = up.GetNrSupport();
  supports[3] = dn.GetNrSupport();
  int support_sum = 0;
  support_sum = ISum4(supports);
  if (!support_sum) {
    return 0;
  }
  weights[0] =
      static_cast<float>(supports[0]) / static_cast<float>(support_sum);
  weights[1] =
      static_cast<float>(supports[1]) / static_cast<float>(support_sum);
  weights[2] =
      static_cast<float>(supports[2]) / static_cast<float>(support_sum);
  weights[3] =
      static_cast<float>(supports[3]) / static_cast<float>(support_sum);
  // weighted average:
  gp->theta = weights[0] * lt.theta + weights[1] * rt.theta +
              weights[2] * up.theta + weights[3] * dn.theta;
  gp->phi = weights[0] * lt.phi + weights[1] * rt.phi + weights[2] * up.phi +
            weights[3] * dn.phi;
  gp->d = weights[0] * lt.d + weights[1] * rt.d + weights[2] * up.d +
          weights[3] * dn.d;
  // compute average - diveided by 4, round to nearest int
  support_sum = IMax(((support_sum + 2) >> 2), 1);
  gp->SetNrSupport(support_sum);
  return 1;
}

ComputeAdaptiveThreshold()

void apollo::perception::algorithm::PlaneFitGroundDetector::ComputeAdaptiveThreshold ( )

protected

在文件 i_ground.cc 第 418 行定义.

                                                      {
  unsigned int r = 0;
  unsigned int c = 0;
  float dr = 0.0f;
  float dc = 0.0f;
  float k = 0.0f;
  float b = 0.0f;
  float min_dist = 0.0f;
  float max_dist = 0.0f;
  float thre = 0.0f;
  float grid_rad = static_cast<float>(param_.nr_grids_coarse - 1) / 2;
  assert(pf_thresholds_ != nullptr);
  for (r = 0; r < param_.nr_grids_coarse; ++r) {
    dr = static_cast<float>(r) - grid_rad;
    dr *= dr;
    for (c = 0; c < param_.nr_grids_coarse; ++c) {
      dc = static_cast<float>(c) - grid_rad;
      dc *= dc;
      // store to center distance:
      pf_thresholds_[r][c] = ISqrt(dr + dc);
    }
  }
  IMinMaxElements(pf_thresholds_[0],
                  param_.nr_grids_coarse * param_.nr_grids_coarse, &min_dist,
                  &max_dist);
  if (max_dist - min_dist < Constant<float>::EPSILON()) {
    thre = (param_.planefit_dist_threshold_near +
            param_.planefit_dist_threshold_far) /
           2;
    for (r = 0; r < param_.nr_grids_coarse; ++r) {
      for (c = 0; c < param_.nr_grids_coarse; ++c) {
        pf_thresholds_[r][c] = thre;
      }
    }
  } else {
    k = param_.planefit_dist_threshold_far -
        param_.planefit_dist_threshold_near;
    k = k / (max_dist - min_dist);
    b = param_.planefit_dist_threshold_far +
        param_.planefit_dist_threshold_near;
    b = (b - k * (min_dist + max_dist)) / 2;
    for (r = 0; r < param_.nr_grids_coarse; ++r) {
      for (c = 0; c < param_.nr_grids_coarse; ++c) {
        thre = k * pf_thresholds_[r][c] + b;
        pf_thresholds_[r][c] = thre;
      }
    }
  }
}

ComputeSignedGroundHeight()

void apollo::perception::algorithm::PlaneFitGroundDetector::ComputeSignedGroundHeight ( const float *  point_cloud, float *  height_above_ground, unsigned int  nr_points, unsigned int  nr_point_elements  )

protected

在文件 i_ground.cc 第 468 行定义.

                                                            {
  unsigned int r = 0;
  unsigned int nm1 = param_.nr_grids_coarse - 1;
  for (r = 0; r < nr_points; ++r) {
    height_above_ground[r] = std::numeric_limits<float>::max();
  }
  ComputeSignedGroundHeightLine(
      point_cloud, ground_planes_[0], ground_planes_[0], ground_planes_[1],
      height_above_ground, 0, nr_points, nr_point_elements);
  for (r = 1; r < nm1; ++r) {
    ComputeSignedGroundHeightLine(point_cloud, ground_planes_[r - 1],
                                  ground_planes_[r], ground_planes_[r + 1],
                                  height_above_ground, r, nr_points,
                                  nr_point_elements);
  }
  ComputeSignedGroundHeightLine(point_cloud, ground_planes_[nm1 - 1],
                                ground_planes_[nm1], ground_planes_[nm1],
                                height_above_ground, nm1, nr_points,
                                nr_point_elements);
}

ComputeSignedGroundHeightLine()

void apollo::perception::algorithm::PlaneFitGroundDetector::ComputeSignedGroundHeightLine ( const float *  point_cloud, const GroundPlaneLiDAR *  up, const GroundPlaneLiDAR *  cn, const GroundPlaneLiDAR *  dn, float *  height_above_ground, unsigned int  r, unsigned int  nr_points, unsigned int  nr_point_elements  )

protected

在文件 i_ground.cc 第 491 行定义.

                                    {
  unsigned int i = 0;
  unsigned int c = 0;
  unsigned int id = 0;
  int pos = 0;
  char label = 0;
  const float *cptr = nullptr;
  float dist[] = {0, 0, 0, 0, 0};
  const float *plane[] = {nullptr, nullptr, nullptr, nullptr, nullptr};
  float min_abs_dist = 0.0f;
  unsigned int nm1 = param_.nr_grids_coarse - 1;
  assert(param_.nr_grids_coarse >= 2);
  plane[0] = cn[0].IsValid() ? cn[0].params : nullptr;
  plane[1] = cn[1].IsValid() ? cn[1].params : nullptr;
  plane[2] = up[0].IsValid() ? up[0].params : nullptr;
  plane[3] = dn[0].IsValid() ? dn[0].params : nullptr;
  std::vector<int>::const_iterator iter = (*vg_coarse_)(r, 0).indices_.cbegin();
  while (iter < (*vg_coarse_)(r, 0).indices_.cend()) {
    pos = *iter;
    assert(pos < static_cast<int>(nr_points));
    label = labels_[pos];
    cptr = point_cloud + (nr_point_elements * pos);
    dist[0] = plane[0] != nullptr
                  ? IPlaneToPointSignedDistanceWUnitNorm(plane[0], cptr)
                  : std::numeric_limits<float>::max();
    min_abs_dist = IAbs(dist[0]);
    id = 0;
    // for candidates we take min dist:
    if (label) {
      dist[1] = plane[1] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[1], cptr)
                    : std::numeric_limits<float>::max();
      dist[2] = plane[2] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[2], cptr)
                    : std::numeric_limits<float>::max();
      dist[3] = plane[3] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[3], cptr)
                    : std::numeric_limits<float>::max();
      for (i = 1; i < 4; ++i) {
        if (min_abs_dist > IAbs(dist[i])) {
          min_abs_dist = IAbs(dist[i]);
          id = i;
        }
      }
    }
    height_above_ground[pos] = dist[id];
    ++iter;
  }
 
  for (c = 1; c < nm1; ++c) {
    plane[0] = cn[c].IsValid() ? cn[c].params : nullptr;
    plane[1] = cn[c - 1].IsValid() ? cn[c - 1].params : nullptr;
    plane[2] = cn[c + 1].IsValid() ? cn[c + 1].params : nullptr;
    plane[3] = up[c].IsValid() ? up[c].params : nullptr;
    plane[4] = dn[c].IsValid() ? dn[c].params : nullptr;
    iter = (*vg_coarse_)(r, c).indices_.cbegin();
    while (iter < (*vg_coarse_)(r, c).indices_.cend()) {
      pos = *iter;
      assert(pos < static_cast<int>(nr_points));
      label = labels_[pos];
      cptr = point_cloud + (nr_point_elements * pos);
      dist[0] = plane[0] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[0], cptr)
                    : std::numeric_limits<float>::max();
      min_abs_dist = IAbs(dist[0]);
      id = 0;
      if (label) {
        dist[1] = plane[1] != nullptr
                      ? IPlaneToPointSignedDistanceWUnitNorm(plane[1], cptr)
                      : std::numeric_limits<float>::max();
        dist[2] = plane[2] != nullptr
                      ? IPlaneToPointSignedDistanceWUnitNorm(plane[2], cptr)
                      : std::numeric_limits<float>::max();
        dist[3] = plane[3] != nullptr
                      ? IPlaneToPointSignedDistanceWUnitNorm(plane[3], cptr)
                      : std::numeric_limits<float>::max();
        dist[4] = plane[4] != nullptr
                      ? IPlaneToPointSignedDistanceWUnitNorm(plane[4], cptr)
                      : std::numeric_limits<float>::max();
        for (i = 1; i < 5; ++i) {
          if (min_abs_dist > IAbs(dist[i])) {
            min_abs_dist = IAbs(dist[i]);
            id = i;
          }
        }
      }
      height_above_ground[pos] = dist[id];
      ++iter;
    }
  }
  plane[0] = cn[nm1].IsValid() ? cn[nm1].params : nullptr;
  plane[1] = cn[nm1 - 1].IsValid() ? cn[nm1 - 1].params : nullptr;
  plane[2] = up[nm1].IsValid() ? up[nm1].params : nullptr;
  plane[3] = dn[nm1].IsValid() ? dn[nm1].params : nullptr;
  iter = (*vg_coarse_)(r, nm1).indices_.cbegin();
  while (iter < (*vg_coarse_)(r, nm1).indices_.cend()) {
    pos = *iter;
    assert(pos < static_cast<int>(nr_points));
    label = labels_[pos];
    cptr = point_cloud + (nr_point_elements * pos);
    dist[0] = plane[0] != nullptr
                  ? IPlaneToPointSignedDistanceWUnitNorm(plane[0], cptr)
                  : std::numeric_limits<float>::max();
    min_abs_dist = IAbs(dist[0]);
    id = 0;
    // for candidates we take min dist:
    if (label) {
      dist[1] = plane[1] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[1], cptr)
                    : std::numeric_limits<float>::max();
      dist[2] = plane[2] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[2], cptr)
                    : std::numeric_limits<float>::max();
      dist[3] = plane[3] != nullptr
                    ? IPlaneToPointSignedDistanceWUnitNorm(plane[3], cptr)
                    : std::numeric_limits<float>::max();
      for (i = 1; i < 4; ++i) {
        if (min_abs_dist > IAbs(dist[i])) {
          min_abs_dist = IAbs(dist[i]);
          id = i;
        }
      }
    }
    height_above_ground[pos] = dist[id];
    ++iter;
  }
}

Detect()

bool apollo::perception::algorithm::PlaneFitGroundDetector::Detect ( const float *  point_cloud, const int *  semantic_value, float *  height_above_ground, unsigned int  nr_points, unsigned int  nr_point_elements  )

virtual

实现了 apollo::perception::algorithm::BaseGroundDetector.

在文件 i_ground.cc 第 1899 行定义.

                                                            {
  assert(point_cloud != nullptr);
  assert(semantic_value != nullptr);
  assert(height_above_ground != nullptr);
  assert(nr_points <= param_.nr_points_max);
  assert(nr_point_elements >= 3);
  // setup the fine voxel grid
  if (!vg_fine_->SetS(point_cloud, semantic_value,
          nr_points, nr_point_elements)) {
    return false;
  }
  // setup the coarse voxel grid
  if (!vg_coarse_->SetS(point_cloud, semantic_value,
          nr_points, nr_point_elements)) {
    return false;
  }
 
  // when single-frame-detect, should empty ground_planes_
  if (param_.single_frame_detect) {
      for (size_t row = 0; row < param_.nr_grids_coarse; ++row) {
          for (size_t col = 0; col < param_.nr_grids_coarse; ++col) {
              ground_planes_[row][col].ForceInvalid();
              ground_planes_sphe_[row][col].ForceInvalid();
          }
      }
  }
  // int nr_candis = 0;
  // int nr_valid_grid = 0;
  unsigned int r = 0;
  unsigned int c = 0;
  // Filter to generate plane fitting candidates
  // int nr_candis =
  if (param_.use_semantic_info) {
      SelectCandidates();
  } else {
      Filter();
  }
  // std::cout << "# of plane candidates: " << nr_candis << std::endl;
  //  Fit local plane using ransac
  // nr_valid_grid = Fit();
  // int nr_valid_grid =
  FitInOrder();
  // std::cout << "# of valid plane geometry (fitting): " << nr_valid_grid <<
  // std::endl;
  // Smooth plane using neighborhood information:
  if (param_.use_math_optimize) {
      SmoothInMath();
  } else {
      for (int iter = 0; iter < param_.nr_smooth_iter; ++iter) {
          // Smooth();
          SmoothInOrder();
      }
  }
 
  for (r = 0; r < param_.nr_grids_coarse; ++r) {
    for (c = 0; c < param_.nr_grids_coarse; ++c) {
      if ((*vg_coarse_)(r, c).Empty()) {
        ground_planes_[r][c].ForceInvalid();
      }
    }
  }
 
  // std::cout << "# of valid plane geometry (Smooth): " << nr_valid_grid <<
  // std::endl;
  // compute point to ground distance
  ComputeSignedGroundHeight(point_cloud, height_above_ground, nr_points,
                            nr_point_elements);
  return true;
}

Filter()

int apollo::perception::algorithm::PlaneFitGroundDetector::Filter ( )

protected

在文件 i_ground.cc 第 678 行定义.

                                   {
  int nr_candis = 0;
  unsigned int i = 0;
  unsigned int r = 0;
  memset(reinterpret_cast<void *>(labels_), 0,
         vg_fine_->NrPoints() * sizeof(char));
  //  Clear candidate list
  for (i = 0; i < vg_coarse_->NrVoxel(); ++i) {
    local_candis_[0][i].Clear();
  }
  //  Filter plane fitting candidates
  for (r = 0; r < param_.nr_grids_fine; ++r) {
    nr_candis += FilterLine(r);
  }
  return nr_candis;
}

FilterCandidates()

int apollo::perception::algorithm::PlaneFitGroundDetector::FilterCandidates ( int  r, int  c, const float *  point_cloud, PlaneFitPointCandIndices *  candi, std::vector< std::pair< int, int > > *  neighbors, unsigned int  nr_point_element  )

protected

在文件 i_ground.cc 第 1136 行定义.

                                   {
  float avg_z = 0.f;
  int count = 0;
  unsigned int i = 0;
  int r_n = 0;
  int c_n = 0;
  float z = 0.f;
  std::vector<int> filtered_indices;
  filtered_indices.reserve(candi->Size());
  for (i = 0; i < neighbors->size(); ++i) {
    r_n = (*neighbors)[i].first;
    c_n = (*neighbors)[i].second;
    if (ground_z_[r_n][c_n].second) {
      avg_z += ground_z_[r_n][c_n].first;
      count++;
    }
  }
  if (count > 0) {
    avg_z /= static_cast<float>(count);
    ground_z_[r][c].first = avg_z;
    ground_z_[r][c].second = true;
    for (i = 0; i < candi->Size(); ++i) {
      z = (point_cloud + (nr_point_element * (*candi)[i]))[2];
      if (z > avg_z - param_.candidate_filter_threshold &&
          z < avg_z + param_.candidate_filter_threshold) {
        filtered_indices.push_back((*candi)[i]);
      } else {
        labels_[(*candi)[i]] = 0;
      }
    }
    if (filtered_indices.size() != candi->Size()) {
      candi->indices.assign(filtered_indices.begin(), filtered_indices.end());
    }
  }
  return count;
}

FilterGrid()

int apollo::perception::algorithm::PlaneFitGroundDetector::FilterGrid ( const Voxel< float > &  vg, const float *  point_cloud, PlaneFitPointCandIndices *  candi, unsigned int  nr_points, unsigned int  nr_point_element  )

protected

在文件 i_ground.cc 第 623 行定义.

                                                                      {
  int pos = 0;
  int rseed = I_DEFAULT_SEED;
  int nr_candis = 0;
  unsigned int i = 0;
  unsigned int nr_samples = IMin(param_.nr_z_comp_candis, vx.NrPoints());
  if (vx.Empty()) {
    return 0;
  }
  //  generate sampled indices
  if (vx.NrPoints() <= param_.nr_z_comp_candis) {
    // IRamp(sampled_indices_, vx.NrPoints());
    //  sampled z values
    for (i = 0; i < vx.NrPoints(); ++i) {
      pos = vx.indices_[i] * nr_point_element;
      //  requires the Z element to be in the third position, i.e., after X, Y
      sampled_z_values_[i] = (point_cloud + pos)[2];
    }
  } else {
    IRandomSample(sampled_indices_, static_cast<int>(param_.nr_z_comp_candis),
                  static_cast<int>(vx.NrPoints()), &rseed);
    //  sampled z values
    for (i = 0; i < nr_samples; ++i) {
      pos = vx.indices_[sampled_indices_[i]] * nr_point_element;
      // requires the Z element to be in the third position, i.e., after X, Y
      sampled_z_values_[i] = (point_cloud + pos)[2];
    }
  }
  // Filter points and get plane fitting candidates
  nr_candis = CompareZ(point_cloud, vx.indices_, sampled_z_values_, candi,
                       nr_points, nr_point_element, nr_samples);
  return nr_candis;
}

FilterLine()

int apollo::perception::algorithm::PlaneFitGroundDetector::FilterLine ( unsigned int  r )

protected

在文件 i_ground.cc 第 661 行定义.

                                                     {
  int nr_candis = 0;
  unsigned int c = 0;
  const float *point_cloud = vg_fine_->const_data();
  unsigned int nr_points = vg_fine_->NrPoints();
  unsigned int nr_point_element = vg_fine_->NrPointElement();
  unsigned int begin = (r * param_.nr_grids_fine);
  int parent = 0;
  for (c = 0; c < param_.nr_grids_fine; c++) {
    parent = map_fine_to_coarse_[begin + c];
    nr_candis +=
        FilterGrid((*vg_fine_)(r, c), point_cloud, &local_candis_[0][parent],
                   nr_points, nr_point_element);
  }
  return nr_candis;
}

Fit()

int apollo::perception::algorithm::PlaneFitGroundDetector::Fit ( )

protected

在文件 i_ground.cc 第 1127 行定义.

                                {
  int nr_grids = 0;
  for (unsigned int r = 0; r < param_.nr_grids_coarse; ++r) {
    nr_grids += FitLine(r);
  }
  return nr_grids;
}

FitGrid()

int apollo::perception::algorithm::PlaneFitGroundDetector::FitGrid ( const float *  point_cloud, PlaneFitPointCandIndices *  candi, GroundPlaneLiDAR *  groundplane, unsigned int  nr_points, unsigned int  nr_point_element, float  dist_thre  )

protected

在文件 i_ground.cc 第 986 行定义.

                                                     {
  // initialize the best plane
  groundplane->ForceInvalid();
  // not enough samples, failed and return
  if (candi->Size() < param_.nr_inliers_min_threshold) {
    return 0;
  }
  GroundPlaneLiDAR plane;
  float ptp_dist = 0.0f;
  float fit_cost = 0.0f;
  float fit_cost_best = dist_thre;
  int nr_inliers = 0;
  int nr_inliers_best = -1;
  int i = 0;
  int j = 0;
  int rseed = I_DEFAULT_SEED;
  int indices_trial[] = {0, 0, 0};
  int nr_samples = candi->Prune(param_.nr_samples_min_threshold,
                                param_.nr_samples_max_threshold);
  int nr_inliers_termi = IRound(static_cast<float>(nr_samples) *
                                param_.termi_inlier_percen_threshold);
  // 3x3 matrix stores: x, y, z; x, y, z; x, y, z;
  float samples[9];
  // copy 3D points
  float *psrc = nullptr;
  float *pdst = pf_threeds_;
  for (i = 0; i < nr_samples; ++i) {
    assert((*candi)[i] < static_cast<int>(nr_points));
    ICopy3(point_cloud + (nr_point_element * (*candi)[i]), pdst);
    pdst += dim_point_;
  }
  // generate plane hypothesis and vote
  for (i = 0; i < param_.nr_ransac_iter_threshold; ++i) {
    IRandomSample(indices_trial, 3, nr_samples, &rseed);
    IScale3(indices_trial, dim_point_);
    ICopy3(pf_threeds_ + indices_trial[0], samples);
    ICopy3(pf_threeds_ + indices_trial[1], samples + 3);
    ICopy3(pf_threeds_ + indices_trial[2], samples + 6);
    IPlaneFitDestroyed(samples, plane.params);
    // check if the plane hypothesis has valid geometry
    if (plane.GetDegreeNormalToZ() > param_.planefit_orien_threshold) {
      continue;
    }
    // iterate samples and check if the point to plane distance is below
    // threshold
    psrc = pf_threeds_;
    nr_inliers = 0;
    fit_cost = 0;
    for (j = 0; j < nr_samples; ++j) {
      ptp_dist = IPlaneToPointDistanceWUnitNorm(plane.params, psrc);
      if (ptp_dist < dist_thre) {
        nr_inliers++;
        fit_cost += ptp_dist;
      }
      psrc += dim_point_;
    }
    // Assign number of supports
    plane.SetNrSupport(nr_inliers);
 
    fit_cost = nr_inliers > 0 ? (fit_cost / static_cast<float>(nr_inliers))
                              : dist_thre;
    // record the best plane
    if (nr_inliers >= nr_inliers_best) {
      if (nr_inliers == nr_inliers_best) {
        if (fit_cost < fit_cost_best) {
          nr_inliers_best = nr_inliers;
          fit_cost_best = fit_cost;
          *groundplane = plane;
        }
      } else {
        nr_inliers_best = nr_inliers;
        fit_cost_best = fit_cost;
        *groundplane = plane;
      }
      // found enough inliers - terminate the ransac
      if (nr_inliers_best > nr_inliers_termi) {
        break;
      }
    }
  }
  // check if meet the inlier number requirement
  if (!groundplane->IsValid()) {
    return 0;
  }
  if (groundplane->GetNrSupport() <
      static_cast<int>(param_.nr_inliers_min_threshold)) {
    groundplane->ForceInvalid();
    return 0;
  }
  // iterate samples and check if the point to plane distance is within
  // threshold
  nr_inliers = 0;
  psrc = pf_threeds_;
  pdst = pf_threeds_;
  for (i = 0; i < nr_samples; ++i) {
    ptp_dist = IPlaneToPointDistanceWUnitNorm(groundplane->params, psrc);
    if (ptp_dist < dist_thre) {
      ICopy3(psrc, pdst);
      pdst += 3;
      nr_inliers++;
    }
    psrc += dim_point_;
  }
  groundplane->SetNrSupport(nr_inliers);
  // note that pf_threeds_ will be destroyed after calling this routine
  IPlaneFitTotalLeastSquare(pf_threeds_, groundplane->params, nr_inliers);
  // filtering: the best plane orientation is not valid*/
  // std::cout << groundplane->GetDegreeNormalToZ() << std::endl;
  if (groundplane->GetDegreeNormalToZ() > param_.planefit_orien_threshold) {
    groundplane->ForceInvalid();
    return 0;
  }
  return nr_inliers;
}

FitGridWithNeighbors()

int apollo::perception::algorithm::PlaneFitGroundDetector::FitGridWithNeighbors ( int  r, int  c, const float *  point_cloud, GroundPlaneLiDAR *  groundplane, unsigned int  nr_points, unsigned int  nr_point_element, float  dist_thre  )

protected

在文件 i_ground.cc 第 1183 行定义.

                                                                            {
  // initialize the best plane
  groundplane->ForceInvalid();
  // not enough samples, failed and return
 
  PlaneFitPointCandIndices &candi = local_candis_[r][c];
  std::vector<std::pair<int, int>> neighbors;
  GetNeighbors(r, c, param_.nr_grids_coarse, param_.nr_grids_coarse,
               &neighbors);
  FilterCandidates(r, c, point_cloud, &candi, &neighbors, nr_point_element);
 
  if (candi.Size() < param_.nr_inliers_min_threshold) {
    return 0;
  }
 
  GroundPlaneLiDAR plane;
  int kNr_iter =
      param_.nr_ransac_iter_threshold + static_cast<int>(neighbors.size());
  //  check hypothesis initialized correct or not
  if (kNr_iter < 1) {
    return 0;
  }
  GroundPlaneLiDAR hypothesis[kNr_iter];
 
  float ptp_dist = 0.0f;
  int best = 0;
  int nr_inliers = 0;
  int nr_inliers_best = -1;
  float angle_best = std::numeric_limits<float>::max();
 
  int rseed = I_DEFAULT_SEED;
  int indices_trial[] = {0, 0, 0};
  int nr_samples = candi.Prune(param_.nr_samples_min_threshold,
                               param_.nr_samples_max_threshold);
  int nr_inliers_termi = IRound(static_cast<float>(nr_samples) *
                                param_.termi_inlier_percen_threshold);
  // 3x3 matrix stores: x, y, z; x, y, z; x, y, z;
  float samples[9];
  // copy 3D points
  float *psrc = nullptr;
  float *pdst = pf_threeds_;
  int r_n = 0;
  int c_n = 0;
  float angle = -1.f;
  for (int i = 0; i < nr_samples; ++i) {
    assert(candi[i] < static_cast<int>(nr_points));
    ICopy3(point_cloud + (nr_point_element * candi[i]), pdst);
    pdst += dim_point_;
  }
  // generate plane hypothesis and vote
  for (int i = 0; i < param_.nr_ransac_iter_threshold; ++i) {
    IRandomSample(indices_trial, 3, nr_samples, &rseed);
    IScale3(indices_trial, dim_point_);
    ICopy3(pf_threeds_ + indices_trial[0], samples);
    ICopy3(pf_threeds_ + indices_trial[1], samples + 3);
    ICopy3(pf_threeds_ + indices_trial[2], samples + 6);
    IPlaneFitDestroyed(samples, hypothesis[i].params);
    // check if the plane hypothesis has valid geometry
    if (hypothesis[i].GetDegreeNormalToZ() > param_.planefit_orien_threshold) {
      continue;
    }
    // iterate samples and check if the point to plane distance is below
    // threshold
    psrc = pf_threeds_;
    nr_inliers = 0;
    for (int j = 0; j < nr_samples; ++j) {
      ptp_dist = IPlaneToPointDistanceWUnitNorm(hypothesis[i].params, psrc);
      if (ptp_dist < dist_thre) {
        nr_inliers++;
      }
      psrc += dim_point_;
    }
    // Assign number of supports
    hypothesis[i].SetNrSupport(nr_inliers);
    hypothesis[i].SetOrigin("self");
 
    if (nr_inliers > nr_inliers_termi) {
      break;
    }
  }
 
  for (size_t i = 0; i < neighbors.size(); ++i) {
    r_n = neighbors[i].first;
    c_n = neighbors[i].second;
    if (ground_planes_[r_n][c_n].IsValid()) {
      hypothesis[i + param_.nr_ransac_iter_threshold] =
          ground_planes_[r_n][c_n];
      psrc = pf_threeds_;
      nr_inliers = 0;
      for (int j = 0; j < nr_samples; ++j) {
        ptp_dist = IPlaneToPointDistanceWUnitNorm(
            hypothesis[i + param_.nr_ransac_iter_threshold].params, psrc);
        if (ptp_dist < dist_thre) {
          nr_inliers++;
        }
        psrc += dim_point_;
      }
      if (nr_inliers < static_cast<int>(param_.nr_inliers_min_threshold)) {
        hypothesis[i + param_.nr_ransac_iter_threshold].ForceInvalid();
        continue;
      }
      hypothesis[i + param_.nr_ransac_iter_threshold].SetNrSupport(nr_inliers);
      hypothesis[i + param_.nr_ransac_iter_threshold].SetOrigin("neighbor");
    }
  }
 
  nr_inliers_best = -1;
  for (int i = 0; i < kNr_iter; ++i) {
    if (!(hypothesis[i].IsValid())) {
      continue;
    }
    nr_inliers = hypothesis[i].GetNrSupport();
    if (nr_inliers >= nr_inliers_best) {
      if (nr_inliers == nr_inliers_best) {
        angle = CalculateAngleDist(hypothesis[i], neighbors);
        if (angle < angle_best && angle > 0) {
          angle_best = angle;
          best = i;
        }
      } else {
        nr_inliers_best = nr_inliers;
        angle_best = CalculateAngleDist(hypothesis[i], neighbors);
        best = i;
      }
    }
  }
 
  *groundplane = hypothesis[best];
 
  // check if meet the inlier number requirement
  if (!groundplane->IsValid()) {
    groundplane->SetInvalidStatus("groundplane Invalid");
    return 0;
  }
  if (groundplane->GetNrSupport() <
      static_cast<int>(param_.nr_inliers_min_threshold)) {
    groundplane->ForceInvalid();
    groundplane->SetInvalidStatus("inliers Small");
    return 0;
  }
  // iterate samples and check if the point to plane distance is within
  // threshold
  nr_inliers = 0;
  psrc = pf_threeds_;
  pdst = pf_threeds_;
  for (int i = 0; i < nr_samples; ++i) {
    ptp_dist = IPlaneToPointDistanceWUnitNorm(groundplane->params, psrc);
    if (ptp_dist < dist_thre) {
      ICopy3(psrc, pdst);
      pdst += 3;
      nr_inliers++;
    }
    psrc += dim_point_;
  }
  groundplane->SetNrSupport(nr_inliers);
 
  // note that pf_threeds_ will be destroyed after calling this routine
  IPlaneFitTotalLeastSquare(pf_threeds_, groundplane->params, nr_inliers);
  if (angle_best <= CalculateAngleDist(*groundplane, neighbors)) {
    *groundplane = hypothesis[best];
    groundplane->SetStatus(true);
  }
 
  if (groundplane->GetDegreeNormalToZ() > param_.planefit_orien_threshold) {
    groundplane->ForceInvalid();
    groundplane->SetInvalidStatus("Z Bigger");
    return 0;
  }
 
  const auto &voxel_cur = (*vg_coarse_)(r, c);
  float radius = voxel_cur.dim_x_ * 0.5f;
  float cx = voxel_cur.v_[0] + radius;
  float cy = voxel_cur.v_[1] + radius;
  float cz = -(groundplane->params[0] * cx + groundplane->params[1] * cy +
               groundplane->params[3]) /
             groundplane->params[2];
  ground_z_[r][c].first = cz;
  ground_z_[r][c].second = true;
 
  return nr_inliers;
}

FitInOrder()

int apollo::perception::algorithm::PlaneFitGroundDetector::FitInOrder ( )

protected

在文件 i_ground.cc 第 1389 行定义.

                                       {
  int nr_grids = 0;
  unsigned int i = 0;
  unsigned int j = 0;
  int r = 0;
  int c = 0;
  GroundPlaneLiDAR gp;
  for (i = 0; i < param_.nr_grids_coarse; ++i) {
    for (j = 0; j < param_.nr_grids_coarse; ++j) {
      ground_z_[i][j].first = 0.f;
      ground_z_[i][j].second = false;
    }
  }
  std::stringstream sstr;
  sstr << "[GroundFitInfo] " << std::to_string(frame_timestamp_) << std::endl;
  size_t count = 0;
  for (i = 0; i < vg_coarse_->NrVoxel(); ++i) {
    r = order_table_[i].first;
    c = order_table_[i].second;
 
    if (param_.debug_output) {
        sstr << "[FitInOrder] row = " << r << " col = " << c
             << " center_x = "
             << (*vg_coarse_)(r, c).v_[0] + (*vg_coarse_)(r, c).dim_x_ * 0.5f
             << " center_y = "
             << (*vg_coarse_)(r, c).v_[1] + (*vg_coarse_)(r, c).dim_y_ * 0.5f
             << " inliers_thres = " << pf_thresholds_[r][c]
             << " [AfterFilterCandidates] count = " << local_candis_[r][c].Size();
 
        float max_z = -5.0, min_z = 5.0;
        for (unsigned int p = 0; p < local_candis_[r][c].Size(); ++p) {
            float x = (vg_coarse_->const_data() + 
                (vg_coarse_->NrPointElement() * (local_candis_[r][c])[p]))[0];
            float y = (vg_coarse_->const_data() + 
                (vg_coarse_->NrPointElement() * (local_candis_[r][c])[p]))[1];
            float z = (vg_coarse_->const_data() + 
                (vg_coarse_->NrPointElement() * (local_candis_[r][c])[p]))[2];
            if (z > max_z) {
                max_z = z;
            }
            if (z < min_z) {
                min_z = z;
            }
            if (p == 0) {
                sstr << " sample_point_cooridinate = (" << x << ", " << y
                     << ", " << z << ")";
            }
        }
        sstr << " min_z =  " << min_z << " max_z= " << max_z;
    }
    count += local_candis_[r][c].Size();
    if (FitGridWithNeighbors(
            r, c, vg_coarse_->const_data(), &gp, vg_coarse_->NrPoints(),
            vg_coarse_->NrPointElement(), pf_thresholds_[r][c]) >=
        static_cast<int>(param_.nr_inliers_min_threshold)) {
      IPlaneEucliToSpher(gp, &ground_planes_sphe_[r][c]);
      ground_planes_[r][c] = gp;
      nr_grids++;
    } else {
      ground_planes_sphe_[r][c].ForceInvalid();
      ground_planes_[r][c].ForceInvalid();
    }
    
    if (param_.debug_output) {
        sstr << " [AfterNeighborFilter] count = " << local_candis_[r][c].Size()
             << " [AfterGridPlaneFit] ground_z_valid = " << ground_z_[r][c].second
             << " ground_z_value = " << ground_z_[r][c].first
             << " plane_valid = " << ground_planes_[r][c].IsValid()
             << " plane_invalid_status = "
             << ground_planes_[r][c].GetInvalidStatus()
             << " plane_origin = " << ground_planes_[r][c].GetOrigin()
             << " support_number = " << ground_planes_[r][c].GetNrSupport()
             << " plane_params = " << ground_planes_[r][c].params[0] << ", "
             << ground_planes_[r][c].params[1] << ", "
             << ground_planes_[r][c].params[2]
             << ", " << ground_planes_[r][c].params[3] << std::endl;
    }
  }
  if (param_.debug_output) {
      AINFO << sstr.str();
      AINFO << "This Frame " << std::to_string(frame_timestamp_)
            << " ground_count is " << count;
  }
  sstr.clear();
  return nr_grids;
}

FitLine()

int apollo::perception::algorithm::PlaneFitGroundDetector::FitLine ( unsigned int  r )

protected

在文件 i_ground.cc 第 1106 行定义.

                                                  {
  int nr_grids = 0;
  unsigned int c = 0;
  GroundPlaneLiDAR gp;
  for (c = 0; c < param_.nr_grids_coarse; c++) {
    if (FitGrid(vg_coarse_->const_data(), &local_candis_[r][c], &gp,
                vg_coarse_->NrPoints(), vg_coarse_->NrPointElement(),
                pf_thresholds_[r][c]) >=
        static_cast<int>(param_.nr_inliers_min_threshold)) {
      // transform to polar coordinates and store:
      IPlaneEucliToSpher(gp, &ground_planes_sphe_[r][c]);
      ground_planes_[r][c] = gp;
      nr_grids++;
    } else {
      ground_planes_sphe_[r][c].ForceInvalid();
      ground_planes_[r][c].ForceInvalid();
    }
  }
  return nr_grids;
}

GetCandis()

PlaneFitPointCandIndices ** apollo::perception::algorithm::PlaneFitGroundDetector::GetCandis

(

)

const

在文件 i_ground.cc 第 2019 行定义.

                                                                   {
  return local_candis_;
}

GetGrid()

const VoxelGridXY< float > * apollo::perception::algorithm::PlaneFitGroundDetector::GetGrid

(

)

const

在文件 i_ground.cc 第 1996 行定义.

                                                                {
  return vg_coarse_;
}

GetGridDimX()

unsigned int apollo::perception::algorithm::PlaneFitGroundDetector::GetGridDimX

(

)

const

在文件 i_ground.cc 第 2007 行定义.

                                                       {
  return vg_coarse_->NrVoxelX();
}

GetGridDimY()

unsigned int apollo::perception::algorithm::PlaneFitGroundDetector::GetGridDimY

(

)

const

在文件 i_ground.cc 第 2011 行定义.

                                                       {
  return vg_coarse_->NrVoxelY();
}

GetGroundPlane()

const GroundPlaneLiDAR * apollo::perception::algorithm::PlaneFitGroundDetector::GetGroundPlane	(	int	r,
		int	c
	)		const

在文件 i_ground.cc 第 2000 行定义.

                                                                            {
  assert(r >= 0 && r < static_cast<int>(param_.nr_grids_coarse));
  assert(c >= 0 && c < static_cast<int>(param_.nr_grids_coarse));
  return ground_planes_ != nullptr ? ground_planes_[r] + c : nullptr;
}

GetGroundPlaneSpherParams()

void apollo::perception::algorithm::PlaneFitGroundDetector::GetGroundPlaneSpherParams ( float *  params )

protected

在文件 i_ground.cc 第 1812 行定义.

                                                                    {
    unsigned int lap_size = param_.nr_grids_coarse * param_.nr_grids_coarse;
    unsigned int offset = 0;
    // for (unsigned int r = 0; r < param_.nr_grids_coarse; ++r) {
    //     for (unsigned int c = 0; c < param_.nr_grids_coarse; ++c) {
    //         if ((*vg_coarse_)(r, c).Empty()) {
    //             ground_planes_sphe_[r][c].ForceInvalid();
    //         } else {
    //             IPlaneEucliToSpher(ground_planes_[r][c], 
    //                &ground_planes_sphe_[r][c]);
    //         }
    //     }
    // }
    for (unsigned int i = 0; i < lap_size; ++i) {
        params[i + offset] = ground_planes_sphe_[0][i].theta;
    }
    offset += lap_size;
    for (unsigned int i = 0; i < lap_size; ++i) {
        params[i + offset] = ground_planes_sphe_[0][i].phi;
    }
    offset += lap_size;
    for (unsigned int i = 0; i < lap_size; ++i) {
        params[i + offset] = ground_planes_sphe_[0][i].d;
    }
}

GetInliers()

void apollo::perception::algorithm::PlaneFitGroundDetector::GetInliers ( int *  inliers )

protected

在文件 i_ground.cc 第 1696 行定义.

                                                    {
    unsigned int lap_size = param_.nr_grids_coarse * param_.nr_grids_coarse;
    for (unsigned int i = 0; i < lap_size; ++i) {
        inliers[i] = ground_planes_sphe_[0][i].GetNrSupport();
    }
}

GetLabel()

const char * apollo::perception::algorithm::PlaneFitGroundDetector::GetLabel

(

)

const

在文件 i_ground.cc 第 1971 行定义.

{ return labels_; }

GetNeighbors()

void apollo::perception::algorithm::PlaneFitGroundDetector::GetNeighbors ( int  r, int  c, int  rows, int  cols, std::vector< std::pair< int, int > > *  neighbors  )

protected

在文件 i_ground.cc 第 1476 行定义.

                                             {
  int left = IMax(0, c - 1);
  int right = IMin(cols - 1, c + 1);
  int up = IMax(0, r - 1);
  int down = IMin(rows - 1, r + 1);
  int x = 0;
  int y = 0;
  neighbors->reserve(8);
  for (x = left; x <= right; ++x) {
    for (y = up; y <= down; ++y) {
      if (x == c && y == r) {
        continue;
      }
      neighbors->push_back(std::pair<int, int>(y, x));
    }
  }
}

GetUnknownHeight()

float apollo::perception::algorithm::PlaneFitGroundDetector::GetUnknownHeight

(

)

在文件 i_ground.cc 第 2015 行定义.

                                               {
  return std::numeric_limits<float>::max();
}

Init()

bool apollo::perception::algorithm::PlaneFitGroundDetector::Init

(

)

在文件 i_ground.cc 第 154 行定义.

                                  {
  unsigned int r = 0;
  unsigned int c = 0;
  unsigned int pr = 0;
  unsigned int pc = 0;
  unsigned int index = 0;
  unsigned int capacity = 0;
  unsigned int sf = param_.nr_grids_fine / param_.nr_grids_coarse;
  if (!param_.Validate()) {
    return false;
  }
  // fine grid:
  vg_fine_ = new VoxelGridXY<float>();
  if (vg_fine_ == nullptr) {
    return false;
  }
  if (!vg_fine_->Alloc(param_.nr_grids_fine, param_.nr_grids_fine,
                       -param_.roi_region_rad_x, param_.roi_region_rad_x,
                       -param_.roi_region_rad_y, param_.roi_region_rad_y,
                       -param_.roi_region_rad_z, param_.roi_region_rad_z)) {
    return false;
  }
  // coarse grid:
  vg_coarse_ = new VoxelGridXY<float>();
  if (vg_coarse_ == nullptr) {
    return false;
  }
  if (!vg_coarse_->Alloc(param_.nr_grids_coarse, param_.nr_grids_coarse,
                         -param_.roi_region_rad_x, param_.roi_region_rad_x,
                         -param_.roi_region_rad_y, param_.roi_region_rad_y,
                         -param_.roi_region_rad_z, param_.roi_region_rad_z)) {
    return false;
  }
 
  // Init order lookup table
  order_table_ = IAlloc<std::pair<int, int>>(vg_fine_->NrVoxel());
  InitOrderTable(vg_coarse_, order_table_);
 
  // ground plane:
  ground_planes_ =
      IAlloc2<GroundPlaneLiDAR>(param_.nr_grids_coarse, param_.nr_grids_coarse);
  if (!ground_planes_) {
    return false;
  }
  ground_planes_sphe_ = IAlloc2<GroundPlaneSpherical>(param_.nr_grids_coarse,
                                                      param_.nr_grids_coarse);
  if (!ground_planes_sphe_) {
    return false;
  }
  ground_z_ = IAlloc2<std::pair<float, bool>>(param_.nr_grids_coarse,
                                              param_.nr_grids_coarse);
  if (!ground_z_) {
    return false;
  }
  // sample candis:
  local_candis_ = IAlloc2<PlaneFitPointCandIndices>(param_.nr_grids_coarse,
                                                    param_.nr_grids_coarse);
  if (!local_candis_) {
    return false;
  }
  // Reserve space to avoid runtime memory re-allocation
  for (r = 0; r < param_.nr_grids_coarse; ++r) {
    for (c = 0; c < param_.nr_grids_coarse; ++c) {
      if (r < (param_.nr_grids_coarse << 3) &&
          c < (param_.nr_grids_coarse << 3)) {
        capacity = 16384;
      } else if (r < (param_.nr_grids_coarse << 2) &&
                 c < (param_.nr_grids_coarse << 2)) {
        capacity = 8192;
      } else if (r < (param_.nr_grids_coarse << 1) &&
                 c < (param_.nr_grids_coarse << 1)) {
        capacity = 4096;
      } else {
        capacity = 512;
      }
      local_candis_[r][c].Reserve(capacity);
    }
  }
  // ground candidate
  ground_candidates_ = IAlloc2<GroundCandidateIndices>(param_.nr_grids_fine,
      param_.nr_grids_fine);
  if (!ground_candidates_) {
    return false;
  }
  // threeds in ransac, in inhomogeneous coordinates:
  pf_threeds_ =
      IAllocAligned<float>(param_.nr_samples_max_threshold * dim_point_, 4);
  if (!pf_threeds_) {
    return false;
  }
  memset(reinterpret_cast<void *>(pf_threeds_), 0,
         param_.nr_samples_max_threshold * dim_point_ * sizeof(float));
  // labels:
  labels_ = IAllocAligned<char>(param_.nr_points_max, 4);
  if (!labels_) {
    return false;
  }
  memset(reinterpret_cast<void *>(labels_), 0,
         param_.nr_points_max * sizeof(char));
  // map of fine grid id to coarse id:
  map_fine_to_coarse_ = IAllocAligned<unsigned int>(
      param_.nr_grids_fine * param_.nr_grids_fine, 4);
  if (!map_fine_to_coarse_) {
    return false;
  }
  for (r = 0; r < param_.nr_grids_fine; ++r) {
    pr = r / sf;
    index = r * param_.nr_grids_fine;
    for (c = 0; c < param_.nr_grids_fine; ++c) {
      pc = c / sf;
      map_fine_to_coarse_[index + c] = pr * param_.nr_grids_coarse + pc;
    }
  }
  // ransac memory:
  sampled_z_values_ = IAllocAligned<float>(param_.nr_z_comp_candis, 4);
  if (!sampled_z_values_) {
    return false;
  }
  memset(reinterpret_cast<void *>(sampled_z_values_), 0,
         param_.nr_z_comp_candis * sizeof(float));
  // ransac memory:
  sampled_indices_ = IAllocAligned<int>(param_.nr_z_comp_candis, 4);
  if (!sampled_indices_) {
    return false;
  }
  memset(reinterpret_cast<void *>(sampled_indices_), 0,
         param_.nr_z_comp_candis * sizeof(int));
  // ransac thresholds:
  pf_thresholds_ =
      IAlloc2<float>(param_.nr_grids_coarse, param_.nr_grids_coarse);
  if (!pf_thresholds_) {
    return false;
  }
  // compute thresholds
  ComputeAdaptiveThreshold();
 
  //global optimization
  unsigned int lap_size = param_.nr_grids_coarse * param_.nr_grids_coarse;
  neighbors_.resize(lap_size);
  for (unsigned int r = 0; r < param_.nr_grids_coarse; ++r) {
    for (unsigned int c = 0; c < param_.nr_grids_coarse; ++c) {
      unsigned int index_cur = r * param_.nr_grids_coarse + c;
      neighbors_[index_cur].clear();
      if (r > 0) {
        neighbors_[index_cur].push_back((r - 1)* param_.nr_grids_coarse + c);
      }
      if (r + 1 < param_.nr_grids_coarse) {
        neighbors_[index_cur].push_back((r + 1)* param_.nr_grids_coarse + c);
      }
      if (c > 0) {
        neighbors_[index_cur].push_back(r * param_.nr_grids_coarse + c - 1);
      }
      if (c + 1 < param_.nr_grids_coarse) {
        neighbors_[index_cur].push_back(r * param_.nr_grids_coarse + c + 1);
      }
      if (c + 1 < param_.nr_grids_coarse && r + 1 < param_.nr_grids_coarse) {
        neighbors_[index_cur].push_back(
           (r + 1)* param_.nr_grids_coarse + c + 1);
      }
      if (c > 0 && r + 1 < param_.nr_grids_coarse) {
        neighbors_[index_cur].push_back(
            (r + 1)* param_.nr_grids_coarse + c - 1);
      }
      if (c + 1 < param_.nr_grids_coarse && r > 0) {
        neighbors_[index_cur].push_back(
            (r - 1)* param_.nr_grids_coarse + c + 1);
      }
      if (c > 0 && r > 0) {
        neighbors_[index_cur].push_back(
            (r - 1)* param_.nr_grids_coarse + c - 1);
      }
    }
  }
  weight_buffer_ = IAlloc2<float>(2, lap_size);
  inliers_ = IAllocAligned<int>(lap_size, 4);
  sphe_params_ = IAllocAligned<float>(lap_size * 3, 4);
  a_array_.resize(lap_size * 2, lap_size);
  b_array_.resize(lap_size * 2, 3);
 
  for (unsigned int i = 0; i < lap_size; ++i) {
    a_array_.coeffRef(i, i) = 1.0;
    float w = -1.0 / neighbors_[i].size();
    for (const auto& j : neighbors_[i]) {
      a_array_.coeffRef(i, j) = w;
    }
  }
 
  return true;
}

InitOrderTable()

void apollo::perception::algorithm::PlaneFitGroundDetector::InitOrderTable ( const VoxelGridXY< float > *  vg, std::pair< int, int > *  order  )

protected

在文件 i_ground.cc 第 125 行定义.

                                                                      {
  std::vector<std::pair<float, int>> map_dist;
  float cx = 0.f;
  float cy = 0.f;
  float dist2 = 0.f;
  float radius = 0.f;
  unsigned int i = 0;
  int id = 0;
  for (i = 0; i < vg->NrVoxel(); ++i) {
    const auto &voxel = vg->GetConstVoxels()[i];
    radius = voxel.dim_x_ * 0.5f;
    cx = voxel.v_[0] + radius;
    cy = voxel.v_[1] + radius;
    dist2 = cx * cx + cy * cy;
    map_dist.push_back(std::pair<float, int>(dist2, i));
  }
  sort(map_dist.begin(), map_dist.end(),
       [](const std::pair<float, int> &a, const std::pair<float, int> &b) {
         return a.first < b.first;
       });
  for (i = 0; i < map_dist.size(); ++i) {
    id = map_dist[i].second;
    const auto &voxel = vg->GetConstVoxels()[id];
    order[i].first = voxel.iy_;
    order[i].second = voxel.ix_;
  }
}

NeighborGroundCandidate()

int apollo::perception::algorithm::PlaneFitGroundDetector::NeighborGroundCandidate ( int  row, int  col, const float *  point_cloud, const int *  semantic_list, const std::vector< int > &  indices, PlaneFitPointCandIndices *  candi, unsigned int  nr_point_element  )

protected

在文件 i_ground.cc 第 764 行定义.

                                   {
    const float pi = 3.1415926535898;
    unsigned int nr_contradi = 0;
    const float *pt_ptr = nullptr;
    const int *semantic_value = nullptr;
    auto voxel = (*vg_fine_)(row, col);
    // default delta = 0.022
    float res = param_.roi_region_rad_x * 2 * 1.0f / param_.nr_grids_fine;
    float delta = tan(param_.planefit_orien_threshold * pi / 180.0) * res;
    delta /= 2.0;
 
    // get neighbors
    std::vector<std::pair<int, int>> neigh;
    GetNeighbors(row, col, param_.nr_grids_fine, param_.nr_grids_fine, &neigh);
    // find around-ground-ave and original candidate methods
    size_t count = 0;
    for (size_t i = 0; i < indices.size(); i++) {
        if (ground_candidates_[row][col].GetIndicesVisitedFlag(i)) {
            continue;
        }
        int pos = indices[i];
        pt_ptr = point_cloud + (pos * nr_point_element);
        float z = pt_ptr[2];
        float candidata_z = 0.0;
        // condition 3: near around-ground-value
        if (ground_candidates_[row][col].GetValidFlag()) {
            candidata_z = ground_candidates_[row][col].GetValidHeight();
            if (fabs(z - candidata_z) < 2 * delta) {
                labels_[pos] = 1;
                candi->PushIndex(pos);
                ground_candidates_[row][col].SetVisitedTrue(i);
                count++;
                continue;
            }
        }
        for (size_t jj = 0; jj < neigh.size(); jj++) {
            int rr = neigh[jj].first;
            int cc = neigh[jj].second;
            if (ground_candidates_[rr][cc].GetValidFlag()) {
                candidata_z = ground_candidates_[rr][cc].GetValidHeight();
                if (fabs(z - candidata_z) < 2 * delta) {
                    labels_[pos] = 1;
                    candi->PushIndex(pos);
                    ground_candidates_[row][col].SetVisitedTrue(i);
                    count++;
                }
            }
        }
    }
    int valid_count = ground_candidates_[row][col].GetValidCount();
    if (count + valid_count > param_.nr_z_comp_candis / 2 ||
        count + valid_count > 0.9 *
            ground_candidates_[row][col].GetIndicesSize()) {
        return count;
    }
    // ground info okay. run extreme-scenes
    if (voxel.Empty()) {
        return count;
    }
    int rseed = I_DEFAULT_SEED;
    std::vector<float> sample_z;
    unsigned int voxel_points = voxel.NrPoints();
    unsigned int nr_samples = IMin(param_.nr_z_comp_candis, voxel_points);
    unsigned int nr_z_comp_fail_threshold =
      IMin(param_.nr_z_comp_fail_threshold, (unsigned int)(nr_samples >> 1));
    sample_z.resize(nr_samples);
    sample_z.clear();
    if (voxel_points <= param_.nr_z_comp_candis) {
        for (size_t ii = 0; ii < voxel_points; ++ii) {
            int pos = voxel.indices_[ii] * nr_point_element;
            sample_z[ii] = (point_cloud + pos)[2];
        }
    } else {
        IRandomSample(sampled_indices_,
            static_cast<int>(param_.nr_z_comp_candis), voxel_points, &rseed);
        //  sampled z values
        for (size_t ii = 0; ii < nr_samples; ++ii) {
            int pos = voxel.indices_[sampled_indices_[ii]] * nr_point_element;
            sample_z[ii] = (point_cloud + pos)[2];
        }
    }
 
    // if semantic is all-wrong, it seems to degarde to original methods
    for (size_t i = 0; i < indices.size(); i++) {
        if (ground_candidates_[row][col].GetIndicesVisitedFlag(i)) {
            continue;
        }
        int pos = indices[i];
        // is ground
        semantic_value = semantic_list + pos;
        int value = semantic_value[0];
        if (value == param_.semantic_ground_value) {
            labels_[pos] = 1;
            candi->PushIndex(pos);
            count++;
            continue;
        }
        if (count + valid_count > param_.nr_z_comp_candis / 2 ||
            count + valid_count > 0.9 *
                ground_candidates_[row][col].GetIndicesSize()) {
            return count;
        }
        pt_ptr = point_cloud + (pos * nr_point_element);
        float z = pt_ptr[2];
        // within roi_near_rad
        if (IAbs(pt_ptr[0]) < param_.roi_near_rad &&
            IAbs(pt_ptr[1]) < param_.roi_near_rad) {
            // if (z <= param_.sample_region_z_upper &&
            //     z >= param_.sample_region_z_lower) {
            //     labels_[pos] = 1;
            //     candi->PushIndex(pos);
            //     count++;
            //     continue;
            // } else {
            //     continue;
            // }
            if (z > param_.sample_region_z_upper ||
                z < param_.sample_region_z_lower) {
                continue;
            }
        }
        // beyond roi_near_rad
        if (nr_samples > nr_z_comp_fail_threshold) {
            for (size_t jj = 0; jj < nr_samples; ++jj) {
                float delta_z = IAbs(sample_z[jj] - z);
                if (delta_z > param_.planefit_filter_threshold) {
                    nr_contradi++;
                    if (nr_contradi > nr_z_comp_fail_threshold) {
                        break;
                    }
                }
            }
        }
        if (nr_contradi <= nr_z_comp_fail_threshold) {
            labels_[pos] = 1;
            candi->PushIndex(pos);
            count++;
        }
    }
    return count;
}

Optimization()

void apollo::perception::algorithm::PlaneFitGroundDetector::Optimization ( )

protected

在文件 i_ground.cc 第 1703 行定义.

                                          {
    unsigned int lap_size = param_.nr_grids_coarse * param_.nr_grids_coarse;
    unsigned int i = 0;
    Eigen::SparseMatrix<float> in;
    Eigen::SparseMatrix<float> out;
    memset((void*)weight_buffer_[0], 0, 2 * lap_size * sizeof(float));
 
    for (i = 0; i < lap_size; ++i) {
        weight_buffer_[0][i] = inliers_[i] > 8 ? 
            std::exp((inliers_[i] - 8) / 20) : 0.1;
    }
 
    for (i = 0; i < lap_size; ++i) {
        for (const auto& j : neighbors_[i]) {
            weight_buffer_[1][i] += weight_buffer_[0][j];
        }
    }
 
    for (i = 0; i < lap_size; ++i) {
        if (inliers_[i] > 0) {
            a_array_.coeffRef(i + lap_size, i) = weight_buffer_[0][i];
        } else {
            a_array_.coeffRef(i + lap_size, i) = 0.0;
        }
        for (const auto& j : neighbors_[i]) {
            // -weight_buffer_[0][j] / weight_buffer_[1][i]
            a_array_.coeffRef(i, j) = -1.0 / neighbors_[i].size();
        }
    }
 
    a_array_.makeCompressed();
    solver_.compute(a_array_.transpose() * a_array_);
    
    for (i = 0; i < lap_size; ++i) {
        if (inliers_[i] > 0) {
            b_array_.coeffRef(i + lap_size, 0) =
                sphe_params_[i] * weight_buffer_[0][i];
            b_array_.coeffRef(i + lap_size, 1) =
                sphe_params_[i + lap_size] * weight_buffer_[0][i];
            b_array_.coeffRef(i + lap_size, 2) =
                sphe_params_[i + lap_size * 2] * weight_buffer_[0][i];
        } else {
            b_array_.coeffRef(i + lap_size, 0) = 0.0; 
            b_array_.coeffRef(i + lap_size, 1) = 0.0; 
            b_array_.coeffRef(i + lap_size, 2) = 0.0; 
        }
    }
    in = a_array_.transpose() * b_array_;
    out = solver_.solve(in);
    for (i = 0; i < lap_size; ++i) {
        sphe_params_[i] = out.coeffRef(i, 0);
        sphe_params_[i + lap_size] = out.coeffRef(i, 1);
        sphe_params_[i + lap_size * 2] = out.coeffRef(i, 2);
    }
}

Pc2Voxel()

void apollo::perception::algorithm::PlaneFitGroundDetector::Pc2Voxel	(	float	x,
		float	y,
		float	z,
		int *	row,
		int *	col
	)

在文件 i_ground.cc 第 1973 行定义.

                                                                                   {
    *row = -1;
    *col = -1;
    // points that are outside the defined BBOX are ignored
    if (x < -param_.roi_region_rad_x || x > param_.roi_region_rad_x ||
        y < -param_.roi_region_rad_y || y > param_.roi_region_rad_y ||
        z < -param_.roi_region_rad_z || z > param_.roi_region_rad_z) {
        return;
    }
    
    float voxel_width_x = IDiv(param_.roi_region_rad_x +
        param_.roi_region_rad_x, static_cast<int>(param_.nr_grids_coarse));
    float voxel_width_y = IDiv(param_.roi_region_rad_y +
        param_.roi_region_rad_y, static_cast<int>(param_.nr_grids_coarse));
    float voxel_width_x_rec = IRec(voxel_width_x);
    float voxel_width_y_rec = IRec(voxel_width_y);
    int grid_size = param_.nr_grids_coarse;
    *col = IMin(grid_size - 1, static_cast<int>(
        (x + param_.roi_region_rad_x) * voxel_width_x_rec));
    *row = IMin(grid_size - 1, static_cast<int>(
        (y + param_.roi_region_rad_y) * voxel_width_y_rec));
}

PlaneRefine()

void apollo::perception::algorithm::PlaneFitGroundDetector::PlaneRefine ( )

protected

在文件 i_ground.cc 第 1759 行定义.

                                         {
    const float *point_cloud = vg_coarse_->const_data();
    unsigned int nr_point_element = vg_coarse_->NrPointElement();
    unsigned int nr_points = vg_coarse_->NrPoints();
    unsigned int r = 0;
    unsigned int c = 0;
    float dist_thre = 0.0;
    float ptp_dist = 0.0;
    int nr_inliers = 0;
    unsigned int i = 0;
    float threshold_rad = 5.0 * Constant<float>::DEGREE_TO_RADIAN();
    for (r = 0; r < param_.nr_grids_coarse; ++r) {
        for (c = 0; c < param_.nr_grids_coarse; ++c) {
            if ((*vg_coarse_)(r, c).Empty()) {
              continue;
            }
            dist_thre = pf_thresholds_[r][c];
            const auto& opt_gd = ground_planes_[r][c];
            GroundPlaneLiDAR fit_gd;
            PlaneFitPointCandIndices &candi = local_candis_[r][c];
            if (candi.Size() < param_.nr_inliers_min_threshold) {
                continue;
            }
            int nr_samples = candi.Prune(param_.nr_samples_min_threshold,
                param_.nr_samples_max_threshold);
 
            float *psrc = pf_threeds_;
            nr_inliers = 0;
            for (i = 0; i < nr_samples; ++i) {
                assert(candi[i] < (int)nr_points);
                ICopy3(point_cloud + (nr_point_element * candi[i]), psrc);
                ptp_dist = IPlaneToPointDistanceWUnitNorm(opt_gd.params, psrc);
                if (ptp_dist < dist_thre) {
                    psrc += dim_point_;
                    nr_inliers++;
                }
            }
            if (nr_inliers < param_.nr_inliers_min_threshold) {
                continue;
            }
            IPlaneFitTotalLeastSquare(pf_threeds_, fit_gd.params, nr_inliers);
            float angle = calculate_two_angles(opt_gd, fit_gd);
            if (angle > threshold_rad) {
                continue;
            }
            ground_planes_[r][c].params[0] = fit_gd.params[0];
            ground_planes_[r][c].params[1] = fit_gd.params[1];
            ground_planes_[r][c].params[2] = fit_gd.params[2];
            ground_planes_[r][c].params[3] = fit_gd.params[3];
        }
    }
}

ResetParams()

void apollo::perception::algorithm::PlaneFitGroundDetector::ResetParams	(	float	ori_z_lower,
		float	ori_z_upper
	)

在文件 i_ground.cc 第 1882 行定义.

                                                                             {
    param_.sample_region_z_lower = ori_z_lower;
    param_.sample_region_z_upper = ori_z_upper;
    AINFO << "[BEFORE]: {} " << param_.sample_region_z_lower << ",{} "
          << param_.sample_region_z_upper;
}

SelectCandidates()

void apollo::perception::algorithm::PlaneFitGroundDetector::SelectCandidates ( )

protected

在文件 i_ground.cc 第 909 行定义.

                                              {
    memset(reinterpret_cast<void *>(labels_), 0,
        vg_fine_->NrPoints() * sizeof(char));
    //  Clear candidate list
    for (size_t i = 0; i < vg_coarse_->NrVoxel(); ++i) {
        local_candis_[0][i].Clear();
    }
    // plane fitting candidates
    const float *point_cloud = vg_fine_->const_data();
    const int* semantic_list = vg_fine_->const_semantic_data();
    unsigned int nr_point_element = vg_fine_->NrPointElement();
    std::stringstream sstr;
    sstr << "[GroundCandidateSelect] " << std::to_string(frame_timestamp_)
         << std::endl;
    for (size_t r = 0; r < param_.nr_grids_fine; ++r) {
        unsigned int begin = (r * param_.nr_grids_fine);
        for (size_t c = 0; c < param_.nr_grids_fine; ++c) {
            int parent = map_fine_to_coarse_[begin + c];
            auto voxel = (*vg_fine_)(r, c);
            //  Clear ground_candidate list
            ground_candidates_[r][c].Clear();
            ground_candidates_[r][c].Resize(voxel.indices_.size(), false);
            if (voxel.indices_.size() < 1) {
                continue;
            }
            SelectGroundCandidate(point_cloud, semantic_list, voxel.indices_,
                &local_candis_[0][parent], &ground_candidates_[r][c],
                nr_point_element);
        }
    }
    // use ground to select again
    for (size_t r = 0; r < param_.nr_grids_fine; ++r) {
        unsigned int begin = (r * param_.nr_grids_fine);
        for (size_t c = 0; c < param_.nr_grids_fine; ++c) {
            auto voxel = (*vg_fine_)(r, c);
            if (param_.debug_output && voxel.indices_.size() > 0) {
                sstr << "row = " << r << " col = " << c
                     << " center_x = " << voxel.v_[0] + voxel.dim_x_ * 0.5f
                     << " center_y = " << voxel.v_[1] + voxel.dim_y_ * 0.5f
                     << " total_indices = " << voxel.indices_.size()
                     << " ground_flag = "
                     << ground_candidates_[r][c].GetGroundFlag()
                     << " ground_count = "
                     << ground_candidates_[r][c].GetGroundCount()
                     << " ground_height = "
                     << ground_candidates_[r][c].GetGroundHeight()
                     << " valid_flag = "
                     << ground_candidates_[r][c].GetValidFlag()
                     << " valid_count = "
                     << ground_candidates_[r][c].GetValidCount()
                     << " valid_height = "
                     << ground_candidates_[r][c].GetValidHeight();
            }
            // Enough Points -> Do PlaneFit
            int valid_c = ground_candidates_[r][c].GetValidCount();
            if (valid_c > param_.nr_z_comp_candis / 2 ||
                valid_c > 0.8 * ground_candidates_[r][c].GetIndicesSize()) {
                if (param_.debug_output) {
                    sstr << " no necessary to get neighbor." << std::endl;
                }
                continue;
            }
            int parent = map_fine_to_coarse_[begin + c];
            int count = NeighborGroundCandidate(r, c, point_cloud,
                semantic_list, voxel.indices_, &local_candis_[0][parent],
                nr_point_element);
            if (param_.debug_output && voxel.indices_.size() > 0) {
                sstr << " after neighbor_count = " << count << std::endl;
            }
        }
    }
    if (param_.debug_output) {
        AINFO << sstr.str();
    }
    sstr.clear();
}

SelectGroundCandidate()

void apollo::perception::algorithm::PlaneFitGroundDetector::SelectGroundCandidate ( const float *  point_cloud, const int *  semantic_list, const std::vector< int > &  indices, PlaneFitPointCandIndices *  candi, GroundCandidateIndices *  ground_candi, unsigned int  nr_point_element  )

protected

在文件 i_ground.cc 第 695 行定义.

                                   {
    const float pi = 3.1415926535898;
    const float *pt_ptr = nullptr;
    const int *semantic_value = nullptr;
    // default delta = 0.022
    float res = param_.roi_region_rad_x * 2 * 1.0f / param_.nr_grids_fine;
    float delta = tan(param_.planefit_orien_threshold * pi / 180.0) * res;
    delta /= 2.0;
    size_t ground_count = 0, valid_count = 0;
    float ground_hei = 0.0, valid_hei = 0.0;
    ground_candi->SetGroundFlag(false);
    ground_candi->SetValidFlag(false);
    // calculate semantic-ground value
    for (size_t i = 0; i < indices.size(); i++) {
        int pos = indices[i];
        pt_ptr = point_cloud + (pos * nr_point_element);
        float z = pt_ptr[2];
        semantic_value = semantic_list + pos;
        int value = semantic_value[0];
        if (value == param_.semantic_ground_value) {
            ground_count++;
            ground_hei += z;
        }
    }
    if (ground_count > 0.3 * indices.size() && ground_count > 0) {
        ground_hei = (ground_hei * 1.0f / ground_count);
        ground_candi->SetGroundFlag(true);
        ground_candi->SetGroundCount(ground_count);
        ground_candi->SetGroundHeight(ground_hei);
        for (size_t i = 0; i < indices.size(); i++) {
            int pos = indices[i];
            pt_ptr = point_cloud + (pos * nr_point_element);
            float z = pt_ptr[2];
            semantic_value = semantic_list + pos;
            int value = semantic_value[0];
            // ground: range is 3 * delta
            // other-semantic: range is 2 * delta
            if (value == param_.semantic_ground_value) {
                if (z > ground_hei - 3 * delta && z < ground_hei + 3 * delta) {
                    labels_[pos] = 1;
                    candi->PushIndex(pos);
                    ground_candi->SetVisitedTrue(i);
                    valid_count++;
                    valid_hei += z;
                    continue;
                }
            } else {
                if (z > ground_hei - 2 * delta && z < ground_hei + 2 * delta) {
                    labels_[pos] = 1;
                    candi->PushIndex(pos);
                    ground_candi->SetVisitedTrue(i);
                    valid_count++;
                    valid_hei += z;
                    continue;
                }
            }
        }
    }
    if (valid_count > 0) {
        valid_hei = (valid_hei * 1.0f / valid_count);
        ground_candi->SetValidFlag(true);
        ground_candi->SetValidCount(valid_count);
        ground_candi->SetValidHeight(valid_hei);
    }
}

SetGroundPlaneSpherParams()

void apollo::perception::algorithm::PlaneFitGroundDetector::SetGroundPlaneSpherParams ( float *  params )

protected

在文件 i_ground.cc 第 1668 行定义.

                                                                    {
    unsigned int lap_size = param_.nr_grids_coarse * param_.nr_grids_coarse;
    unsigned int offset = 0;
    for (unsigned int i = 0; i < lap_size; ++i) {
        ground_planes_sphe_[0][i].theta = params[i + offset];
    }
    offset += lap_size;
    for (unsigned int i = 0; i < lap_size; ++i) {
        ground_planes_sphe_[0][i].phi = params[i + offset];
    }
    offset += lap_size;
    for (unsigned int i = 0; i < lap_size; ++i) {
        ground_planes_sphe_[0][i].d = params[i + offset];
    }
    for (unsigned int r = 0; r < param_.nr_grids_coarse; ++r) {
        for (unsigned int c = 0; c < param_.nr_grids_coarse; ++c) {
            // if ((*vg_coarse_)(r, c).Empty()) {
            //     ground_planes_[r][c].ForceInvalid();
            // } else {
            //     ground_planes_sphe_[r][c].SetNrSupport(1);
            //     IPlaneSpherToEucli(ground_planes_sphe_[r][c],
            //        &ground_planes_[r][c]);
            // }
            IPlaneSpherToEucli(ground_planes_sphe_[r][c], &ground_planes_[r][c]);
        }
    }
}

Smooth()

int apollo::perception::algorithm::PlaneFitGroundDetector::Smooth ( )

protected

在文件 i_ground.cc 第 1649 行定义.

                                   {
  int nr_grids = 0;
  unsigned int r = 0;
  unsigned int c = 0;
  unsigned int nm1 = param_.nr_grids_coarse - 1;
  assert(param_.nr_grids_coarse >= 2);
  nr_grids += SmoothLine(0, 0, 1);
  for (r = 1; r < nm1; ++r) {
    nr_grids += SmoothLine(r - 1, r, r + 1);
  }
  nr_grids += SmoothLine(nm1 - 1, nm1, nm1);
  for (r = 0; r < param_.nr_grids_coarse; ++r) {
    for (c = 0; c < param_.nr_grids_coarse; ++c) {
      IPlaneEucliToSpher(ground_planes_[r][c], &ground_planes_sphe_[r][c]);
    }
  }
  return nr_grids;
}

SmoothGrid()

int apollo::perception::algorithm::PlaneFitGroundDetector::SmoothGrid ( const GroundPlaneSpherical &  g, const GroundPlaneSpherical &  lt, const GroundPlaneSpherical &  rt, const GroundPlaneSpherical &  up, const GroundPlaneSpherical &  dn, GroundPlaneSpherical *  gp  )

protected

在文件 i_ground.cc 第 1589 行定义.

                                                                 {
  int supports[] = {0, 0, 0, 0, 0};
  float weights[] = {0.f, 0.f, 0.f, 0.f, 0.f};
  gp->ForceInvalid();
  if (!g.IsValid()) {
    return 0;
  }
  //  geometry weight:
  //   1
  // 1 4 1
  //   1
  if (lt.GetStatus()) {
    supports[0] = lt.GetNrSupport();
  }
  if (rt.GetStatus()) {
    supports[1] = rt.GetNrSupport();
  }
  if (up.GetStatus()) {
    supports[2] = up.GetNrSupport();
  }
  if (dn.GetStatus()) {
    supports[3] = dn.GetNrSupport();
  }
  supports[4] = (g.GetNrSupport() << 2);
  int support_sum = 0;
  support_sum = ISum4(supports);
  if (support_sum == 0) {
    *gp = g;
    return 0;
  }
  support_sum += supports[4];
  weights[0] =
      static_cast<float>(supports[0]) / static_cast<float>(support_sum);
  weights[1] =
      static_cast<float>(supports[1]) / static_cast<float>(support_sum);
  weights[2] =
      static_cast<float>(supports[2]) / static_cast<float>(support_sum);
  weights[3] =
      static_cast<float>(supports[3]) / static_cast<float>(support_sum);
  weights[4] =
      static_cast<float>(supports[4]) / static_cast<float>(support_sum);
  // weighted average:
  gp->theta = weights[0] * lt.theta + weights[1] * rt.theta +
              weights[2] * up.theta + weights[3] * dn.theta +
              weights[4] * g.theta;
  gp->phi = weights[0] * lt.phi + weights[1] * rt.phi + weights[2] * up.phi +
            weights[3] * dn.phi + weights[4] * g.phi;
  gp->d = weights[0] * lt.d + weights[1] * rt.d + weights[2] * up.d +
          weights[3] * dn.d + weights[4] * g.d;
  // compute weighted average - diveided by 8, round to nearest int
  support_sum = IMax(((support_sum + 2) >> 3), 1);
  gp->SetNrSupport(support_sum);
  return 1;
}

SmoothInMath()

void apollo::perception::algorithm::PlaneFitGroundDetector::SmoothInMath ( )

protected

在文件 i_ground.cc 第 1838 行定义.

                                          {
    GetInliers(inliers_);
    GetGroundPlaneSpherParams(sphe_params_);
    Optimization();
    PlaneRefine();
    SetGroundPlaneSpherParams(sphe_params_);
}

SmoothInOrder()

int apollo::perception::algorithm::PlaneFitGroundDetector::SmoothInOrder ( )

protected

在文件 i_ground.cc 第 1846 行定义.

                                          {
  int nr_grids = 0;
  assert(param_.nr_grids_coarse >= 2);
  GroundPlaneSpherical plane;
  int cols = param_.nr_grids_coarse;
  int rows = param_.nr_grids_coarse;
  // from center to edge
  for (size_t i = 0; i < vg_coarse_->NrVoxel(); i++) {
      int row = order_table_[i].first;
      int col = order_table_[i].second;
      int last_col = IMax(0, col - 1);
      int next_col = IMin(cols - 1, col + 1);
      int last_row = IMax(0, row - 1);
      int next_row = IMin(rows - 1, row + 1);
      if (!ground_planes_sphe_[row][col].IsValid()) {
          nr_grids += CompleteGrid(ground_planes_sphe_[row][last_col],
            ground_planes_sphe_[row][next_col],
            ground_planes_sphe_[last_row][col],
            ground_planes_sphe_[next_row][col], &plane);
      } else {
          nr_grids += SmoothGrid(ground_planes_sphe_[row][col],
          ground_planes_sphe_[row][last_col],
          ground_planes_sphe_[row][next_col],
          ground_planes_sphe_[last_row][col],
          ground_planes_sphe_[next_row][col], &plane);
      }
      IPlaneSpherToEucli(plane, &ground_planes_[row][col]);
  }
  for (unsigned int r = 0; r < param_.nr_grids_coarse; ++r) {
      for (unsigned int c = 0; c < param_.nr_grids_coarse; ++c) {
          IPlaneEucliToSpher(ground_planes_[r][c], &ground_planes_sphe_[r][c]);
     }
  }
  return nr_grids;
}

SmoothLine()

int apollo::perception::algorithm::PlaneFitGroundDetector::SmoothLine ( unsigned int  up, unsigned int  r, unsigned int  dn  )

protected

(*_vg_coarse)(r, 0).empty()

(*_vg_coarse)(r, c).empty()

(*_vg_coarse)(r, nm1).empty()

在文件 i_ground.cc 第 1496 行定义.

                                                        {
  int nr_grids = 0;
  unsigned int c = 0;
  unsigned int nm1 = param_.nr_grids_coarse - 1;
  GroundPlaneSpherical plane;
  assert(param_.nr_grids_coarse >= 2);
  assert(up < param_.nr_grids_coarse);
  assert(r < param_.nr_grids_coarse);
  assert(dn < param_.nr_grids_coarse);
  if ( true) {
    if (!ground_planes_sphe_[r][0].IsValid()) {
      nr_grids += CompleteGrid(
          ground_planes_sphe_[r][0], ground_planes_sphe_[r][1],
          ground_planes_sphe_[up][0], ground_planes_sphe_[dn][0], &plane);
    } else {
      nr_grids +=
          SmoothGrid(ground_planes_sphe_[r][0], ground_planes_sphe_[r][0],
                     ground_planes_sphe_[r][1], ground_planes_sphe_[up][0],
                     ground_planes_sphe_[dn][0], &plane);
    }
    IPlaneSpherToEucli(plane, &ground_planes_[r][0]);
  }
  for (c = 1; c < nm1; ++c) {
    if ( true) {
      if (!ground_planes_sphe_[r][c].IsValid()) {
        nr_grids += CompleteGrid(
            ground_planes_sphe_[r][c - 1], ground_planes_sphe_[r][c + 1],
            ground_planes_sphe_[up][c], ground_planes_sphe_[dn][c], &plane);
      } else {
        nr_grids += SmoothGrid(
            ground_planes_sphe_[r][c], ground_planes_sphe_[r][c - 1],
            ground_planes_sphe_[r][c + 1], ground_planes_sphe_[up][c],
            ground_planes_sphe_[dn][c], &plane);
      }
      IPlaneSpherToEucli(plane, &ground_planes_[r][c]);
    }
  }
  if ( true) {
    if (!ground_planes_sphe_[r][nm1].IsValid()) {
      nr_grids += CompleteGrid(
          ground_planes_sphe_[r][nm1 - 1], ground_planes_sphe_[r][nm1],
          ground_planes_sphe_[up][nm1], ground_planes_sphe_[dn][nm1], &plane);
    } else {
      nr_grids += SmoothGrid(
          ground_planes_sphe_[r][nm1], ground_planes_sphe_[r][nm1 - 1],
          ground_planes_sphe_[r][nm1], ground_planes_sphe_[up][nm1],
          ground_planes_sphe_[dn][nm1], &plane);
    }
    IPlaneSpherToEucli(plane, &ground_planes_[r][nm1]);
  }
 
  return nr_grids;
}

UpdateParams()

void apollo::perception::algorithm::PlaneFitGroundDetector::UpdateParams	(	float	parsing_ground_z,
		float	buffer,
		double	timestamp
	)

在文件 i_ground.cc 第 1889 行定义.

                                                                                                {
    param_.sample_region_z_lower = IMax(param_.sample_region_z_lower,
        parsing_ground_z - buffer);
    param_.sample_region_z_upper = IMin(param_.sample_region_z_upper,
        parsing_ground_z + buffer);
    frame_timestamp_ = timestamp;
    AINFO << "[AFTER]: {} " << param_.sample_region_z_lower << ",{} "
          << param_.sample_region_z_upper;
}

类成员变量说明

a_array_

Eigen::SparseMatrix<float> apollo::perception::algorithm::PlaneFitGroundDetector::a_array_

protected

在文件 i_ground.h 第 454 行定义.

age_

int apollo::perception::algorithm::PlaneFitGroundDetector::age_ = 0

protected

在文件 i_ground.h 第 445 行定义.

b_array_

Eigen::SparseMatrix<float> apollo::perception::algorithm::PlaneFitGroundDetector::b_array_

protected

在文件 i_ground.h 第 455 行定义.

frame_timestamp_

double apollo::perception::algorithm::PlaneFitGroundDetector::frame_timestamp_ = 0.0

protected

在文件 i_ground.h 第 447 行定义.

ground_candidates_

GroundCandidateIndices** apollo::perception::algorithm::PlaneFitGroundDetector::ground_candidates_

protected

在文件 i_ground.h 第 436 行定义.

ground_planes_

GroundPlaneLiDAR** apollo::perception::algorithm::PlaneFitGroundDetector::ground_planes_

protected

在文件 i_ground.h 第 433 行定义.

ground_planes_sphe_

GroundPlaneSpherical** apollo::perception::algorithm::PlaneFitGroundDetector::ground_planes_sphe_

protected

在文件 i_ground.h 第 434 行定义.

ground_z_

std::pair<float, bool>** apollo::perception::algorithm::PlaneFitGroundDetector::ground_z_

protected

在文件 i_ground.h 第 437 行定义.

inliers_

int* apollo::perception::algorithm::PlaneFitGroundDetector::inliers_ = nullptr

protected

在文件 i_ground.h 第 451 行定义.

labels_

char* apollo::perception::algorithm::PlaneFitGroundDetector::labels_

protected

在文件 i_ground.h 第 440 行定义.

local_candis_

PlaneFitPointCandIndices** apollo::perception::algorithm::PlaneFitGroundDetector::local_candis_

protected

在文件 i_ground.h 第 435 行定义.

map_fine_to_coarse_

unsigned int* apollo::perception::algorithm::PlaneFitGroundDetector::map_fine_to_coarse_

protected

在文件 i_ground.h 第 439 行定义.

neighbors_

std::vector<std::vector<unsigned int> > apollo::perception::algorithm::PlaneFitGroundDetector::neighbors_

protected

在文件 i_ground.h 第 453 行定义.

order_table_

std::pair<int, int>* apollo::perception::algorithm::PlaneFitGroundDetector::order_table_

protected

在文件 i_ground.h 第 446 行定义.

pf_threeds_

float* apollo::perception::algorithm::PlaneFitGroundDetector::pf_threeds_

protected

在文件 i_ground.h 第 442 行定义.

pf_thresholds_

float** apollo::perception::algorithm::PlaneFitGroundDetector::pf_thresholds_

protected

在文件 i_ground.h 第 438 行定义.

sampled_indices_

int* apollo::perception::algorithm::PlaneFitGroundDetector::sampled_indices_

protected

在文件 i_ground.h 第 444 行定义.

sampled_z_values_

float* apollo::perception::algorithm::PlaneFitGroundDetector::sampled_z_values_

protected

在文件 i_ground.h 第 441 行定义.

solver_

Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > apollo::perception::algorithm::PlaneFitGroundDetector::solver_

protected

在文件 i_ground.h 第 456 行定义.

sphe_params_

float* apollo::perception::algorithm::PlaneFitGroundDetector::sphe_params_ = nullptr

protected

在文件 i_ground.h 第 452 行定义.

spher_params_

float* apollo::perception::algorithm::PlaneFitGroundDetector::spher_params_

protected

在文件 i_ground.h 第 443 行定义.

vg_coarse_

VoxelGridXY<float>* apollo::perception::algorithm::PlaneFitGroundDetector::vg_coarse_

protected

在文件 i_ground.h 第 432 行定义.

vg_fine_

VoxelGridXY<float>* apollo::perception::algorithm::PlaneFitGroundDetector::vg_fine_

protected

在文件 i_ground.h 第 431 行定义.

weight_buffer_

float** apollo::perception::algorithm::PlaneFitGroundDetector::weight_buffer_ = nullptr

protected

在文件 i_ground.h 第 450 行定义.

---

该类的文档由以下文件生成:

• modules/perception/common/algorithm/i_lib/pc/i_ground.h
• modules/perception/common/algorithm/i_lib/pc/i_ground.cc