hdl_graph_slam (3)地面分割过滤 代码分析
在点云预处理后,与点云配准同时进行的时输入点云的地面分割,分割采用基本的RANSAC算法(随机抽样一致),本篇文章分析一下代码的结构及关键部分。
-
根据传感器位置和设定的高度阈值进行对应高度平面的滤波 plane_clip,仅保留sensor_height±range中间值的点云
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);
下面为plane_clip函数
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; }
-
若使用法线滤波(适用于地面较为平整的情况)则使用表面法线估计过滤掉点云中非垂直法线的点(仅保留法线是垂直的点)
if(use_normal_filtering) { filtered = normal_filtering(filtered); }
法线滤波函数,创建法线估计对象,使用kdtree搜索并计算近邻点法线,滤除掉法线不垂直的点
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; }
-
RANSAC平面参数拟合 直接调用pcl库中的ransacmodelplane 模型进行参数赋值计算即可
// too few points for RANSAC 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); double dot = coeffs.head<3>().dot(reference.head<3>()); 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 if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) { coeffs *= -1.0f; } if(floor_points_pub.getNumSubscribers()) { pcl::PointCloud<PointT>::Ptr inlier_cloud(new pcl::PointCloud<PointT>); pcl::ExtractIndices<PointT> extract; extract.setInputCloud(filtered); extract.setIndices(inliers); extract.filter(*inlier_cloud); inlier_cloud->header = cloud->header; floor_points_pub.publish(inlier_cloud); } return Eigen::Vector4f(coeffs); }
-
小结:地面检测使用RANSAC,pcl自带,平面模型检测添加初步滤波,缩小样本的数量,加速滤波过程。但在滤波过程中只对z轴进行 高度滤波。若地面在某处产生小幅度阶跃,会将不同高度地面拟合为同一平面。可使用分段地面检测进行模型拟合。
-
参考内容:
[1] ransac地面分割
[2] pcl法线估计