點(diǎn)云濾波與匹配進(jìn)階
0. 簡介
之前作者專門為點(diǎn)云匹配寫了幾篇博客，但是我們發(fā)現(xiàn)最近幾年有更多的新方法已經(jīng)在不斷地被使用。
同時之前有些內(nèi)容也沒有很好的概括，所以這里我們將作為一篇進(jìn)階文章來介紹這些方法的使用。
1. 地面點(diǎn)去除
處了使用一些復(fù)雜的方法（FEC）或是一些簡單的方法（根據(jù)高度來濾除）以外，還可以使用Ransac的方法完成平面擬合
void RemovePointsUnderGround(const pcl::PointCloud<pcl::PointXYZI>& cloud_in,pcl::PointCloud<pcl::PointXYZI>& cloud_out){// 對輸入點(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)建一個濾波器對象，用于提取地面平面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軸進(jìn)行濾波pass_through.setFilterLimits(-1.5, 0.5); // 設(shè)置濾波范圍，過濾掉z軸在-1.5到0.5之間的點(diǎn)pass_through.filter(*cloud_filtered);// 創(chuàng)建一個分割對象，用于提取地面平面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)建一個提取對象，用于提取地面點(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對應(yīng)的點(diǎn)extract.filter(*cloud_ground);// 創(chuàng)建一個提取對象，用于提取非地面點(diǎn)pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_non_ground(new pcl::PointCloud<pcl::PointXYZI>);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ù)得到的矩陣中的三個列向量分別對應(yīng)于數(shù)據(jù)的主成分軸。
這些主成分軸是按照數(shù)據(jù)方差的降序排列的，即第一個列向量對應(yīng)的是數(shù)據(jù)的第一主成分軸，第二個列向量對應(yīng)的是數(shù)據(jù)的第二主成分軸，第三個列向量對應(yīng)的是數(shù)據(jù)的第三主成分軸。對于PCA的特征值和特征向量可以從這里理解：
https://blog.csdn.net/lazysnake666/article/details/122404671
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對象pcl::PCA<pcl::PointXYZ> pca;pca.setInputCloud(cloud);// 計算主成分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都存在比較大的主成分// 如果是其他的比如燈桿這種，一般的就會是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// 如果是其他的比如燈桿這種，一般的就會是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ì)講過了，下面就是一個示例代碼
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配準(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)化計算(如果點(diǎn)比較少還可以放棄ESKF，轉(zhuǎn)而用EstimatePlane來估算出平面，并利用點(diǎn)到平面的距離殘差來估算)
int feats_down_size = 0esekfom::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ù)最新估計位姿 增量添加點(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)//這個拿到的是轉(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); //存儲近鄰點(diǎn)的vectorkf.update_iterated_dyn_share_modified(0.001, feats_down_body, ikdtree, Nearest_Points, 4, true);#匹配相關(guān)的內(nèi)容都在里面，核心就是ikdtree.Nearest_Searchstate_point = kf.get_x();pos_lid = state_point.pos + state_point.rot.matrix() * state_point.offset_T_L_I;}
本文僅做學(xué)術(shù)分享，如有侵權(quán)，請聯(lián)系刪文。
—THE END—
評論
圖片
表情