點(diǎn)云濾波與匹配進(jìn)階

#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){    // 對(duì)輸入點(diǎn)云進(jìn)行降采樣    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); // 設(shè)置體素格大小    voxel_grid.filter(*cloud_downsampled);
    // 創(chuàng)建一個(gè)濾波器對(duì)象，用于提取地面平面    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"); // 對(duì)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è)分割對(duì)象，用于提取地面平面    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); // 設(shè)置距離閾值，點(diǎn)到平面的距離小于該閾值的點(diǎn)將被認(rèn)為是地面點(diǎn)    segmentation.segment(*inliers, *coefficients);
    // 創(chuàng)建一個(gè)提取對(duì)象，用于提取地面點(diǎn)    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); // 提取地面點(diǎn)，即保留inliers對(duì)應(yīng)的點(diǎn)    extract.filter(*cloud_ground);
    // 創(chuàng)建一個(gè)提取對(duì)象，用于提取非地面點(diǎn)    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_non_ground(new pcl::PointCloud<pcl::PointXYZI>);    extract.setNegative(true); // 提取非地面點(diǎn)，即去除inliers對(duì)應(yīng)的點(diǎn)    extract.filter(*cloud_non_ground);
    // 將結(jié)果保存到輸出點(diǎn)云中    cloud_out = *cloud_non_ground;}

#include <iostream>#include <vector>#include <pcl/point_types.h>#include <pcl/common/pca.h>
bool EstimatePlane(const pcl::PointCloud<pcl::PointXYZI>& 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對(duì)象    pcl::PCA<pcl::PointXYZ> 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)//最小成分的主成分向量，對(duì)應(yīng)的是地面的法線，因?yàn)榈孛鎄Y都存在比較大的主成分    // 如果是其他的比如燈桿這種，一般的就會(huì)是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重要程度滿足要求，因?yàn)榈孛婊镜扔?，所以特征值也很小   https://blog.csdn.net/xinxiangwangzhi_/article/details/118228160    // 如果是其他的比如燈桿這種，一般的就會(huì)是eigen_values(0) > 10 * eigen_values(1)
    return is_ground;}

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();}

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)云到mapvoid init_ikdtree(KD_TREE<PointType> ikdtree){    //加載讀取點(diǎn)云數(shù)據(jù)到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)//這個(gè)拿到的是轉(zhuǎn)到全局坐標(biāo)系下去過噪聲的點(diǎn){  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);  //點(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!\n");      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;
}