#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