【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 << "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";
}

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