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 

編輯：黃飛