apollo自动驾驶-感知-点云地面检测：pointcloud_ground_detection
功能介绍
该组件是检测打到地面的激光点云，将上一步过滤得到的ROI点云的基础上，再细分为地面点和非地面点两部分。
虽然代码有些难读，其实整个实现过程并不难。以下针对实现的细节做个较为详细的介绍。
1）首先将在感兴趣的距离（80m）范围内等距离划分出粗分（256）方格和细分（32）方格；如下图所示，粗分方格中包含细分方格。
2）将ROI点云根据实际位置分发到不同的方格中，同一个激光点，在粗分方格和细分方格都有对应的位置。为增加点云的处理速度，这里使用了SSE2指令集。
3）地面候选点云过滤。以细分方格为单位进行地面过滤，把细分方格的过滤结果存储在隶属于同一粗分方格的方格内。细分方格地面点云过滤时，满足以下条件的点云可作为地面的候选点：
- 距离范围在激光雷达80m内；
- 低于激光雷达1m到3m的点云，此范围的点云会有分布特性（注意这个条件需要根据激光雷达的高度设置）；
- 体素方格中的点与此分布范围的点高度值相差太大，也不能将该点加入候选点。
4）通过ransac算法，以粗分方格为单位，对每个粗分方格拟合平面。ransac算法主要针对噪声较大的场景，用在这里比较合适。这里分为几小步：
- 获取本粗分体素周围8个相邻粗分体素方格的索引，并滤除体素中的z值超出相邻体素z值的均值的±1.0的点云；对于体素方格中含有较多的候选点的体素方格，需要执行下采样。
- 对每个粗分体素方格，随机抽取3个候选点，求取这3个点决定的平面，再求取候选点到此平面的距离小于设定阈值（位于0.1~0.2）的点的个数。循环执行48次，存储每次的结果，作为候选平面。另外还需要将相邻8个平面的拟合结果也加入候选平面。
- 从这56个候选平面中，选取候选点到此平面的距离小于设定阈值（位于0.1~0.2）的点的个数最多的平面作为最优平面。如果候选点一样，则选择与相邻平面夹角最小的作为拟合结果。
- 针对最优平面附近的候选点（位于0.1~0.2范围），使用最小二乘法，重新拟合成一个平面，最为最终的拟合结果。
- 拟合后的平面需要与相邻平面做平滑处理，针对没有被拟合到的体素方格，需要利用前后左右四个方格做平滑计算，针对被拟合到的体素方格，会连同前后左右四个方格一同做平滑计算。平滑过程用到欧几里得坐标系与球体坐标系之间的转换。
- 针对含有拟合平面的粗分体素方格，求取该体素方格内每个激光点相对于拟合平面的高度；如果粗分体素方格没有拟合平面（候选点太少或拟合失败），则置高度值为一个很大的数。
5）配置文件中设定一个阈值0.25，按照上一步求取的粗分体素方格的每个激光点的高度值，低于该阈值的认为是地面点，否则认为是非地面点。
核心代码解读
若您想了解更多关于此模块的内容，请参考上方与该文章配套的资源，里面含有源码详细的注释和打印，以方便大家理解 。
如何实现将点云分配到不同的体素方格中
以细分体素方格为例，下面的函数为整个实现的过程。为了处理快速，使用了SSE2指令集。求取激光点云在x和y方向的方格索引后，以y为列，x为行展开成一维索引存储在voxels_容器中。
bool VoxelGridXY<T>::SetS(const float *data, unsigned int nr_points, unsigned int nr_point_element) {
if (!data || !nr_points || !nr_point_element || !nr_voxel_x_ || !nr_voxel_y_ || nr_voxel_z_ != 1) {
initialized_ = false;
return initialized_;
}
data_ = data;
nr_points_ = nr_points;
nr_point_element_ = nr_point_element;
// 以细分体素网格为例说明
// nr_voxel_x_和nr_voxel_y_，对应配置文件中的 big_grid_size: 256
// 分隔成256*256个方格
unsigned int nr_voxel = (nr_voxel_x_ * nr_voxel_y_);
unsigned int i, n = 0;
int id;
// 细分步长的倒数
float voxel_width_x_rec = IRec(static_cast<float>(voxel_dim_[0]));
float voxel_width_y_rec = IRec(static_cast<float>(voxel_dim_[1]));
// SSE2（Streaming SIMD Extensions 2）指令集
// (256,0,256,0)
__m128i iv_nr_voxel_x = _mm_setr_epi32(static_cast<int>(nr_voxel_x_), 0, static_cast<int>(nr_voxel_x_), 0);
// 四个值都设置为255
__m128i iv_nr_voxel_x_m1 = _mm_set1_epi32(static_cast<int>(nr_voxel_x_ - 1));
__m128i iv_nr_voxel_y_m1 = _mm_set1_epi32(static_cast<int>(nr_voxel_y_ - 1));
// 四个值都设置为细分步长的倒数
__m128 v_width_x_rec = _mm_set_ps1(voxel_width_x_rec);
__m128 v_width_y_rec = _mm_set_ps1(voxel_width_y_rec);
// 设置感兴趣的距离范围，配置文件中是80m
__m128 v_x_min = _mm_set_ps1(dim_x_[0]);
__m128 v_x_max = _mm_set_ps1(dim_x_[1]);
__m128 v_y_min = _mm_set_ps1(dim_y_[0]);
__m128 v_y_max = _mm_set_ps1(dim_y_[1]);
__m128 v_z_min = _mm_set_ps1(dim_z_[0]);
__m128 v_z_max = _mm_set_ps1(dim_z_[1]);
__m128 v_cmp_x, v_cmp_y, v_cmp_z, v_in_roi;
v_in_roi = _mm_setr_ps(1.0, 1.0, 1.0, 1.0);
__m128 v_xs, v_ys, v_zs;
__m128i iv_indices, iv_x_indices, iv_y_indices, iv_v_indices_02, iv_v_indices_13;
for (i = 0; i < nr_voxel; ++i) {
voxels_[i].indices_.clear();
}
unsigned int nr_loops = (nr_points >> 2);
unsigned int nr_fast_processed = (nr_loops << 2);
// 求未处理的剩余的点云
unsigned int remainder = nr_points - nr_fast_processed;
// d1=3;d2=6;d3=9;d4=12
unsigned int d1 = nr_point_element;
unsigned int d2 = (nr_point_element << 1);
unsigned int d3 = d1 + d2;
unsigned int d4 = (nr_point_element << 2);
// xyz are required to be stored in continuous memory
// 点云数据，分别提取的是xyz坐标
const float *cptr_x = data;
const float *cptr_y = data + 1;
const float *cptr_z = data + 2;
const float *cptr_remainder = data + (nr_fast_processed * nr_point_element);
for (i = 0; i < nr_loops; ++i, n += 4) {
// Set memory
// 提取的是连续4个点x坐标
v_xs = _mm_setr_ps(cptr_x[0], cptr_x[d1], cptr_x[d2], cptr_x[d3]);
// 提取的是连续4个点y坐标
v_ys = _mm_setr_ps(cptr_y[0], cptr_y[d1], cptr_y[d2], cptr_y[d3]);
v_zs = _mm_setr_ps(cptr_z[0], cptr_z[d1], cptr_z[d2], cptr_z[d3]);
// compare range:
// v_in_roi为全1，则证明点在感兴趣的范围内
v_cmp_x = _mm_and_ps(_mm_cmpge_ps(v_xs, v_x_min), _mm_cmple_ps(v_xs, v_x_max));
v_cmp_y = _mm_and_ps(_mm_cmpge_ps(v_ys, v_y_min), _mm_cmple_ps(v_ys, v_y_max));
v_cmp_z = _mm_and_ps(_mm_cmpge_ps(v_zs, v_z_min), _mm_cmple_ps(v_zs, v_z_max));
v_in_roi = _mm_and_ps(_mm_and_ps(v_cmp_x, v_cmp_y), v_cmp_z);
// vector operations, cast into signed integers
// x坐标减去最小值(-80)
v_xs = _mm_sub_ps(v_xs, v_x_min);
// 除以细分步长，可以得到细分方格的索引
v_xs = _mm_mul_ps(v_xs, v_width_x_rec);
// 转换为int类型
iv_x_indices = _mm_cvttps_epi32(v_xs); // truncate towards zero
// 索引值不能大于255，否则等于255
iv_x_indices = _mm_min_epi32(iv_nr_voxel_x_m1, iv_x_indices);
// vector operations, cast into signed integers
v_ys = _mm_sub_ps(v_ys, v_y_min);
v_ys = _mm_mul_ps(v_ys, v_width_y_rec);
iv_y_indices = _mm_cvttps_epi32(v_ys); // truncate towards zero
iv_y_indices = _mm_min_epi32(iv_nr_voxel_y_m1, iv_y_indices);
// 示例：mem_iv_x_indices_:121,121,122,122,mem_iv_y_indices_:125,124,124,123
_mm_store_si128(reinterpret_cast<__m128i *>(mem_iv_x_indices_), iv_x_indices);
_mm_store_si128(reinterpret_cast<__m128i *>(mem_iv_y_indices_), iv_y_indices);
// AWARN << "autodrv-VoxelGridXY<T>::SetS-mem_iv_x_indices_:" << mem_iv_x_indices_[0] << ","
// << mem_iv_x_indices_[1] << "," << mem_iv_x_indices_[2] << "," << mem_iv_x_indices_[3]
// << ",mem_iv_y_indices_:" << mem_iv_y_indices_[0] << "," << mem_iv_y_indices_[1] << ","
// << mem_iv_y_indices_[2] << "," << mem_iv_y_indices_[3];
// y方格索引(125,124,124,123)与(256,0,256,0)乘积得到(32000,0,31744,0)
iv_v_indices_02 = _mm_mullo_epi32(iv_y_indices, iv_nr_voxel_x);
// y方格索引执行位置交换得到(124,125,123,124)
iv_y_indices = _mm_shuffle_epi32(iv_y_indices, _MM_SHUFFLE(2, 3, 0, 1));
_mm_store_si128(reinterpret_cast<__m128i *>(mem_iv_nr_voxel_x_), iv_nr_voxel_x);
_mm_store_si128(reinterpret_cast<__m128i *>(mem_iv_v_indices_02_), iv_v_indices_02);
_mm_store_si128(reinterpret_cast<__m128i *>(mem_iv_y_indices2_), iv_y_indices);
// 示例：mem_iv_nr_voxel_x_:256,0,256,0,mem_iv_v_indices_02_:32000,0,31744,0,mem_iv_y_indices2_:124,125,123,124
// AWARN << "autodrv-VoxelGridXY<T>::SetS-mem_iv_nr_voxel_x_:" << mem_iv_nr_voxel_x_[0] << ","
// << mem_iv_nr_voxel_x_[1] << "," << mem_iv_nr_voxel_x_[2] << "," << mem_iv_nr_voxel_x_[3]
// << ",mem_iv_v_indices_02_:" << mem_iv_v_indices_02_[0] << "," << mem_iv_v_indices_02_[1] << ","
// << mem_iv_v_indices_02_[2] << "," << mem_iv_v_indices_02_[3]
// << ",mem_iv_y_indices2_:" << mem_iv_y_indices2_[0] << "," << mem_iv_y_indices2_[1] << ","
// << mem_iv_y_indices2_[2] << "," << mem_iv_y_indices2_[3];
// 将交换后的y方格索引(124,125,123,124)与(256,0,256,0)乘积得到(31744,0,31488,0)
iv_v_indices_13 = _mm_mullo_epi32(iv_y_indices, iv_nr_voxel_x);
// iv_v_indices_13执行位置交换得到(0,31744,0,31488)
iv_v_indices_13 = _mm_shuffle_epi32(iv_v_indices_13, _MM_SHUFFLE(2, 3, 0, 1));
// 变量相加后得到(32000,31744,31744,31488)
iv_indices = _mm_add_epi32(iv_v_indices_02, iv_v_indices_13);
// (32000,31744,31744,31488)再与x方格索引(121,121,122,122)相加得到(32121,31865,31866,31610)
// 从而得到连续4个激光点云在voxels_中的位置索引
iv_indices = _mm_add_epi32(iv_indices, iv_x_indices);
// store values from registers to memory
// address 16byte-aligned
_mm_store_ps(mem_aligned16_f32_, v_in_roi);
// address 16byte-aligned
_mm_store_si128(reinterpret_cast<__m128i *>(mem_aligned16_i32_), iv_indices);
// 示例：mem_aligned16_i32_:32121,31865,31866,31610
// AWARN << "autodrv-VoxelGridXY<T>::SetS-mem_aligned16_i32_:" << mem_aligned16_i32_[0] << ","
// << mem_aligned16_i32_[1] << "," << mem_aligned16_i32_[2] << "," << mem_aligned16_i32_[3] << ",n:" << n;
// voxels_中存储的是每个细分方格中点云索引的集合，分开来看就是一个0.625*0.625的方格有多少个点云
// voxels_是一个容器，将y行和x列的方格展开成一维后，与voxels_的位置对应
if (mem_aligned16_f32_[0] != 0) {
voxels_[mem_aligned16_i32_[0]].indices_.push_back(n);
}
if (mem_aligned16_f32_[1] != 0) {
voxels_[mem_aligned16_i32_[1]].indices_.push_back(n + 1);
}
if (mem_aligned16_f32_[2] != 0) {
voxels_[mem_aligned16_i32_[2]].indices_.push_back(n + 2);
}
if (mem_aligned16_f32_[3] != 0) {
voxels_[mem_aligned16_i32_[3]].indices_.push_back(n + 3);
}
cptr_x += d4;
cptr_y += d4;
cptr_z += d4;
}
// handling remaining points
// 上面是按照4的倍数处理的点云，有可能还会剩下1、2或3个点未处理，下面是对剩余的点做的处理
for (i = 0; i < remainder; i++, n++) {
id = IAssignPointToVoxel(
cptr_remainder,
dim_x_[0],
dim_x_[1],
dim_y_[0],
dim_y_[1],
dim_z_[0],
dim_z_[1],
voxel_width_x_rec,
voxel_width_y_rec,
static_cast<int>(nr_voxel_x_),
static_cast<int>(nr_voxel_y_));
if (id >= 0) {
voxels_[id].indices_.push_back(n);
}
cptr_remainder += nr_point_element;
}
// for (i = 0; i < nr_voxel; ++i) {
// voxels_[i].indices_.clear();
// }
// for (i = 0; i < nr_points; i++, n++) {
// float x = data_[nr_point_element_ * i];
// float y = data_[nr_point_element_ * i + 1];
// float z = data_[nr_point_element_ * i + 2];
// id = AssignPointToVoxel(x, y, z, dim_x_[0], dim_x_[1], dim_y_[0], dim_y_[1], dim_z_[0], dim_z_[1],
// voxel_width_x_rec, voxel_width_y_rec, static_cast<int>(nr_voxel_x_), static_cast<int>(nr_voxel_y_));
// if (id >= 0) {
// voxels_[id].indices_.push_back(i);
// }
// }
initialized_ = true;
return (initialized_);
}
如何使用ransac算法，实现地平面拟合
以下代码是实现地面检测的核心实现，其中，
- vg_fine_->SetS(point_cloud, nr_points, nr_point_elements)，实现将点云分配的细分体素方格中；
- Filter();以细分体素方格为单位实现了候选点的过滤，把过滤后的候选点存储在隶属于同一粗分方格的方格内；
- FitInOrder();以粗分体素方格为单位，使用ransac算法，实现平面拟合；
- SmoothInOrder();实现了粗分平面间的平滑处理，其中权重的计算与参与拟合的候选点相关；
- ComputeSignedGroundHeight(point_cloud, height_above_ground, nr_points, nr_point_elements);以粗分体素方格为单位，计算体素方格内激光点到拟合平面的高度。
bool PlaneFitGroundDetector::Detect(
const float *point_cloud,
float *height_above_ground,
unsigned int nr_points,
unsigned int nr_point_elements) {
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
// 配置细分的体素网格，其中，
// point_cloud：激光点云数据；nr_points：有效点云个数；nr_point_elements：3
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_
// single_ground_detect: false
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 =
// 通过ransac算法，以粗分方格为单位，拟合地平面
FitInOrder();
// std::cout << "# of valid plane geometry (fitting): " << nr_valid_grid <<
// std::endl;
// Smooth plane using neighborhood information:
// use_math_optimize: false
// 相邻方格之间做平滑处理
if (param_.use_math_optimize) {
SmoothInMath();
} else {
// nr_smooth_iter: 5
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;
}
下面的函数是FitInOrder()中的主体实现，其中应用的就是ransac算法。
- hypothesis[kNr_iter]存储了候选平面。其有2个获得途径，第一个是通过随机采样3个点，算出一个平面，配置文件中设置为48次；第二个是加入周围8个体素方格的平面；会形成总共56后候选平面；
- *groundplane = hypothesis[best];选出了最优的平面作为最终的结果。最优结果的选取有2个条件，第一个是符合高度阈值的内点最多；第二个是如果内点数量相同，则选择与z轴夹角最小的；
- IPlaneFitTotalLeastSquare(pf_threeds_, groundplane->params, nr_inliers);对最优平面的内点，使用最小二乘法，重新求出一个平面作为拟合结果。
int PlaneFitGroundDetector::FitGridWithNeighbors(
int r,
int c,
const float *point_cloud,
GroundPlaneLiDAR *groundplane,
unsigned int nr_points,
unsigned int nr_point_element,
float dist_thre) {
// 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;
// 获取本体素周围8个体素方格的索引
GetNeighbors(r, c, param_.nr_grids_coarse, param_.nr_grids_coarse, &neighbors);
// 如果体素中的点大于周围z的均值的±1.0，则滤除该点
FilterCandidates(r, c, point_cloud, &candi, &neighbors, nr_point_element);
// inliers_min_threshold: 6
if (candi.Size() < param_.nr_inliers_min_threshold) {
return 0;
}
GroundPlaneLiDAR plane;
// nr_ransac_iter_threshold = 48;
// 48+8
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};
// nr_samples_min_threshold = 256;nr_samples_max_threshold = 1024;
// 对候选点做修剪，随机减去一些候选点
int nr_samples = candi.Prune(param_.nr_samples_min_threshold, param_.nr_samples_max_threshold);
// termi_inlier_percen_threshold = 0.99f;
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;
// 将点云重新赋值给pf_threeds_
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
// 拟合平面48次
for (int i = 0; i < param_.nr_ransac_iter_threshold; ++i) {
// 在[0,nr_samples)范围随机采样3个索引点，存储在indices_trial中
IRandomSample(indices_trial, 3, nr_samples, &rseed);
IScale3(indices_trial, dim_point_);
// 把采样到的点(x,y,z)赋值给samples
ICopy3(pf_threeds_ + indices_trial[0], samples);
ICopy3(pf_threeds_ + indices_trial[1], samples + 3);
ICopy3(pf_threeds_ + indices_trial[2], samples + 6);
// 三个点决定一个平面，一个平面由4个参数组成，从而求出候选平面
IPlaneFitDestroyed(samples, hypothesis[i].params);
// check if the plane hypothesis has valid geometry
// 求解候选平面与z轴的夹角，如果夹角大于planefit_orien_threshold: 5.0度，则忽略
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);
// 该距离小于该粗分体素方格的设定阈值（位于0.1~0.2）之间
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;
}
// inliers_min_threshold: 6
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
// 用最小二乘法，将pf_threeds_重新拟合一套平面参数，存储在groundplane->params
IPlaneFitTotalLeastSquare(pf_threeds_, groundplane->params, nr_inliers);
if (angle_best <= CalculateAngleDist(*groundplane, neighbors)) {
*groundplane = hypothesis[best];
groundplane->SetStatus(true);
}
// inliers_min_threshold: 6
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;
}
效果展示
过滤结果展示
下图为不同角度的三维展示效果，其中红色部分为地面点，蓝色部分为非地面点，从中可以看出，符合实际情况。
扫一扫
375
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
