PCL:通过特征描述子获取对应关系

34 篇文章 ¥59.90 ¥99.00
本文介绍了如何利用Point Cloud Library(PCL)进行点云处理,包括导入PCL库,加载点云数据,提取如SHOT描述子等特征描述子,并通过匹配算法如ICP建立点云之间的对应关系。

在计算机视觉领域中,点云是一种常见的数据形式,用于表示三维空间中的点集。Point Cloud Library(PCL)是一个广泛使用的开源库,提供了许多用于点云处理和分析的算法和工具。在点云匹配和对应关系建立的任务中,特征描述子是一种常用的工具,用于描述点云中的特征信息。本文将介绍如何使用PCL库提取特征描述子,并建立对应关系。

  1. 导入必要的库和模块

首先,我们需要导入PCL库以及其他必要的模块。以下是一个示例导入语句:

import pcl
from pcl import PointCloud
from pcl import IterativeClosestPoint
  1. 加载点云数据

接下来,我们需要加载点云数据。可以使用PCL库提供的pcl.load函数从文件中加载点云数据,也可以通过其他方式获取点云数据。以下是一个示例加载点云数据的代码:

cloud = pcl
#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/features/esf.h> // esf特征头文件 #include <pcl\common\common.h> #include <pcl/common/angles.h> #include <pcl/common/centroid.h> #include <pcl/registration/icp.h> // icp算法 #include <pcl/registration/registration.h> #include <pcl/registration/correspondence_rejection_one_to_one.h> // 一对一删除重复点对 #include <pcl/common/file_io.h> // 批量处理头文件 #include <pcl/console/time.h> // 利用控制台计算时间 #include <pcl/common/distances.h> // 扇形分区 std::pair <std::vector<pcl::PointCloud<pcl::PointXYZI>>, std::vector<pcl::PointCloud<pcl::PointXYZI>>> sectorPartition(pcl::PointCloud<pcl::PointXYZI>::Ptr& source, pcl::PointCloud<pcl::PointXYZI>::Ptr& target, int sectorNum = 12) { // 计算质心 Eigen::Vector4f centroid; // 质心 pcl::compute3DCentroid(*source, centroid); // 齐次坐标,(c0,c1,c2,1) float Xo = centroid[0]; float Zo = centroid[2]; float Xi = 0.0; float Zi = 0.0; float rad = 0.0; // 点到质心的弧度 std::vector<float>sourceDeg(source->size(), -1.0); // 遍历源点云 for (size_t i = 0; i < source->size(); ++i) { Xi = source->points[i].x; Zi = source->points[i].z; // 点到质心的弧度 rad = atan2(Xi - Xo, Zi - Zo); if (rad < 0) { sourceDeg[i] = pcl::rad2deg(rad) + 360; } else { sourceDeg[i] = pcl::rad2deg(rad); } } std::vector<float>targetDeg(target->size(), -1.0); // 遍历目标点云 for (size_t i = 0; i < target->size(); ++i) { Xi = target->points[i].x; Zi = target->points[i].z; // 点到质心的弧度 rad = atan2(Xi - Xo, Zi - Zo); if (rad < 0) { targetDeg[i] = pcl::rad2deg(rad) + 360; } else { targetDeg[i] = pcl::rad2deg(rad); } } float sectorSize = 360 / (1.0 * sectorNum); // 存储分区点的容器 std::vector<pcl::PointCloud<pcl::PointXYZI>> sourceInSector(sectorNum); std::vector<pcl::PointCloud<pcl::PointXYZI>> targetInSector(sectorNum); // 计算源点云任一点所在的分区 for (int i = 0; i < source->size(); ++i) { int sectorID = std::floor(sourceDeg[i] / sectorSize); sourceInSector[sectorID].push_back(source->points[i]); } // 计算目标点云任一点所在的分区 for (int i = 0; i < target->size(); ++i) { int sectorID = std::floor(targetDeg[i] / sectorSize); targetInSector[sectorID].push_back(target->points[i]); } std::pair <std::vector<pcl::PointCloud<pcl::PointXYZI>>, std::vector<pcl::PointCloud<pcl::PointXYZI>>> sectorLabel(sourceInSector, targetInSector); return sectorLabel; } // 计算断面扇形分区配准偏差 void calCrossRegistrationDeviation(pcl::PointCloud<pcl::PointXYZI>::Ptr& baseCloud, pcl::PointCloud<pcl::PointXYZI>::Ptr& computedCloud, double& SD) { pcl::PointCloud<pcl::PointXYZI>::Ptr cloudSizeLarge(new pcl::PointCloud<pcl::PointXYZI>); pcl::PointCloud<pcl::PointXYZI>::Ptr cloudSizeSmall(new pcl::PointCloud<pcl::PointXYZI>); if (baseCloud->points.size() > computedCloud->points.size()) { cloudSizeLarge = baseCloud; cloudSizeSmall = computedCloud; } else { cloudSizeLarge = computedCloud; cloudSizeSmall = baseCloud; } std::vector<double> deviation; // 最临近点距离传递至强度 pcl::KdTreeFLANN<pcl::PointXYZI> kdtree; kdtree.setInputCloud(cloudSizeLarge); //采样后根据最邻近点提取的样本点下标索引 pcl::PointIndicesPtr inds = std::shared_ptr<pcl::PointIndices>(new pcl::PointIndices()); for (size_t i = 0; i < cloudSizeSmall->points.size(); i++) { pcl::PointXYZI searchPoint; searchPoint.x = cloudSizeSmall->points[i].x; searchPoint.y = cloudSizeSmall->points[i].y; searchPoint.z = cloudSizeSmall->points[i].z; searchPoint.intensity = cloudSizeSmall->points[i].intensity; int K = 1;// 最近邻搜索 std::vector<int> pointIdxNKNSearch(K); std::vector<float> pointNKNSquaredDistance(K); if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { if (pointNKNSquaredDistance[0] < 0.01) { deviation.push_back(pointNKNSquaredDistance[0]); } } } // 求和 double sum = 0; for (double value : deviation) { sum += value; } // 计算均值 double mean = sum / deviation.size(); // 计算方差 double squaredDiffSum = 0.0; for (double value : deviation) { squaredDiffSum += std::pow(value - mean, 2); } double variance = squaredDiffSum / (deviation.size() - 1); // 计算标准差 if (deviation.size() == 0) { SD = -1; } else { SD = std::sqrt(variance); } } // 计算最终配准误差 void calFinalRegistrationRMSE(pcl::PointCloud<pcl::PointXYZI>::Ptr& baseCloud, pcl::PointCloud<pcl::PointXYZI>::Ptr& computedCloud, double& RMSE) { // 获取匹配点对 pcl::registration::CorrespondenceEstimation<pcl::PointXYZI, pcl::PointXYZI>core; core.setInputSource(baseCloud); core.setInputTarget(computedCloud); pcl::Correspondences corres; core.determineCorrespondences(corres, 0.012);// 确定输入点云与目标点云之间的距离 12 mm // 一对一删除重复点对 pcl::CorrespondencesPtr correspondencesRejOneToOne(new pcl::Correspondences); pcl::registration::CorrespondenceRejectorOneToOne corrRejOneToOne; corrRejOneToOne.getRemainingCorrespondences(corres, corres);// 一对一删除重复点对 float sum = 0.0; for (size_t i = 0; i < corres.size(); ++i) { sum += corres[i].distance; } if (corres.size() == 0) { RMSE = -1.0; } else { RMSE = sqrt(sum / corres.size()); // 均方根误差; } } // ICP配准 void ICP(pcl::PointCloud<pcl::PointXYZI>::Ptr& oldSection, pcl::PointCloud<pcl::PointXYZI>::Ptr& baseSection, pcl::PointCloud<pcl::PointXYZI>::Ptr& newSection, int maximumIterateTime, float maxCorrespondenceDistance, float registrationAccuracy, Eigen::Matrix4f& transform, double& SD) { // ICP精配准 pcl::PointCloud<pcl::PointXYZI>::Ptr ICPCrossIn = oldSection; // 初始化ICP对象 pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp; // KD树加速搜索 pcl::search::KdTree<pcl::PointXYZI>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZI>); pcl::search::KdTree<pcl::PointXYZI>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZI>); tree1->setInputCloud(ICPCrossIn); tree2->setInputCloud(baseSection); icp.setSearchMethodSource(tree1); icp.setSearchMethodTarget(tree2); icp.setInputSource(ICPCrossIn); // 源点云 icp.setInputTarget(baseSection); // 目标点云 icp.setTransformationEpsilon(1e-10); // 为终止条件设置最小转换差异 icp.setMaxCorrespondenceDistance(maxCorrespondenceDistance); // 设置对应点对之间的最大距离 icp.setMaximumIterations(maximumIterateTime); // 最大迭代次数 // 设置收敛条件是均方误差和小于阈值, 停止迭代; icp.setEuclideanFitnessEpsilon(registrationAccuracy); //使用相互对应关系 icp.setUseReciprocalCorrespondences(true); pcl::PointCloud<pcl::PointXYZI>::Ptr ICPcloud(new pcl::PointCloud<pcl::PointXYZI>); icp.align(*ICPcloud); // ICP依次转换对象 transform = icp.getFinalTransformation(); pcl::transformPointCloud(*ICPCrossIn, *ICPcloud, transform); newSection = ICPcloud; calCrossRegistrationDeviation(newSection, baseSection, SD); } // 计算ESF特征 pcl::PointCloud<pcl::ESFSignature640>::Ptr ESFEstimation(pcl::PointCloud<pcl::PointXYZI>::Ptr& object) { // Object for storing the ESF descriptor. pcl::PointCloud<pcl::ESFSignature640>::Ptr descriptor(new pcl::PointCloud<pcl::ESFSignature640>); // ESF estimation object. pcl::ESFEstimation<pcl::PointXYZI, pcl::ESFSignature640> esf; esf.setInputCloud(object); //esf.setKSearch(10); esf.compute(*descriptor); return descriptor; } // 分区加权ICP配准 pcl::PointCloud<pcl::PointXYZI>::Ptr blockICP(pcl::PointCloud<pcl::PointXYZI>::Ptr& source, pcl::PointCloud<pcl::PointXYZI>::Ptr& target, float& maxCorrespondenceDistance, int& sectorNum, double& frmse) { // 扇形分区 auto sectorResults = sectorPartition(source, target, sectorNum); // 分区配准 std::vector<Eigen::Matrix4f>RotaionGroup; std::map<int, pcl::PointCloud<pcl::PointXYZI>::Ptr> newSectionICP; std::vector<double>weight; for (int i = 0; i < sectorNum; i++) { if (sectorResults.first[i].empty() || sectorResults.second[i].empty()) { continue; } // 计算ESF特征描述 auto sourceFeature = ESFEstimation(sectorResults.first[i].makeShared()); auto targetFeature = ESFEstimation(sectorResults.second[i].makeShared()); float sumAB = 0.0; float sumAA = 0.0; float sumBB = 0.0; for (int k = 0; k < 640; ++k) { float Vik = sourceFeature->points[0].histogram[k]; float Vjk = targetFeature->points[0].histogram[k]; sumAB += Vik * Vjk; sumAA += Vik * Vik; sumBB += Vjk * Vjk; } // 计算余弦相似度 float sim = sumAB / static_cast<float>(sqrt(sumAA) * sqrt(sumBB)); if (sim < 0.9) { //std::cout << "第" << i << "个分区的匹配点对不够" << std::endl; continue; } // 配准 Eigen::Matrix4f Rotation; double SD = 0.0; ICP(sectorResults.first[i].makeShared(), sectorResults.second[i].makeShared(), newSectionICP[i], 100, maxCorrespondenceDistance, 0.001, Rotation, SD); if (SD <= 0) { //std::cout << "第" << i << "个扇区配准失败" << std::endl; continue; } RotaionGroup.push_back(Rotation); weight.push_back(1.0 / SD); } // 加权获得最后的变换矩阵 Eigen::Matrix4f bestRotaion = Eigen::Matrix4f::Zero(4, 4); double weightSum = 0; for (int i = 0; i < weight.size(); i++) { weightSum += weight[i]; } for (int i = 0; i < weight.size(); i++) { bestRotaion += (weight[i] / weightSum) * RotaionGroup[i]; } pcl::PointCloud<pcl::PointXYZI>::Ptr registResult(new pcl::PointCloud<pcl::PointXYZI>); pcl::transformPointCloud(*source, *registResult, bestRotaion); // 计算最终配准误差 calFinalRegistrationRMSE(registResult, target, frmse); return registResult; } int main() { pcl::console::TicToc time; pcl::PointCloud<pcl::PointXYZI>::Ptr source(new pcl::PointCloud<pcl::PointXYZI>); pcl::io::loadPCDFile<pcl::PointXYZI>("WW//228//source_slice_228.pcd", *source); pcl::PointCloud<pcl::PointXYZI>::Ptr target(new pcl::PointCloud<pcl::PointXYZI>); pcl::io::loadPCDFile<pcl::PointXYZI>("WW//228//target_slice_228.pcd", *target); time.tic(); float maxCorrespondenceDistance = 0.8; // 匹配点对之间的最大距离 int sectorNum = 1; // 分区个数 double frmse = 0.0; // 配准误差 int maxSectorNum = 6; // 最大分区个数 float m_errorTh = 0.00001; // 最小转换差异 double errorMin = FLT_MAX; int bestSegNum = 1; float error = std::numeric_limits<float>::infinity(); pcl::PointCloud<pcl::PointXYZI>::Ptr registResult(new pcl::PointCloud<pcl::PointXYZI>); while (error > m_errorTh && sectorNum <= maxSectorNum) { float lastError = error; registResult = blockICP(source, target, maxCorrespondenceDistance, sectorNum, frmse); //std::cout << "分区为" << sectorNum << "时的配准误差为;" << frmse << std::endl; error = frmse; if (frmse <= 0) { break; } if (error < errorMin) { errorMin = error; bestSegNum = sectorNum; } sectorNum++; // 前后两次迭代的误差小于1e-4,则停止迭代 if ((error <= lastError) && (lastError - error < m_errorTh)) { break; } } std::cout << "最佳分块为:" << bestSegNum << std::endl; registResult = blockICP(source, target, maxCorrespondenceDistance, bestSegNum, frmse); std::cout << "算法耗时:" << time.toc() << " ms" << std::endl; std::cout << "最佳分块配准误差为:" << frmse << std::endl; pcl::io::savePCDFileBinary("WW//result//source_slice_1427reg.pcd", *registResult); return 0; } 代码封装成类,.h和.cpp分开保存,命名为:BlockWeightedRegistration
08-29
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值