PCL点云获取+拼接(2)

本次博文介绍固定点拼接点云,kinect拍摄两幅画面,二者之间旋转10度,运行环境vs2012 pcl1.7.2

使用方法:

1.采样一致性初始配准算法SAC-IA,粗略估计初始变换矩阵

2.ICP算法,精确配准

原始点云(拼接前,隔着10度)

正视图

PCL点云获取+拼接(2)_第1张图片

俯视图

PCL点云获取+拼接(2)_第2张图片


代码:


     
     
     
     
  1. #include
  2. #include
  3. #include
  4. #include
  5. #include
  6. #include
  7. #include
  8. #include
  9. #include
  10. #include
  11. #include
  12. #include
  13. #include
  14. #include
  15. using namespace std;
  16. using namespace pcl;
  17. class FeatureCloud
  18. {
  19. public:
  20. // A bit of shorthand
  21. typedef pcl::PointCloud PointCloud;
  22. typedef pcl::PointCloud SurfaceNormals;
  23. typedef pcl::PointCloud LocalFeatures;
  24. typedef pcl::search::KdTree SearchMethod;
  25. FeatureCloud () :
  26. search_method_xyz_ ( new SearchMethod),
  27. normal_radius_ ( 0.5f),
  28. feature_radius_ ( 0.5f),
  29. voxel_grid_size ( 0.07f)
  30. {
  31. }
  32. ~FeatureCloud () {}
  33. // Process the given cloud
  34. void setInputCloud (PointCloud::Ptr xyz)
  35. {
  36. xyz_ = xyz;
  37. processInput ();
  38. }
  39. // Load and process the cloud in the given PCD file
  40. void loadInputCloud (const std::string &pcd_file)
  41. {
  42. xyz_ = PointCloud::Ptr ( new PointCloud);
  43. pcl::io::loadPCDFile (pcd_file, *xyz_);
  44. processInput ();
  45. }
  46. // Get a pointer to the cloud 3D points
  47. PointCloud:: Ptr getPointCloud () const
  48. {
  49. return (tempCloud);
  50. }
  51. // Get a pointer to the cloud of 3D surface normals
  52. SurfaceNormals:: Ptr getSurfaceNormals () const
  53. {
  54. return (normals_);
  55. }
  56. // Get a pointer to the cloud of feature descriptors
  57. LocalFeatures:: Ptr getLocalFeatures () const
  58. {
  59. return (features_);
  60. }
  61. protected:
  62. // Compute the surface normals and local features
  63. void processInput ()
  64. {
  65. mysample();
  66. computeSurfaceNormals ();
  67. computeLocalFeatures ();
  68. }
  69. void mysample()
  70. {
  71. gridsample = PointCloud::Ptr ( new PointCloud);
  72. tempCloud = PointCloud::Ptr ( new PointCloud);
  73. pcl::VoxelGrid vox_grid;
  74. vox_grid.setInputCloud (xyz_);
  75. vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
  76. vox_grid.filter (*gridsample);
  77. pcl::StatisticalOutlierRemoval sor;
  78. sor.setInputCloud(gridsample);
  79. sor.setMeanK( 50);
  80. sor.setStddevMulThresh( 1.0);
  81. sor.filter(*tempCloud);
  82. cout<< “cloud size before filtering:”<size()<< endl;
  83. cout<< “cloud size after filtering:”<size()<< endl;
  84. }
  85. // Compute the surface normals
  86. void computeSurfaceNormals ()
  87. {
  88. normals_ = SurfaceNormals::Ptr ( new SurfaceNormals);
  89. pcl::NormalEstimation norm_est;
  90. norm_est.setInputCloud (tempCloud);
  91. norm_est.setSearchMethod (search_method_xyz_);
  92. norm_est.setRadiusSearch (normal_radius_);
  93. norm_est.compute (*normals_);
  94. }
  95. // Compute the local feature descriptors
  96. void computeLocalFeatures ()
  97. {
  98. features_ = LocalFeatures::Ptr ( new LocalFeatures);
  99. pcl::FPFHEstimation fpfh_est;
  100. fpfh_est.setInputCloud (tempCloud);
  101. fpfh_est.setInputNormals (normals_);
  102. fpfh_est.setSearchMethod (search_method_xyz_);
  103. fpfh_est.setRadiusSearch (feature_radius_);
  104. fpfh_est.compute (*features_);
  105. }
  106. private:
  107. // Point cloud data
  108. PointCloud::Ptr xyz_;
  109. PointCloud::Ptr gridsample;
  110. PointCloud::Ptr tempCloud;
  111. SurfaceNormals::Ptr normals_;
  112. LocalFeatures::Ptr features_;
  113. SearchMethod::Ptr search_method_xyz_;
  114. // Parameters
  115. float normal_radius_;
  116. float feature_radius_;
  117. float voxel_grid_size;
  118. };
  119. class TemplateAlignment
  120. {
  121. public:
  122. TemplateAlignment () :
  123. min_sample_distance_ ( 0.01f),
  124. max_correspondence_distance_ ( 0.01f* 0.01f),
  125. nr_iterations_ ( 300)
  126. {
  127. // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
  128. sac_ia_.setMinSampleDistance (min_sample_distance_);
  129. sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
  130. sac_ia_.setMaximumIterations (nr_iterations_);
  131. }
  132. ~TemplateAlignment () {}
  133. void setSourceCloud(FeatureCloud &source_cloud)
  134. {
  135. sac_ia_.setInputCloud (source_cloud.getPointCloud ());
  136. sac_ia_.setSourceFeatures (source_cloud.getLocalFeatures ());
  137. }
  138. void setTargetCloud (FeatureCloud &target_cloud)
  139. {
  140. sac_ia_.setInputTarget (target_cloud.getPointCloud ());
  141. sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
  142. }
  143. // Align the given template cloud to the target specified by setTargetCloud ()
  144. void align ()
  145. {
  146. pcl::PointCloud registration_output;
  147. sac_ia_.align (registration_output);
  148. fitness_score = ( float) sac_ia_.getFitnessScore (max_correspondence_distance_);
  149. final_transformation = sac_ia_.getFinalTransformation ();
  150. }
  151. Eigen:: Matrix4f getMatrix()
  152. {
  153. return final_transformation;
  154. }
  155. float getScore()
  156. {
  157. return fitness_score;
  158. }
  159. private:
  160. // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
  161. pcl::SampleConsensusInitialAlignment sac_ia_;
  162. float min_sample_distance_;
  163. float fitness_score;
  164. float max_correspondence_distance_;
  165. Eigen::Matrix4f final_transformation;
  166. int nr_iterations_;
  167. };
  168. class MyICP
  169. {
  170. public:
  171. MyICP ()
  172. {
  173. // Intialize the parameters in the ICP algorithm
  174. icp.setMaxCorrespondenceDistance( 0.01);
  175. icp.setTransformationEpsilon( 1e-7);
  176. icp.setEuclideanFitnessEpsilon( 1);
  177. icp.setMaximumIterations( 100);
  178. }
  179. ~MyICP () {}
  180. void setSourceCloud(PointCloud::ConstPtr source_cloud)
  181. {
  182. icp.setInputCloud(source_cloud);
  183. }
  184. void setTargetCloud (PointCloud::ConstPtr target_cloud)
  185. {
  186. icp.setInputTarget(target_cloud);
  187. }
  188. // Align the given template cloud to the target specified by setTargetCloud ()
  189. void align (PointCloud &temp)
  190. {
  191. pcl::PointCloud registration_output;
  192. icp.align (temp);
  193. fitness_score = icp.getFitnessScore();
  194. final_transformation = icp.getFinalTransformation ();
  195. }
  196. float getScore()
  197. {
  198. return fitness_score;
  199. }
  200. Eigen:: Matrix4f getMatrix()
  201. {
  202. return final_transformation;
  203. }
  204. private:
  205. IterativeClosestPoint icp;
  206. Eigen::Matrix4f final_transformation;
  207. float fitness_score;
  208. };
  209. int main (int argc, char **argv)
  210. {
  211. int begin = 0;
  212. int end = 2;
  213. std:: vector object_templates;
  214. std:: stringstream ss;
  215. TemplateAlignment my_alignment;
  216. MyICP my_icp;
  217. Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform, pairTransform2;
  218. PointCloud:: Ptr result (new PointCloud);
  219. PointCloud:: Ptr my_cloud (new PointCloud);
  220. PointCloud:: Ptr Color_in (new PointCloud);
  221. PointCloud:: Ptr Color_out (new PointCloud);
  222. PointCloud Final_Color;
  223. PointCloud:: Ptr temp (new PointCloud);
  224. PointCloud temp2;
  225. ss.str( “”);
  226. ss<< “color_”<“.pcd”;
  227. if(io::loadPCDFile(ss.str(),*Color_in)== -1) //*打开点云文件
  228. {
  229. PCL_ERROR( “Couldn’t read file test_pcd.pcd\n”);
  230. return( -1);
  231. }
  232. //load data
  233. for( int j = begin;j < end;j++)
  234. {
  235. std:: stringstream ss;
  236. ss << j << “.pcd”;
  237. FeatureCloud template_cloud;
  238. template_cloud.loadInputCloud (ss.str());
  239. object_templates.push_back (template_cloud);
  240. }
  241. Final_Color = *Color_in;
  242. for ( size_t i = begin + 1; i < begin + object_templates.size (); ++i)
  243. {
  244. cout<endl;
  245. //cout<<”first size:”<size()<<”, second size:”<size()<
  246. my_alignment.setTargetCloud(object_templates[i -1-begin]);
  247. my_alignment.setSourceCloud(object_templates[i-begin]);
  248. my_alignment.align();
  249. cout<< “sac_ia fitness score:”<endl;
  250. //update the global transform
  251. pairTransform = my_alignment.getMatrix();
  252. //print matrix
  253. printf ( “\n”);
  254. printf ( ” | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform ( 0, 0), pairTransform ( 0, 1), pairTransform ( 0, 2), pairTransform ( 0, 3));
  255. printf ( “R = | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform ( 1, 0), pairTransform ( 1, 1), pairTransform ( 1, 2), pairTransform ( 1, 3));
  256. printf ( ” | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform ( 2, 0), pairTransform ( 2, 1), pairTransform ( 2, 2), pairTransform ( 2, 3));
  257. printf ( ” | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform ( 3, 0), pairTransform ( 3, 1), pairTransform ( 3, 2), pairTransform ( 3, 3));
  258. GlobalTransform = GlobalTransform * pairTransform;
  259. //GlobalTransform = pairTransform;
  260. //transform current pair into the global transform
  261. pcl::transformPointCloud (*object_templates[i-begin].getPointCloud(), *temp, GlobalTransform);
  262. my_icp.setSourceCloud(temp);
  263. my_icp.setTargetCloud(object_templates[i -1-begin].getPointCloud());
  264. my_icp.align(temp2);
  265. cout<< “icp fitness score:”<endl;
  266. pairTransform2 = my_icp.getMatrix();
  267. printf ( “\n”);
  268. printf ( ” | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 ( 0, 0), pairTransform2 ( 0, 1), pairTransform2 ( 0, 2), pairTransform2 ( 0, 3));
  269. printf ( “R = | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 ( 1, 0), pairTransform2 ( 1, 1), pairTransform2 ( 1, 2), pairTransform2 ( 1, 3));
  270. printf ( ” | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 ( 2, 0), pairTransform2 ( 2, 1), pairTransform2 ( 2, 2), pairTransform2 ( 2, 3));
  271. printf ( ” | %6.3f %6.3f %6.3f %6.3f| \n”, pairTransform2 ( 3, 0), pairTransform2 ( 3, 1), pairTransform2 ( 3, 2), pairTransform2 ( 3, 3));
  272. GlobalTransform = GlobalTransform * pairTransform2;
  273. ss.str( “”);
  274. ss<< “color_”<“.pcd”;
  275. if(pcl::io::loadPCDFile(ss.str(),*Color_out)== -1) //*打开点云彩色文件
  276. {
  277. PCL_ERROR( “Couldn’t read file test_pcd.pcd\n”);
  278. return( -1);
  279. }
  280. //transform current pair into the global transform
  281. pcl::transformPointCloud (*Color_out, *result, GlobalTransform);
  282. Final_Color = Final_Color + *result;
  283. }
  284. //构造拼接临时的点云
  285. for( int i= 0;i< Final_Color.points.size();i++)
  286. {
  287. pcl::PointXYZRGB basic_point;
  288. basic_point.x = Final_Color.points[i].x;
  289. basic_point.y = Final_Color.points[i].y;
  290. basic_point.z = Final_Color.points[i].z;
  291. basic_point.r = Final_Color.points[i].r;
  292. basic_point.g = Final_Color.points[i].g;
  293. basic_point.b = Final_Color.points[i].b;
  294. my_cloud->points.push_back(basic_point);
  295. }
  296. pcl::visualization:: CloudViewer viewer(”My Cloud Viewer”);
  297. viewer.showCloud(my_cloud);
  298. while(!viewer.wasStopped())
  299. {
  300. }
  301. return ( 0);
  302. }

