0. 簡介

之前作者專門為點(diǎn)云匹配寫了幾篇博客，但是我們發(fā)現(xiàn)最近幾年有更多的新方法已經(jīng)在不斷地被使用。

同時(shí)之前有些內(nèi)容也沒有很好的概括，所以這里我們將作為一篇進(jìn)階文章來介紹這些方法的使用。

1. 地面點(diǎn)去除

處了使用一些復(fù)雜的方法（FEC）或是一些簡單的方法（根據(jù)高度來濾除）以外，還可以使用Ransac的方法完成平面擬合

#include 
#include 
#include 
#include 
#include 


void RemovePointsUnderGround(const pcl::PointCloud& cloud_in,
               pcl::PointCloud& cloud_out)
{
  // 對輸入點(diǎn)云進(jìn)行降采樣
  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); // 設(shè)置體素格大小
  voxel_grid.filter(*cloud_downsampled);


  // 創(chuàng)建一個(gè)濾波器對象，用于提取地面平面
  pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud);
  pcl::PassThrough pass_through;
  pass_through.setInputCloud(cloud_downsampled);
  pass_through.setFilterFieldName("z"); // 對z軸進(jìn)行濾波
  pass_through.setFilterLimits(-1.5, 0.5); // 設(shè)置濾波范圍，過濾掉z軸在-1.5到0.5之間的點(diǎn)
  pass_through.filter(*cloud_filtered);


  // 創(chuàng)建一個(gè)分割對象，用于提取地面平面
  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); // 設(shè)置距離閾值，點(diǎn)到平面的距離小于該閾值的點(diǎn)將被認(rèn)為是地面點(diǎn)
  segmentation.segment(*inliers, *coefficients);


  // 創(chuàng)建一個(gè)提取對象，用于提取地面點(diǎn)
  pcl::PointCloud::Ptr cloud_ground(new pcl::PointCloud);
  pcl::ExtractIndices extract;
  extract.setInputCloud(cloud_filtered);
  extract.setIndices(inliers);
  extract.setNegative(false); // 提取地面點(diǎn)，即保留inliers對應(yīng)的點(diǎn)
  extract.filter(*cloud_ground);


  // 創(chuàng)建一個(gè)提取對象，用于提取非地面點(diǎn)
  pcl::PointCloud::Ptr cloud_non_ground(new pcl::PointCloud);
  extract.setNegative(true); // 提取非地面點(diǎn)，即去除inliers對應(yīng)的點(diǎn)
  extract.filter(*cloud_non_ground);


  // 將結(jié)果保存到輸出點(diǎn)云中
  cloud_out = *cloud_non_ground;
}

2. PCA主成分判別

除了去除點(diǎn)云以外，還可以通過主成分判別來判斷我們分割的是否是地面。其中eigenvectors()函數(shù)得到的矩陣中的三個(gè)列向量分別對應(yīng)于數(shù)據(jù)的主成分軸。

這些主成分軸是按照數(shù)據(jù)方差的降序排列的，即第一個(gè)列向量對應(yīng)的是數(shù)據(jù)的第一主成分軸，第二個(gè)列向量對應(yīng)的是數(shù)據(jù)的第二主成分軸，第三個(gè)列向量對應(yīng)的是數(shù)據(jù)的第三主成分軸。對于PCA的特征值和特征向量可以從這里理解：

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

#include 
#include 
#include 
#include 


bool EstimatePlane(const pcl::PointCloud& cloud)
{


  // 將輸入點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為PCL點(diǎn)云格式
  for (const auto& point : cloud)
  {
    pcl::PointXYZ pclPoint;
    pclPoint.x = point.x();
    pclPoint.y = point.y();
    pclPoint.z = point.z();
    cloud->push_back(pclPoint);
  }


  // 創(chuàng)建PCA對象
  pcl::PCA pca;
  pca.setInputCloud(cloud);


  // 計(jì)算主成分
  Eigen::Vector3f eigenValues = pca.getEigenValues();
  Eigen::Matrix3f eigenVectors = pca.getEigenVectors();


  // 獲取地面法向量，因?yàn)樽钚〉木褪堑谌?，所以最后一列是地面?，0，1），如果是墻面那就（x，1-x，0）
  Eigen::Vector3f groundNormal = eigenVectors.col(2);#eigen_vector.block<3, 1>(0, 2)//最小成分的主成分向量，對應(yīng)的是地面的法線，因?yàn)榈孛鎄Y都存在比較大的主成分
  // 如果是其他的比如燈桿這種，一般的就會(huì)是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重要程度滿足要求，因?yàn)榈孛婊镜扔?，所以特征值也很小 ? https://blog.csdn.net/xinxiangwangzhi_/article/details/118228160
 ? ?// 如果是其他的比如燈桿這種，一般的就會(huì)是eigen_values(0) > 10 * eigen_values(1)


  return is_ground;
}

3. GICP配準(zhǔn)

GICP配準(zhǔn)這塊在之前的博客經(jīng)典論文閱讀之-GICP（ICP大一統(tǒng)）中已經(jīng)詳細(xì)講過了，下面就是一個(gè)示例代碼

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配準(zhǔn)

ikd-Tree的建圖配準(zhǔn)離不開eskf的相關(guān)內(nèi)容，相關(guān)的代碼太多了；所以這里大概整理了一下思路， 即導(dǎo)入地圖，然后將當(dāng)前的點(diǎn)云與GPS結(jié)合轉(zhuǎn)到全局坐標(biāo)系下，然后使用ikdtree完成檢索，并傳入ESKF中完成優(yōu)化計(jì)算(如果點(diǎn)比較少還可以放棄ESKF，轉(zhuǎn)而用EstimatePlane來估算出平面，并利用點(diǎn)到平面的距離殘差來估算)

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預(yù)積分部分（也可以用GPS代替IMU做積分）


//根據(jù)最新估計(jì)位姿 增量添加點(diǎn)云到map
void init_ikdtree(KD_TREE ikdtree)
{
  //加載讀取點(diǎn)云數(shù)據(jù)到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)//這個(gè)拿到的是轉(zhuǎn)到全局坐標(biāo)系下去過噪聲的點(diǎn)
{
 KD_TREE ikdtree;
 if (ikdtree_.Root_Node == nullptr) {
  KD_TREE ikdtree;
 }
 pcl::VoxelGrid downSizeFilterSurf;
 downSizeFilterSurf.setLeafSize(0.5, 0.5, 0.5);
 //點(diǎn)云下采樣
 downSizeFilterSurf.setInputCloud(feats_undistort);
 PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); //畸變糾正后降采樣的單幀點(diǎn)云，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); //存儲(chǔ)近鄰點(diǎn)的vector
 ?kf.update_iterated_dyn_share_modified(0.001, feats_down_body, ikdtree, Nearest_Points, 4, true);#匹配相關(guān)的內(nèi)容都在里面，核心就是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 

編輯：黃飛