【3D篇】点云拼接

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);
}

#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 &lt;&lt; &#34;PointCloud representing the Cluster: &#34; &lt;&lt; j &lt;&lt; &#34; , &#34; &lt;&lt; cloud_cluster-&gt;points.size() &lt;&lt; &#34; data points.&#34; &lt;&lt; std::endl;
j++;
cloudListOutput.push_back(cloud_cluster);

}

end = clock();

std::cout << "RegionGrowingClusterExtraction has finished in "

<< float(end - start) / CLOCKS_PER_SEC << " s \n";

}

#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 &lt;&lt; &#34;comprehensive tran: &#34; &lt;&lt; std::endl &lt;&lt; tranTemp &lt;&lt; std::endl;

}

else {

std::cout << "Error: RotationTranslationCompute was failed." << std::endl;

}

return 0;

}