#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, 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)

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_

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 第 263 行定义.

构造及析构函数说明

◆ PlaneFitGroundDetector()

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

explicit

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

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

◆ ~PlaneFitGroundDetector()

apollo::perception::algorithm::PlaneFitGroundDetector::~PlaneFitGroundDetector ( )

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

                                                { 
    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 第 1067 行定义.

                                                   {
  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 第 336 行定义.

                                     {
  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_);
  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 第 356 行定义.

                                                               {
  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 第 1251 行定义.

                                                                   {
  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 第 409 行定义.

                                                      {
  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 第 459 行定义.

                                                            {
  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 第 482 行定义.

                                    {
  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,
		float *	height_above_ground,
		unsigned int	nr_points,
		unsigned int	nr_point_elements
	)

virtual

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

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

                                                                    {
  assert(point_cloud != 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, nr_points, nr_point_elements)) {
    return false;
  }
  // setup the coarse voxel grid
  if (!vg_coarse_->SetS(point_cloud, 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 =
  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 第 669 行定义.

                                   {
  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 第 836 行定义.

                                   {
  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 第 614 行定义.

                                                                      {
  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 第 652 行定义.

                                                     {
  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 第 827 行定义.

                                {
  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 第 686 行定义.

                                                     {
  // 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 第 883 行定义.

                                                                            {
  // 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(0);
 
    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(1);
    }
  }
 
  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(0);
    return 0;
  }
  if (groundplane->GetNrSupport() <
      static_cast<int>(param_.nr_inliers_min_threshold)) {
    groundplane->ForceInvalid();
    groundplane->SetInvalidStatus(1);
    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(2);
    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 第 1089 行定义.

                                       {
  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 第 806 行定义.

                                                  {
  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 第 1713 行定义.

                                                                   {
  return local_candis_;
}

◆ GetGrid()

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

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

                                                                {
  return vg_coarse_;
}

◆ GetGridDimX()

unsigned int apollo::perception::algorithm::PlaneFitGroundDetector::GetGridDimX ( ) const

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

                                                       {
  return vg_coarse_->NrVoxelX();
}

◆ GetGridDimY()

unsigned int apollo::perception::algorithm::PlaneFitGroundDetector::GetGridDimY ( ) const

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

                                                       {
  return vg_coarse_->NrVoxelY();
}

◆ GetGroundPlane()

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

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

                                                                            {
  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 第 1512 行定义.

                                                                    {
    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 第 1396 行定义.

                                                    {
    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 第 1665 行定义.

1665{ 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 第 1176 行定义.

                                             {
  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 第 1709 行定义.

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

◆ Init()

bool apollo::perception::algorithm::PlaneFitGroundDetector::Init ( )

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

                                  {
  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);
    }
  }
  // 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 第 123 行定义.

                                                                      {
  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_;
  }
}

◆ Optimization()

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

protected

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

                                          {
    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 第 1667 行定义.

                                                                                   {
    *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 第 1459 行定义.

                                         {
    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 第 1582 行定义.

                                                                             {
    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;
}

◆ SetGroundPlaneSpherParams()

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

protected

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

                                                                    {
    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 第 1349 行定义.

                                   {
  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 第 1289 行定义.

                                                                 {
  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 第 1538 行定义.

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

◆ SmoothInOrder()

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

protected

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

                                          {
  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 第 1196 行定义.

                                                        {
  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 第 1589 行定义.

                                                                                                {
    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 第 368 行定义.

◆ age_

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

protected

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

◆ b_array_

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

protected

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

◆ frame_timestamp_

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

protected

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

◆ ground_planes_

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

protected

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

◆ ground_planes_sphe_

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

protected

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

◆ ground_z_

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

protected

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

◆ inliers_

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

protected

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

◆ labels_

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

protected

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

◆ local_candis_

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

protected

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

◆ map_fine_to_coarse_

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

protected

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

◆ neighbors_

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

protected

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

◆ order_table_

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

protected

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

◆ pf_threeds_

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

protected

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

◆ pf_thresholds_

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

protected

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

◆ sampled_indices_

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

protected

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

◆ sampled_z_values_

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

protected

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

◆ solver_

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

protected

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

◆ sphe_params_

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

protected

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

◆ spher_params_

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

protected

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

◆ vg_coarse_

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

protected

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

◆ vg_fine_

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

protected

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

◆ weight_buffer_

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

protected

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

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

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

Public 成员函数

Protected 成员函数

Protected 属性

详细描述

构造及析构函数说明

◆ PlaneFitGroundDetector()

◆ ~PlaneFitGroundDetector()

成员函数说明

◆ CalculateAngleDist()

◆ CleanUp()

◆ CompareZ()

◆ CompleteGrid()

◆ ComputeAdaptiveThreshold()

◆ ComputeSignedGroundHeight()

◆ ComputeSignedGroundHeightLine()

◆ Detect()

◆ Filter()

◆ FilterCandidates()

◆ FilterGrid()

◆ FilterLine()

◆ Fit()

◆ FitGrid()

◆ FitGridWithNeighbors()

◆ FitInOrder()

◆ FitLine()

◆ GetCandis()

◆ GetGrid()

◆ GetGridDimX()

◆ GetGridDimY()

◆ GetGroundPlane()

◆ GetGroundPlaneSpherParams()

◆ GetInliers()

◆ GetLabel()

◆ GetNeighbors()

◆ GetUnknownHeight()

◆ Init()

◆ InitOrderTable()

◆ Optimization()

◆ Pc2Voxel()

◆ PlaneRefine()

◆ ResetParams()

◆ SetGroundPlaneSpherParams()

◆ Smooth()

◆ SmoothGrid()

◆ SmoothInMath()

◆ SmoothInOrder()

◆ SmoothLine()

◆ UpdateParams()

类成员变量说明

◆ a_array_

◆ age_

◆ b_array_

◆ frame_timestamp_

◆ ground_planes_

◆ ground_planes_sphe_

◆ ground_z_

◆ inliers_

◆ labels_

◆ local_candis_

◆ map_fine_to_coarse_

◆ neighbors_

◆ order_table_

◆ pf_threeds_

◆ pf_thresholds_

◆ sampled_indices_

◆ sampled_z_values_

◆ solver_

◆ sphe_params_

◆ spher_params_

◆ vg_coarse_

◆ vg_fine_

◆ weight_buffer_