align.cpp 4.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118
  1. #include <iostream>
  2. #include <ros/ros.h>
  3. #include <pcl/io/pcd_io.h>
  4. #include <pcl/point_types.h>
  5. #include <pcl/point_cloud.h>
  6. #include <pcl/registration/ndt.h>
  7. #include <pcl/registration/gicp.h>
  8. #include <pcl/filters/voxel_grid.h>
  9. #include <pcl/visualization/pcl_visualizer.h>
  10. #include <pclomp/ndt_omp.h>
  11. #include <pclomp/gicp_omp.h>
  12. // align point clouds and measure processing time
  13. pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
  14. registration->setInputTarget(target_cloud);
  15. registration->setInputSource(source_cloud);
  16. pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
  17. auto t1 = ros::WallTime::now();
  18. registration->align(*aligned);
  19. auto t2 = ros::WallTime::now();
  20. std::cout << "single : " << (t2 - t1).toSec() * 1000 << "[msec]" << std::endl;
  21. for(int i=0; i<10; i++) {
  22. registration->align(*aligned);
  23. }
  24. auto t3 = ros::WallTime::now();
  25. std::cout << "10times: " << (t3 - t2).toSec() * 1000 << "[msec]" << std::endl;
  26. std::cout << "fitness: " << registration->getFitnessScore() << std::endl << std::endl;
  27. return aligned;
  28. }
  29. int main(int argc, char** argv) {
  30. if(argc != 3) {
  31. std::cout << "usage: align target.pcd source.pcd" << std::endl;
  32. return 0;
  33. }
  34. std::string target_pcd = argv[1];
  35. std::string source_pcd = argv[2];
  36. pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  37. pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  38. if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) {
  39. std::cerr << "failed to load " << target_pcd << std::endl;
  40. return 0;
  41. }
  42. if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) {
  43. std::cerr << "failed to load " << source_pcd << std::endl;
  44. return 0;
  45. }
  46. // downsampling
  47. pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>());
  48. pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
  49. voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f);
  50. voxelgrid.setInputCloud(target_cloud);
  51. voxelgrid.filter(*downsampled);
  52. *target_cloud = *downsampled;
  53. voxelgrid.setInputCloud(source_cloud);
  54. voxelgrid.filter(*downsampled);
  55. source_cloud = downsampled;
  56. ros::Time::init();
  57. // benchmark
  58. std::cout << "--- pcl::GICP ---" << std::endl;
  59. pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
  60. pcl::PointCloud<pcl::PointXYZ>::Ptr aligned = align(gicp, target_cloud, source_cloud);
  61. std::cout << "--- pclomp::GICP ---" << std::endl;
  62. pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
  63. aligned = align(gicp_omp, target_cloud, source_cloud);
  64. std::cout << "--- pcl::NDT ---" << std::endl;
  65. pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
  66. ndt->setResolution(1.0);
  67. aligned = align(ndt, target_cloud, source_cloud);
  68. std::vector<int> num_threads = {1, omp_get_max_threads()};
  69. std::vector<std::pair<std::string, pclomp::NeighborSearchMethod>> search_methods = {
  70. {"KDTREE", pclomp::KDTREE},
  71. {"DIRECT7", pclomp::DIRECT7},
  72. {"DIRECT1", pclomp::DIRECT1}
  73. };
  74. pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
  75. ndt_omp->setResolution(1.0);
  76. for(int n : num_threads) {
  77. for(const auto& search_method : search_methods) {
  78. std::cout << "--- pclomp::NDT (" << search_method.first << ", " << n << " threads) ---" << std::endl;
  79. ndt_omp->setNumThreads(n);
  80. ndt_omp->setNeighborhoodSearchMethod(search_method.second);
  81. aligned = align(ndt_omp, target_cloud, source_cloud);
  82. }
  83. }
  84. // visualization
  85. pcl::visualization::PCLVisualizer vis("vis");
  86. pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_handler(target_cloud, 255.0, 0.0, 0.0);
  87. pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_handler(source_cloud, 0.0, 255.0, 0.0);
  88. pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_handler(aligned, 0.0, 0.0, 255.0);
  89. vis.addPointCloud(target_cloud, target_handler, "target");
  90. vis.addPointCloud(source_cloud, source_handler, "source");
  91. vis.addPointCloud(aligned, aligned_handler, "aligned");
  92. vis.spin();
  93. return 0;
  94. }