结果如下,角度不见了~~

PCL点云获取+拼接(2)_第3张图片

PCL点云获取+拼接(2)_第4张图片

PCL点云获取+拼接(2)_第5张图片


别高兴太早,这套算法如果这么牛逼,我也不用这么蛋疼了。如果用他拼接360度,必定失败,如果有用这个方法能搞定连续多幅图片拼接的朋友,请私信我。


下面是我用NDT方法,连续拼接90度的结果,只能这样了。。。


     
     
     
     
  1. Filtered cloud contains 540
  2. ndt fitness score: 0.0227071
  3. | 0.985 0.007 -0.174 0.003|
  4. R = | -0.007 1.000 0.002 0.000|
  5. | 0.174 -0.000 0.985 -0.006|
  6. | 0.000 0.000 0.000 1.000|
  7. Filtered cloud contains 420
  8. ndt fitness score: 0.0343324
  9. | 0.989 0.040 -0.146 0.005|
  10. R = | -0.037 0.999 0.021 -0.005|
  11. | 0.146 -0.015 0.989 -0.005|
  12. | 0.000 0.000 0.000 1.000|
  13. Filtered cloud contains 552
  14. ndt fitness score: 0.0802134
  15. | 0.968 -0.016 -0.249 0.152|
  16. R = | 0.021 1.000 0.016 -0.014|
  17. | 0.248 -0.020 0.969 -0.012|
  18. | 0.000 0.000 0.000 1.000|
  19. Filtered cloud contains 926
  20. ndt fitness score: 0.0198928
  21. | 0.978 -0.015 -0.210 0.148|
  22. R = | 0.019 1.000 0.017 -0.024|
  23. | 0.209 -0.020 0.978 0.016|
  24. | 0.000 0.000 0.000 1.000|
  25. Filtered cloud contains 575
  26. ndt fitness score: 0.0492542
  27. | 0.962 -0.007 -0.273 0.085|
  28. R = | 0.006 1.000 -0.001 -0.002|
  29. | 0.273 -0.000 0.962 -0.009|
  30. | 0.000 0.000 0.000 1.000|
  31. Filtered cloud contains 412
  32. ndt fitness score: 0.00171811
  33. | 0.992 -0.024 -0.127 0.001|
  34. R = | 0.023 1.000 -0.007 -0.000|
  35. | 0.127 0.004 0.992 0.003|
  36. | 0.000 0.000 0.000 1.000|
  37. Filtered cloud contains 295
  38. ndt fitness score: 0.00152303
  39. | 0.983 -0.001 -0.182 0.086|
  40. R = | 0.003 1.000 0.010 0.038|
  41. | 0.182 -0.011 0.983 0.090|
  42. | 0.000 0.000 0.000 1.000|
  43. Filtered cloud contains 191
  44. ndt fitness score: 0.023204
  45. | 0.975 -0.080 -0.208 0.121|
  46. R = | 0.092 0.995 0.047 -0.142|
  47. | 0.203 -0.065 0.977 0.103|
  48. | 0.000 0.000 0.000 1.000|
  49. Filtered cloud contains 133
  50. ndt fitness score: 0.00556793
  51. | 0.983 0.003 -0.184 0.008|
  52. R = | -0.004 1.000 -0.002 0.000|
  53. | 0.184 0.003 0.983 0.011|
  54. | 0.000 0.000 0.000 1.000|

PCL点云获取+拼接(2)_第6张图片

PCL点云获取+拼接(2)_第7张图片

            

你可能感兴趣的:(PCL)