一 点云拼接流程
二 基于特征描述子的点云配准实验
2.1 第一组实验：源点云与目标点云完全相同，位姿不同；
输出信息：
VoxelGrid_Filter has finished in 0 s
VoxelGrid_Filter has finished in 0 s
resolution : 0.33944
computeResolution has finished in 0 s
computeSurfaceNormals() has finished in 0 s
computeSurfaceNormals() has finished in 0 s
computeFeatures_FPFH has finished in 3 s
computeFeatures_FPFH has finished in 3 s
--------------- SAC-IA ------------------
calculate time is: 5.83s
SAC has converged:1 score: 2.22288
0.918413 -0.379178 0.112884 25.9688
0.35343 0.914572 0.196583 32.3238
-0.17778 -0.140648 0.973967 15.8231
0 0 0 1
----------------- ICP -----------
ICP, FitnessScore: 0.0294127
0.808435 -0.588591 0.000618075 25.9931
0.588591 0.808434 -0.00204244 29.9981
0.000702663 0.0020145 1 15.0284
0 0 0 1
配准后效果图（绿色：源点云，红色：目标点云，蓝色：配准点云）：
可以发现，红色点云与蓝色点云完全重合，配准效果很好。
2.2 第二组实验：源点云（45°）与目标点云（0°）存在较大部分重合；
输出信息：
D:PCLDatabunny6bun000.pcd
D:PCLDatabunny6bun045.pcd
resolution : 0.000583691
computeResolution has finished in 1 s
VoxelGrid_Filter, Input points: 40256; Output points: 3648
VoxelGrid_Filter has finished in 0 s
VoxelGrid_Filter, Input points: 40097; Output points: 3523
VoxelGrid_Filter has finished in 0 s
resolution : 0.00194114
computeResolution has finished in 0 s
computeSurfaceNormals() has finished in 0 s
computeSurfaceNormals() has finished in 0 s
computeFeatures_FPFH has finished in 3 s
computeFeatures_FPFH has finished in 3 s
--------------- SAC-IA ------------------
calculate time is: 7.877s
SAC has converged:1 score: 0.000152522
0.837255 0.38372 0.389567 -0.0998685
-0.34413 0.923413 -0.169951 0.0126603
-0.424945 0.0082306 0.905182 -0.00992364
0 0 0 1
----------------- ICP -----------
ICP, FitnessScore: 9.50956e-06
0.861307 -0.00769163 0.508033 -0.0513097
0.01093 0.999939 -0.00338782 1.89042e-05
-0.507974 0.00847142 0.861337 -0.0132623
0 0 0 1
配准后效果图（绿色：源点云，红色：目标点云，蓝色：配准点云）：
可以发现，红色点云与蓝色点云几乎完全重合，配准效果很好。
2.3 第三组实验：源点云（90°）与目标点云（45°）存在少部分重合；
配准后效果图（绿色：源点云，红色：目标点云，蓝色：配准点云）：
可以发现，红色点云与蓝色点云重合度很低，配准效果很差。
针对此问题，将源点云与目标点云中具有相同特征的点云部分分割出来，来计算变换矩阵；
分割出相同特征的点云（兔头）配准如下：
可以发现，红色点云与蓝色点云重合度较高，配准效果较好。
计算兔头的配准后转换矩阵，对源点云和目标点云进行配准。
可以发现，红色点云与蓝色点云配准效果改善很多，但也存在一定误差。
由以上实验可以得出：
1）重合点云的数量越多，配准越好。
2）配准比较耗时，特别是粗配准的SAC耗时较长；
四 源代码简介
4.1 头文件
代码语言：javascript复制br#pragma once
#include "typeDef.h"
extern "C"
{
//计算分辨率
float computeResolution(PointCloudPtr cloud);
//体素滤波
void VoxelGrid_Filter(PointCloudPtr input,
PointCloudPtr output,
float leafsize = 1.0);
//基于统计学滤波
void StatisticalOutlierRemoval_Filter(PointCloudPtr input,
PointCloudPtr output,
int K = 30,
float stddevMulThresh = 1.0);
//法向量计算
void computeSurfaceNormals(FeatureCloud& cloud,
int K = 50,
float radius = 0,
int numofthreads = 4);
//FPFH计算
void computeFeatures_FPFH(FeatureCloud &cloud,
float R);
/*****配准***/
//SAC-IA
void SAC_IA_Transform(FeatureCloud &source_cloud,
FeatureCloud &target_cloud,
float minsampleDistance,
int numofSample,
int correspondenceRandomness,
Eigen::Matrix4f& final_transformation);
//ICP
float iterative_closest_points(std::string solver,
bool flag_reciprocal, bool flag_ransac,
FeatureCloud &source_cloud, FeatureCloud &target_cloud,
float transEps, float corresDist, float EuclFitEps, float outlThresh, int maxIteration,
Eigen::Matrix4f &final_transformation);
//计算转换矩阵
bool RotationTranslationCompute(FeatureCloud& cloudtarget,
FeatureCloud& cloudsource,
Eigen::Matrix4f &tranResult);
//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
PointCloud::Ptr pcd_tgt,
PointCloud::Ptr pcd_final);
//基于欧氏距离的聚类
void EuclideanClusterExtraction(PointCloudPtr pointInput,
std::vector<PointCloudPtr>& cloudListOutput,
float clusterTolerance = 0.02,
int minClusterSize = 100,
int maxClusterSize = 1000);
//基于区域生长的聚类
void RegionGrowingClusterExtraction(PointCloudPtr pointInput,
std::vector<PointCloudPtr>& cloudListOutput,
SurfaceNormals::Ptr normalInput,
int minClusterSize,
int maxClusterSize,
int numofNeighbour = 30,
float smoothThreshold = 3.0 / 180.0 * M_PI,
float curvatureThreshold = 1.0);
}4.2 源文件
代码语言：javascript复制#include "utilities.h"
//计算分辨率
float computeResolution(PointCloudPtr cloud)
{
clock_t start, end;
start = clock();
float resolution = 0.0;
int n_points = 0;
int nres;
std::vector<int> indices(2);
std::vector<float> sqr_distances(2);
pcl::search::KdTree<pcl::PointXYZ> tree;
tree.setInputCloud(cloud);
for (size_t i = 0; i < cloud->size(); i)
{
if (!pcl_isfinite((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
if (nres == 2)
{
resolution = sqrt(sqr_distances[1]);
n_points;
}
}
if (n_points != 0)
{
resolution /= n_points;
}
std::cout << "resolution : " << resolution << endl;
end = clock();
std::cout << "computeResolution has finished in "
<< (end - start) / CLOCKS_PER_SEC << " s n";
return resolution;
}
//体素滤波
void VoxelGrid_Filter(PointCloudPtr input,
PointCloudPtr output,
float leafsize)
{
clock_t start, end;
start = clock();
int num = input->size();
pcl::VoxelGrid<PointT> voxelgrid_filter;//对体素网格中所有点求均值,以期望均值点代替原始点集,更精确
voxelgrid_filter.setLeafSize(leafsize, leafsize, leafsize);
voxelgrid_filter.setInputCloud(input);
voxelgrid_filter.filter(*output);
//pcl::ApproximateVoxelGrid<PointT> approximate_voxel_filter;//利用体素网格的中心（长方体的中心）代替原始点
//approximate_voxel_filter.setLeafSize(leafsize, leafsize, leafsize);
//approximate_voxel_filter.setInputCloud(input);
//approximate_voxel_filter.filter(*output);
std::cout << "VoxelGrid_Filter, Input points: " << num
<< "; Output points: " << output->size() << std::endl;
end = clock();
std::cout << "VoxelGrid_Filter has finished in "
<< (end - start) / CLOCKS_PER_SEC << " s n";
}
//统计学滤波
void StatisticalOutlierRemoval_Filter(PointCloudPtr input,
PointCloudPtr output,
int K,
float stddevMulThresh)
{
clock_t start, end;
start = clock();
int num = input->size();
pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
statistical_filter.setMeanK(K);
statistical_filter.setStddevMulThresh(stddevMulThresh);
statistical_filter.setInputCloud(input);
statistical_filter.filter(*output);
std::cout << "StatisticalOutlierRemoval_Filter, Input points: " << num
<< "; Output points: " << output->size() << std::endl;
end = clock();
std::cout << "StatisticalOutlierRemoval_Filter has finished in "
<< (end - start) / CLOCKS_PER_SEC << " s n";
}
//法向量
void computeSurfaceNormals(FeatureCloud& cloud,
int K,
float radius,
int numofthreads)
{
clock_t start, end;
start = clock();
NormalsPtr normals_(new SurfaceNormals);
pcl::NormalEstimationOMP<PointT, NormalT> norm_est;
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
norm_est.setNumberOfThreads(numofthreads);
norm_est.setSearchMethod(tree);
if (radius != 0) {
norm_est.setRadiusSearch(radius);
}
else {
norm_est.setKSearch(K);
}
norm_est.setInputCloud(cloud.getCloud());
norm_est.compute(*normals_);
cloud.setInputNormal(normals_);
end = clock();
std::cout << "computeSurfaceNormals() has finished in "
<< (end - start) / CLOCKS_PER_SEC << " s n";
};
//FPFH计算
void computeFeatures_FPFH(FeatureCloud &cloud,
float R)
{
clock_t start, end;
start = clock();
FPFH_features::Ptr fpfh_features_(new FPFH_features);
FPFH_features::Ptr nanremoved_(new FPFH_features);
pcl::FPFHEstimationOMP<PointT, NormalT, FPFH33_FeatureT> fpfh;
fpfh.setNumberOfThreads(4);
fpfh.setSearchSurface(cloud.getCloud());
fpfh.setInputCloud(cloud.getCloud());
fpfh.setInputNormals(cloud.getNormal());
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
fpfh.setSearchMethod(tree);
fpfh.setRadiusSearch(R);
// Use all neighbors in a sphere of radius 5cm
// IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!!
// Compute the features
fpfh.compute(*fpfh_features_);
cloud.setFeatures_FPFH(fpfh_features_);
end = clock();
std::cout << "computeFeatures_FPFH has finished in "
<< (end - start) / CLOCKS_PER_SEC << " s n";
}
//ICP
float iterative_closest_points(FeatureCloud &source_cloud, FeatureCloud &target_cloud,
float transEps, float corresDist, float EuclFitEps, float outlThresh, int maxIteration,
Eigen::Matrix4f &final_transformation)
{
std::cout << "----------------- ICP -----------" << std::endl;
PointCloud::Ptr Final(new PointCloud);
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputSource(source_cloud.getCloud());
icp.setInputTarget(target_cloud.getCloud());
icp.setMaximumIterations(maxIteration);
icp.setTransformationEpsilon(transEps);
//icp.setMaxCorrespondenceDistance(corresDist);
//icp.setEuclideanFitnessEpsilon(EuclFitEps);
icp.setRANSACOutlierRejectionThreshold(0.01);
icp.align(*Final, final_transformation);
final_transformation = icp.getFinalTransformation();
std::cout << "ICP, FitnessScore: " << icp.getFitnessScore() << std::endl;
std::cout << std::endl << final_transformation << std::endl;
return icp.getFitnessScore();
}
//SAC-IA
void SAC_IA_Transform(FeatureCloud &source_cloud,
FeatureCloud &target_cloud,
float minsampleDistance,
int numofSample,
int correspondenceRandomness,
Eigen::Matrix4f& final_transformation)
{
std::cout << "--------------- SAC-IA ------------------" << std::endl;
clock_t start;
clock_t end;
start = clock();
//SAC配准
pcl::SampleConsensusInitialAlignment<PointT, PointT, FPFH33_FeatureT> scia;
scia.setInputSource(source_cloud.getCloud());
scia.setInputTarget(target_cloud.getCloud());
scia.setSourceFeatures(source_cloud.getFeatures_FPFH());
scia.setTargetFeatures(target_cloud.getFeatures_FPFH());
scia.setMaximumIterations(30); //协方差越精确，但是计算效率越低.(可省)
scia.setMinSampleDistance(minsampleDistance);
//scia.setNumberOfSamples(numofSample);//设置每次迭代计算中使用的样本数量（可省）,可节省时间
scia.setCorrespondenceRandomness(correspondenceRandomness);//设置计算协方差时选择多少近邻点，该值越大，
PointCloudPtr result(new PointCloud);
scia.align(*result);
end = clock();
std::cout << "calculate time is: " << float(end - start) / CLOCKS_PER_SEC << "s" << endl;
std::cout << "SAC has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl;
std::cout << std::endl << scia.getFinalTransformation() << std::endl;
final_transformation = scia.getFinalTransformation();
}
//计算转换矩阵
bool RotationTranslationCompute(FeatureCloud& cloudtarget,
FeatureCloud& cloudsource,
Eigen::Matrix4f &tranResult)
{
std::vector<int> mapping;
pcl::removeNaNFromPointCloud(*cloudtarget.getCloud(), *cloudtarget.getCloud(), mapping);
pcl::removeNaNFromPointCloud(*cloudsource.getCloud(), *cloudsource.getCloud(), mapping);
//Resolution Calculation
float resolution = 0.0;
resolution = computeResolution(cloudtarget.getCloud());
if (false)
{
//Statistical filter
StatisticalOutlierRemoval_Filter(cloudtarget.getCloud(), cloudtarget.getCloud(), 30, 1.0);
StatisticalOutlierRemoval_Filter(cloudsource.getCloud(), cloudsource.getCloud(), 30, 1.0);
}
//降采样获取关键点
if (true)
{
float leafSize = resolution * 5;
VoxelGrid_Filter(cloudtarget.getCloud(), cloudtarget.getCloud(), leafSize);
VoxelGrid_Filter(cloudsource.getCloud(), cloudsource.getCloud(), leafSize);
}
resolution = computeResolution(cloudtarget.getCloud());//更新分辨率
//Normal Calculation
int normal_K = 50;
float normal_R = resolution * 5;
computeSurfaceNormals(cloudtarget, normal_K, normal_R);
computeSurfaceNormals(cloudsource, normal_K, normal_R);
//Feature describe
float FPFH_radius = resolution * 5;
computeFeatures_FPFH(cloudtarget, FPFH_radius);
computeFeatures_FPFH(cloudsource, FPFH_radius);
Eigen::Matrix4f sac_trans;
//SAC-IA
if (true)
{
float minsampleDistance = resolution * 2;
int numofSample = 4;
int correspondenceRandomness = 280;
SAC_IA_Transform(cloudsource, cloudtarget, minsampleDistance,
numofSample, correspondenceRandomness, sac_trans);
}
//ICP
if (true)
{
float transEps = 1e-10;//设置两次变化矩阵之间的差值（一般设置为1e-10即可）
float maxCorresDist = resolution * 0.4;//设置对应点对之间的最大距离（此值对配准结果影响较大）
float EuclFitEps = 0.003;//设置收敛条件是均方误差和小于阈值,停止迭代；
float outlThresh = resolution * 1.5;
int maxIteration = 80;
iterative_closest_points(cloudsource, cloudtarget,
transEps, maxCorresDist, EuclFitEps,
outlThresh, maxIteration, sac_trans);
}
tranResult = sac_trans;
return true;
}
//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
PointCloud::Ptr pcd_tgt,
PointCloud::Ptr pcd_final)
{
//int vp_1, vp_2;
// Create a PCLVisualizer object
pcl::visualization::PCLVisualizer viewer("registration Viewer");
//viewer.createViewPort (0.0, 0, 0.5, 0.2, vp_1);
// viewer.createViewPort (0.5, 0, 0.2, 0.2, vp_2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(pcd_final, 0, 0, 255);
viewer.addPointCloud(pcd_src, src_h, "source cloud");
viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
viewer.addPointCloud(pcd_final, final_h, "final cloud");
//viewer.addCoordinateSystem(1.0);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
//基于区域生长的聚类
void RegionGrowingClusterExtraction(PointCloudPtr pointInput,
std::vector<PointCloudPtr>& cloudListOutput,
SurfaceNormals::Ptr normalInput,
int minClusterSize,
int maxClusterSize,
int numofNeighbour,
float smoothThreshold,
float curvatureThreshold)
{
clock_t start, end;
start = clock();
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
pcl::RegionGrowing<PointT, NormalT> reg;
reg.setMinClusterSize(minClusterSize);
reg.setMaxClusterSize(maxClusterSize);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(numofNeighbour);
reg.setInputCloud(pointInput);
reg.setInputNormals(normalInput);
reg.setSmoothnessThreshold(smoothThreshold);
std::vector<pcl::PointIndices> cluster_indices;
reg.extract(cluster_indices);
std::cout << "Number of clusters is equal to " << cluster_indices.size() << std::endl;
int j = 1;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it)
{
PointCloudPtr cloud_cluster(new PointCloud);
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit)
{
cloud_cluster->points.push_back(pointInput->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << j << " , " << cloud_cluster->points.size() << " data points." << std::endl;
j ;
cloudListOutput.push_back(cloud_cluster);
}
end = clock();
std::cout << "RegionGrowingClusterExtraction has finished in "
<< float(end - start) / CLOCKS_PER_SEC << " s n";
}4.3 Demo
代码语言：javascript复制#include "utilities.h"
#include <iostream>
#include"sstream"
#include <fstream>
int main()
{
std::string cloud_path1 = "D:\PCLData\bunny6\bun045.pcd";
std::string cloud_path2 = "D:\PCLData\bunny6\bun090.pcd";
PointCloudPtr tempcloud1(new PointCloud);//0
PointCloudPtr tempcloud2(new PointCloud);//45
pcl::io::loadPCDFile(cloud_path1, *tempcloud1);
std::cout << cloud_path1 << std::endl;
pcl::io::loadPCDFile(cloud_path2, *tempcloud2);
std::cout << cloud_path2 << std::endl;
PointCloudPtr cloud0(new PointCloud);//0
PointCloudPtr cloud45(new PointCloud);//45
FeatureCloud temp1;
FeatureCloud temp2;
pcl::copyPointCloud(*tempcloud1, *cloud0);
pcl::copyPointCloud(*tempcloud2, *cloud45);
temp1.setInputCloud(cloud0);
temp2.setInputCloud(cloud45);
PointCloudPtr tempCloud(new PointCloud);
Eigen::Matrix4f tranTemp = Eigen::Matrix4f::Identity();
if (RotationTranslationCompute(temp1, temp2, tranTemp))
{
pcl::copyPointCloud(*tempcloud2, *tempCloud);
pcl::transformPointCloud(*tempCloud, *tempCloud, tranTemp);
std::cout << "comprehensive tran: " << std::endl << tranTemp << std::endl;
}
else {
std::cout << "Error: RotationTranslationCompute was failed." << std::endl;
}
return 0;
}
