点云滤波与匹配进阶干货收藏

0. 简介

之前作者专门为点云匹配写了几篇博客，但是我们发现最近几年有更多的新方法已经在不断地被使用。

同时之前有些内容也没有很好的概括，所以这里我们将作为一篇进阶文章来介绍这些方法的使用。

1. 地面点去除

处了使用一些复杂的方法（FEC）或是一些简单的方法（根据高度来滤除）以外，还可以使用Ransac的方法完成平面拟合

#include 
#include 
#include 
#include 
#include 


void RemovePointsUnderGround(const pcl::PointCloud& cloud_in,
               pcl::PointCloud& cloud_out)
{
  // 对输入点云进行降采样
  pcl::PointCloud::Ptr cloud_downsampled(new pcl::PointCloud);
  pcl::VoxelGrid voxel_grid;
  voxel_grid.setInputCloud(cloud_in.makeShared());
  voxel_grid.setLeafSize(0.1f, 0.1f, 0.1f); // 设置体素格大小
  voxel_grid.filter(*cloud_downsampled);


  // 创建一个滤波器对象，用于提取地面平面
  pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud);
  pcl::PassThrough 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 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::Ptr cloud_ground(new pcl::PointCloud);
  pcl::ExtractIndices extract;
  extract.setInputCloud(cloud_filtered);
  extract.setIndices(inliers);
  extract.setNegative(false); // 提取地面点，即保留inliers对应的点
  extract.filter(*cloud_ground);


  // 创建一个提取对象，用于提取非地面点
  pcl::PointCloud::Ptr cloud_non_ground(new pcl::PointCloud);
  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 
#include 
#include 
#include 


bool EstimatePlane(const pcl::PointCloud& 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 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::UnitZ()))的形式，也就是最大主成分，沿着最大主成分方向


  bool is_ground = (fabs(groundNormal.dot(
               Eigen::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::Ptr source_cloud,
  pcl::PointCloud::Ptr target_cloud) {
 CHECK(source_cloud);
 CHECK(target_cloud);


 pcl::GeneralizedIterativeClosestPoint 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::Ptr aligned_source =
   boost::make_shared>();
 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::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做积分）


//根据最新估计位姿 增量添加点云到map
void init_ikdtree(KD_TREE ikdtree)
{
  //加载读取点云数据到cloud中
  string all_points_dir(string(string(ROOT_DIR) + "PCD/") + "GlobalMap_ikdtree.pcd");
  if (pcl::loadPCDFile(all_points_dir, *cloud) == -1)
  {
    PCL_ERROR("Read file fail!
");
  }


  ikdtree.set_downsample_param(filter_size_map_min);
  ikdtree.Build(cloud->points);
  std::cout << "---- ikdtree size: " << ikdtree.size() << std::endl;
}


void IkdTreeMapping(pcl::PointCloud::Ptr feats_undistort)//这个拿到的是转到全局坐标系下去过噪声的点
{
 KD_TREE ikdtree;
 if (ikdtree_.Root_Node == nullptr) {
  KD_TREE ikdtree;
 }
 pcl::VoxelGrid 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!
");
      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;
bainji :hf 

编辑：黄飞