单位网站建设运维情况,模板王字库下载,大良网站建设价位,西安免费网站搭建制作0. 简介
之前作者专门为点云匹配写了几篇博客#xff0c;但是我们发现最近几年有更多的新方法已经在不断地被使用。同时之前有些内容也没有很好的概括#xff0c;所以这里我们将作为一篇进阶文章来介绍这些方法的使用。
1. 地面点去除
处了使用一些复杂的方法#xff08;…0. 简介
之前作者专门为点云匹配写了几篇博客但是我们发现最近几年有更多的新方法已经在不断地被使用。同时之前有些内容也没有很好的概括所以这里我们将作为一篇进阶文章来介绍这些方法的使用。
1. 地面点去除
处了使用一些复杂的方法FEC或是一些简单的方法根据高度来滤除以外还可以使用Ransac的方法完成平面拟合
#include pcl/point_types.h
#include pcl/filters/extract_indices.h
#include pcl/filters/voxel_grid.h
#include pcl/filters/passthrough.h
#include pcl/segmentation/sac_segmentation.hvoid RemovePointsUnderGround(const pcl::PointCloudpcl::PointXYZI cloud_in,pcl::PointCloudpcl::PointXYZI cloud_out)
{// 对输入点云进行降采样pcl::PointCloudpcl::PointXYZI::Ptr cloud_downsampled(new pcl::PointCloudpcl::PointXYZI);pcl::VoxelGridpcl::PointXYZI voxel_grid;voxel_grid.setInputCloud(cloud_in.makeShared());voxel_grid.setLeafSize(0.1f, 0.1f, 0.1f); // 设置体素格大小voxel_grid.filter(*cloud_downsampled);// 创建一个滤波器对象用于提取地面平面pcl::PointCloudpcl::PointXYZI::Ptr cloud_filtered(new pcl::PointCloudpcl::PointXYZI);pcl::PassThroughpcl::PointXYZI pass_through;pass_through.setInputCloud(cloud_downsampled);pass_through.setFilterFieldName(z); // 对z轴进行滤波pass_through.setFilterLimits(-1.5, 0.5); // 设置滤波范围过滤掉z轴在-1.5到0.5之间的点pass_through.filter(*cloud_filtered);// 创建一个分割对象用于提取地面平面pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);pcl::PointIndices::Ptr inliers(new pcl::PointIndices);pcl::SACSegmentationpcl::PointXYZI segmentation;segmentation.setInputCloud(cloud_filtered);segmentation.setModelType(pcl::SACMODEL_PLANE);segmentation.setMethodType(pcl::SAC_RANSAC);segmentation.setDistanceThreshold(0.1); // 设置距离阈值点到平面的距离小于该阈值的点将被认为是地面点segmentation.segment(*inliers, *coefficients);// 创建一个提取对象用于提取地面点pcl::PointCloudpcl::PointXYZI::Ptr cloud_ground(new pcl::PointCloudpcl::PointXYZI);pcl::ExtractIndicespcl::PointXYZI extract;extract.setInputCloud(cloud_filtered);extract.setIndices(inliers);extract.setNegative(false); // 提取地面点即保留inliers对应的点extract.filter(*cloud_ground);// 创建一个提取对象用于提取非地面点pcl::PointCloudpcl::PointXYZI::Ptr cloud_non_ground(new pcl::PointCloudpcl::PointXYZI);extract.setNegative(true); // 提取非地面点即去除inliers对应的点extract.filter(*cloud_non_ground);// 将结果保存到输出点云中cloud_out *cloud_non_ground;
}2. PCA主成分判别
除了去除点云以外还可以通过主成分判别来判断我们分割的是否是地面。其中eigenvectors()函数得到的矩阵中的三个列向量分别对应于数据的主成分轴。这些主成分轴是按照数据方差的降序排列的即第一个列向量对应的是数据的第一主成分轴第二个列向量对应的是数据的第二主成分轴第三个列向量对应的是数据的第三主成分轴。对于PCA的特征值和特征向量可以从PCA | 特征值和特征向量-CSDN博客这里理解。
#include iostream
#include vector
#include pcl/point_types.h
#include pcl/common/pca.hbool EstimatePlane(const pcl::PointCloudpcl::PointXYZI cloud)
{// 将输入点云数据转换为PCL点云格式for (const auto point : cloud){pcl::PointXYZ pclPoint;pclPoint.x point.x();pclPoint.y point.y();pclPoint.z point.z();cloud-push_back(pclPoint);}// 创建PCA对象pcl::PCApcl::PointXYZ pca;pca.setInputCloud(cloud);// 计算主成分Eigen::Vector3f eigenValues pca.getEigenValues();Eigen::Matrix3f eigenVectors pca.getEigenVectors();// 获取地面法向量因为最小的就是第三列所以最后一列是地面001如果是墙面那就x1-x0Eigen::Vector3f groundNormal eigenVectors.col(2);#eigen_vector.block3, 1(0, 2)//最小成分的主成分向量对应的是地面的法线因为地面XY都存在比较大的主成分// 如果是其他的比如灯杆这种一般的就会是fabs(eigen_vector.block3, 1(0, 0).dot(Eigen::Vector3f::UnitZ()))的形式也就是最大主成分沿着最大主成分方向bool is_ground (fabs(groundNormal.dot(Eigen::Vector3f::UnitZ())) 0.98) (eigenValues(2) 0.05 * 0.05);//最小得列和地面法线重合|a||b|cos,并且eigenValues重要程度满足要求因为地面基本等于0所以特征值也很小 https://blog.csdn.net/xinxiangwangzhi_/article/details/118228160// 如果是其他的比如灯杆这种一般的就会是eigen_values(0) 10 * eigen_values(1)return is_ground;
}3. GICP配准
GICP配准这块在之前的博客经典论文阅读之-GICPICP大一统中已经详细讲过了下面就是一个示例代码
Eigen::Matrix4d gicp_trans(pcl::PointCloudPointType::Ptr source_cloud,pcl::PointCloudPointType::Ptr target_cloud) {CHECK(source_cloud);CHECK(target_cloud);pcl::GeneralizedIterativeClosestPointPointType, PointType gicp;gicp.setInputSource(source_cloud);gicp.setInputTarget(target_cloud);gicp.setMaxCorrespondenceDistance(10.0);gicp.setMaximumIterations(100);gicp.setMaximumOptimizerIterations(100);gicp.setRANSACIterations(100);gicp.setRANSACOutlierRejectionThreshold(1.0);gicp.setTransformationEpsilon(0.01);gicp.setUseReciprocalCorespondences(false);LOG(INFO) MaxCorrespondenceDistance: gicp.getMaxCorrespondenceDistance();LOG(INFO) MaximumIterations: gicp.getMaximumIterations();LOG(INFO) MaximumOptimizerIterations: gicp.getMaximumOptimizerIterations();LOG(INFO) RANSACIterations: gicp.getRANSACIterations();LOG(INFO) RANSACOutlierRejectionThreshold: gicp.getRANSACOutlierRejectionThreshold();LOG(INFO) TransformationEpsilon: gicp.getTransformationEpsilon();LOG(INFO) MaxCorrespondenceDistance: gicp.getMaxCorrespondenceDistance();LOG(INFO) RANSACOutlierRejectionThreshold: gicp.getRANSACOutlierRejectionThreshold();LOG(INFO) UseReciprocalCorrespondences: gicp.getUseReciprocalCorrespondences();pcl::PointCloudPointType::Ptr aligned_source boost::make_sharedpcl::PointCloudPointType();gicp.align(*aligned_source);CHECK(aligned_source);LOG(INFO) Final transformation: std::endl gicp.getFinalTransformation();if (gicp.hasConverged()) {LOG(INFO) GICP converged. std::endl The score is gicp.getFitnessScore();} else {LOG(INFO) GICP did not converge.;}LOG(INFO) Saving aligned source cloud to: params_.aligned_cloud_filename;pcl::io::savePCDFile(params_.aligned_cloud_filename, *aligned_source);return gicp.getFinalTransformation();
}
点击 点云滤波与匹配进阶 - 古月居可查看全文
