#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
本文详细介绍了在Struts2框架中如何通过HTML代码控制s:select标签使其只读，同时探讨了这种方法是否适用于s:radio标签，并提供了一个实际应用示例。
扫一扫
224
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
