点云滤波与匹配进阶

新机器视觉

共 13227字,需浏览 27分钟

 ·

2023-11-07 12:35


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.h>
void RemovePointsUnderGround(const pcl::PointCloud<pcl::PointXYZI>& cloud_in,                             pcl::PointCloud<pcl::PointXYZI>& cloud_out){    // 对输入点云进行降采样    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZI>);    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;    voxel_grid.setInputCloud(cloud_in.makeShared());    voxel_grid.setLeafSize(0.1f, 0.1f, 0.1f); // 设置体素格大小    voxel_grid.filter(*cloud_downsampled);
   // 创建一个滤波器对象,用于提取地面平面    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);    pcl::PassThrough<pcl::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::SACSegmentation<pcl::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::PointCloud<pcl::PointXYZI>::Ptr cloud_ground(new pcl::PointCloud<pcl::PointXYZI>);    pcl::ExtractIndices<pcl::PointXYZI> extract;    extract.setInputCloud(cloud_filtered);    extract.setIndices(inliers);    extract.setNegative(false); // 提取地面点,即保留inliers对应的点    extract.filter(*cloud_ground);
   // 创建一个提取对象,用于提取非地面点    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_non_ground(new pcl::PointCloud<pcl::PointXYZI>);    extract.setNegative(true); // 提取非地面点,即去除inliers对应的点    extract.filter(*cloud_non_ground);
   // 将结果保存到输出点云中    cloud_out = *cloud_non_ground;}



2. PCA主成分判别


除了去除点云以外,还可以通过主成分判别来判断我们分割的是否是地面。其中eigenvectors()函数得到的矩阵中的三个列向量分别对应于数据的主成分轴。


这些主成分轴是按照数据方差的降序排列的,即第一个列向量对应的是数据的第一主成分轴,第二个列向量对应的是数据的第二主成分轴,第三个列向量对应的是数据的第三主成分轴。对于PCA的特征值和特征向量可以从这里理解:

https://blog.csdn.net/lazysnake666/article/details/122404671


#include <iostream>#include <vector>#include <pcl/point_types.h>#include <pcl/common/pca.h>
bool EstimatePlane(const pcl::PointCloud<pcl::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::PCA<pcl::PointXYZ> pca;    pca.setInputCloud(cloud);
   // 计算主成分    Eigen::Vector3f eigenValues = pca.getEigenValues();    Eigen::Matrix3f eigenVectors = pca.getEigenVectors();
   // 获取地面法向量,因为最小的就是第三列,所以最后一列是地面(0,0,1),如果是墙面那就(x,1-x,0)    Eigen::Vector3f groundNormal = eigenVectors.col(2);#eigen_vector.block<3, 1>(0, 2)//最小成分的主成分向量,对应的是地面的法线,因为地面XY都存在比较大的主成分    // 如果是其他的比如灯杆这种,一般的就会是fabs(eigen_vector.block<3, 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配准这块在之前的博客经典论文阅读之-GICP(ICP大一统)中已经详细讲过了,下面就是一个示例代码


Eigen::Matrix4d gicp_trans(    pcl::PointCloud<PointType>::Ptr source_cloud,    pcl::PointCloud<PointType>::Ptr target_cloud) {  CHECK(source_cloud);  CHECK(target_cloud);
 pcl::GeneralizedIterativeClosestPoint<PointType, 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::PointCloud<PointType>::Ptr aligned_source =      boost::make_shared<pcl::PointCloud<PointType>>();  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();}



4. ikd-Tree配准


ikd-Tree的建图配准离不开eskf的相关内容,相关的代码太多了;所以这里大概整理了一下思路, 即导入地图,然后将当前的点云与GPS结合转到全局坐标系下,然后使用ikdtree完成检索,并传入ESKF中完成优化计算(如果点比较少还可以放弃ESKF,转而用EstimatePlane来估算出平面,并利用点到平面的距离残差来估算)


int feats_down_size = 0
esekfom::esekf kf;state_ikfom state_point;state_point = kf.get_x();state_point.pos = Eigen::Vector3d(init_pos[0], init_pos[1], init_pos[2]);Eigen::Quaterniond q(init_rot[3], init_rot[0], init_rot[1], init_rot[2]);Sophus::SO3 SO3_q(q);state_point.rot = SO3_q;kf.change_x(state_point);// IMU预积分部分(也可以用GPS代替IMU做积分)
//根据最新估计位姿  增量添加点云到mapvoid init_ikdtree(KD_TREE<PointType> ikdtree){    //加载读取点云数据到cloud中    string all_points_dir(string(string(ROOT_DIR) + "PCD/") + "GlobalMap_ikdtree.pcd");    if (pcl::io::loadPCDFile<PointType>(all_points_dir, *cloud) == -1)    {        PCL_ERROR("Read file fail!\n");    }
   ikdtree.set_downsample_param(filter_size_map_min);    ikdtree.Build(cloud->points);    std::cout << "---- ikdtree size: " << ikdtree.size() << std::endl;}
void IkdTreeMapping(pcl::PointCloud<PointType>::Ptr feats_undistort)//这个拿到的是转到全局坐标系下去过噪声的点{  KD_TREE<PointType> ikdtree;  if (ikdtree_.Root_Node == nullptr) {    KD_TREE<PointType> ikdtree;  }  pcl::VoxelGrid<PointType> downSizeFilterSurf;  downSizeFilterSurf.setLeafSize(0.5, 0.5, 0.5);  //点云下采样  downSizeFilterSurf.setInputCloud(feats_undistort);  PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI());  //畸变纠正后降采样的单帧点云,lidar系  downSizeFilterSurf.filter(*feats_down_body);  feats_down_size = feats_down_body->points.size();
 // std::cout << "feats_down_size :" << feats_down_size << std::endl;  if (feats_down_size < 5)  {      ROS_WARN("No point, skip this scan!\n");      return;  }

 /*** iterated state estimation ***/  Nearest_Points.resize(feats_down_size); //存储近邻点的vector  kf.update_iterated_dyn_share_modified(0.001, feats_down_body, ikdtree, Nearest_Points, 4, true);#匹配相关的内容都在里面,核心就是ikdtree.Nearest_Search
 state_point = kf.get_x();  pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I;
}


本文仅做学术分享,如有侵权,请联系删文。

       
       

—THE END—

浏览 184
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报
评论
图片
表情
推荐
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报