# 1. 概述

1. 根据高度参数把地面分割出来，由函数plane_clip完成
2. 把地面点云中，把法向量和地面实际法向量差别较大的点去掉，这一步由函数normal_filtering完成
3. 对剩下的点进行平面参数拟合，这部分代码在函数detect中（前两部对应的函数也是在detect中被调用）

# 2. 分割地面

pcl::PointCloud<PointT>::Ptr plane_clip(const pcl::PointCloud<PointT>::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative) const {
pcl::PlaneClipper3D<PointT> clipper(plane);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);

clipper.clipPointCloud3D(*src_cloud, indices->indices);

pcl::PointCloud<PointT>::Ptr dst_cloud(new pcl::PointCloud<PointT>);

pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(src_cloud);
extract.setIndices(indices);
extract.setNegative(negative);
extract.filter(*dst_cloud);

return dst_cloud;
}


pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);



# 3. 去掉法向量异常的点

pcl::PointCloud<PointT>::Ptr normal_filtering(const pcl::PointCloud<PointT>::Ptr& cloud) const {
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud(cloud);

pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
ne.setSearchMethod(tree);

pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
ne.setKSearch(10);
ne.setViewPoint(0.0f, 0.0f, sensor_height);
ne.compute(*normals);//计算地面点云的法向量

pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
filtered->reserve(cloud->size());
//这段是核心，遍历所有的点，法向量满足要求时这个点才被选取
for (int i = 0; i < cloud->size(); i++) {
float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ());
if (std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) {
filtered->push_back(cloud->at(i));
}
}

filtered->width = filtered->size();
filtered->height = 1;
filtered->is_dense = false;

return filtered;
}


# 4. 拟合平面参数

    //点太少，则直接退出
if(filtered->size() < floor_pts_thresh) {
return boost::none;
}

// RANSAC
pcl::SampleConsensusModelPlane<PointT>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointT>(filtered));
pcl::RandomSampleConsensus<PointT> ransac(model_p);
ransac.setDistanceThreshold(0.1);
ransac.computeModel();

pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
ransac.getInliers(inliers->indices);

// too few inliers
//内点过少，也退出
if(inliers->indices.size() < floor_pts_thresh) {
return boost::none;
}

// verticality check of the detected floor's normal
Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();

Eigen::VectorXf coeffs;
ransac.getModelCoefficients(coeffs);

//法向量不合理，退出
if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) {
// the normal is not vertical
return boost::none;
}

// make the normal upward
// 法向量颠倒个方向
coeffs *= -1.0f;
}



10-24

05-13 1925
12-10 2502
12-07 6032
05-08 2157
05-16 1691
09-21 32