pcl registeration

 

//pcl
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h> 
#include <pcl/common/transforms.h>    


int main212565(int argc, char **argv)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_src (new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZI>);
  if (pcl::io::loadPCDFile<pcl::PointXYZI> ("/home/ddb/SharedFile/projet_src.pcd", *cloud_src) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }

  if (pcl::io::loadPCDFile<pcl::PointXYZI> ("/home/ddb/SharedFile/project.pcd", *cloud_target) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }

  Eigen::Quaterniond q( 1.f, 0.f, 0.f, 0.f );
  Eigen::Vector3d t( 0.f, 0.f, 0.f );
  runIcp(cloud_target, cloud_src, q, t);

  std::cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<"\n"
           <<t.x()<<" "<<t.y()<<" "<<t.z()<<"\n";  

  //Eigen::Matrix4f init_transform = Eigen::Matrix4f::Identity();
  //ndt
  // pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;
    // ndt.setTransformationEpsilon(0.01);   //为终止条件设置最小转换差异
    // ndt.setStepSize(0.1);    //为more-thuente线搜索设置最大步长
    // ndt.setResolution(0.2);   //设置NDT网格网格结构的分辨率（voxelgridcovariance）
    // ndt.setMaximumIterations(35);
    // ndt.setInputSource(cloud_src);  //源点云
    // ndt.setInputTarget(cloud_target);  //目标点云

    // pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    // ndt.align(*output_cloud, init_transform);
    // std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
    //     << " score: " << ndt.getFitnessScore() << std::endl;

  // init_transform = ndt.getFinalTransformation();
  ////pcl icp 
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_icp (new pcl::PointCloud<pcl::PointXYZI>);
  // pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
    // icp.setInputSource(cloud_src);   //设置输入的点云
    // icp.setInputTarget(cloud_target);    //目标点云
    // icp.align(*cloud_icp);          //匹配后源点云
    // icp.setMaximumIterations(100);  // 设置为1以便下次调用
  // icp.setMaxCorrespondenceDistance(5.0);  
  // icp.setTransformationEpsilon(1e-10); 
  // icp.setEuclideanFitnessEpsilon(0.5); 

    // std::cout << "Applied " << 20 << " ICP iteration(s) " << std::endl;

  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();  //初始化
    // if (icp.hasConverged())//icp.hasConverged ()=1（true）输出变换矩阵的适合性评估
    // {
    //     std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;

    //     transformation_matrix = icp.getFinalTransformation().cast<double>();

  //   Eigen::Matrix3d rotation= transformation_matrix.block(0, 0, 3, 3);
    //     q = rotation;
  //   t = transformation_matrix.block(0, 3, 3, 1);
  //   std::cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<"\n"
  //         <<t.x()<<" "<<t.y()<<" "<<t.z()<<"\n";
    // }
    // else
    // {
    //     PCL_ERROR("\nICP has not converged.\n");
    //     return -1;
    // }
  Eigen::Matrix3d rotation(q);
  transformation_matrix.block<0, 0>(3, 3) = rotation.block(0, 0, 3, 3);
  transformation_matrix(0,3) = t(0);  //(行, 列)
  transformation_matrix(1,3) = t(1);
  transformation_matrix(2,3) = t(2);

  pcl::transformPointCloud (*cloud_src, *cloud_icp, transformation_matrix);
  pcl::io::savePCDFile ("/home/ddb/SharedFile/projet_icp2.pcd", *cloud_icp);

  return 0;
}