• PCL - 3D点云配准(registration)介绍

    前面多篇博客都提到过，要善于从官网去熟悉一样东西。API部分详细介绍见

    Point Cloud Library (PCL): Module registration

    这里博主主要借鉴Tutorial里内容（博主整体都有看完）

    Introduction — Point Cloud Library 0.0 documentation

    接下来主要跑下Registration中的sample例子

    一.直接运行下How to use iterative closet point中的代码（稍微做了变化，打印输出了Final点云）

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5.  
    6. int main()
    7. {
    8. 	pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud(5, 1));
    9. 	pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud);
    10. 	
    11. // source->CloudIn data
    12. 	for (auto& point : *cloud_in)
    13. 	{
    14. 		point.x = 1024 * rand() / (RAND_MAX + 1.0f);
    15. 		point.y = 1024 * rand() / (RAND_MAX + 1.0f);
    16. 		point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    17.     }
    18. 	
    19.    std::cout << "source->cloud_in:" << std::endl;
    20. 	
    21. 	for (auto& point : *cloud_in)
    22. 		std::cout << point << std::endl;
    23. 	
    24. 	std::cout << std::endl;
    25.  
    26. //target->CloudOut data
    27. 	* cloud_out = *cloud_in;
    28. 	for (auto& point : *cloud_out)
    29. 	{
    30. 		point.x += 1.6f;
    31. 		point.y += 2.4f;
    32. 		point.z += 3.2f;
    33. 	}
    34.  
    35.     std::cout << "target->cloud_out:" << std::endl;
    36.  
    37. 	for (auto& point : *cloud_out)
    38. 		std::cout << point << std::endl;
    39.  
    40.     std::cout << std::endl;
    41.  
    42. 	pcl::IterativeClosestPoint icp;
    43. 	icp.setInputSource(cloud_in);
    44. 	icp.setInputTarget(cloud_out);
    45. 	
    46. //final->
    47.     pcl::PointCloud Final;
    48. 	icp.align(Final);
    49.  
    50. 	std::cout << "Final data points:" << std::endl;
    51.  
    52. 	for (auto& point : Final)
    53. 		std::cout << point << std::endl;
    54. 	std::cout << std::endl;
    55.  
    56. //transform info
    57.  
    58. 	std::cout << "has converged:" << icp.hasConverged() << " score: " <<
    59. 	icp.getFitnessScore() << std::endl;
    60. 	std::cout << icp.getFinalTransformation() << std::endl;
    61. 	
    62. 	return (0);
    63. }

     输出结果如下：

     这里需要清楚的一点，是将源点云向目标点云对齐，让其变换到目标点云的样子。

    从上面结果中也能够看出，输出的final点云数据是和target是非常近似的，也验证了上面的表述。同时也能够看到transform的信息也是对的。

    这边在上面代码基础上增加设置迭代次数，部分代码如下

    1. 	//target->CloudOut data
    2. 	*cloud_out = *cloud_in;
    3. 	for (auto& point : *cloud_out)
    4. 	{
    5. 		point.x += 1.6f;
    6. 		point.y += 2.4f;
    7. 		point.z += 3.2f;
    8. 	}
    9.  
    10. 	std::cout << "target->cloud_out:" << std::endl;
    11.  
    12. 	for (auto& point : *cloud_out)
    13. 		std::cout << point << std::endl;
    14.  
    15. 	std::cout << std::endl;
    16.  
    17. 	pcl::IterativeClosestPoint icp;
    18. 	icp.setInputSource(cloud_in);
    19. 	icp.setInputTarget(cloud_out);
    20. 	std::cout << "default itera times: " << icp.getMaximumIterations() << std::endl;
    21. 	icp.setMaximumIterations(1);
    22. 	std::cout << "set itera times: " << icp.getMaximumIterations() << std::endl;
    23.  
    24. 	std::cout << std::endl;
    25.  
    26. 	
    27. 	//final->

    可看到设为1时精度并没有迭代次数默认值为10高。

    这边可以统计下对齐函数运行的时间，还是在上面代码的基础上

    1. //final->
    2. 	pcl::PointCloud Final;
    3.  
    4. 	clock_t start = clock();
    5. 	icp.align(Final);
    6. 	clock_t end = clock();
    7.  
    8. 	std::cout << end - start << " ms" << std::endl;
    9. 	std::cout << std::endl;

    当迭代次数设为30时，耗时9ms.

     当迭代次数设为1时，耗时2ms

    二. 这里改写下How to use iterative closest point中的代码，如下：

    1. // test_vtk63.cpp : 定义控制台应用程序的入口点。
    2. //
    3. #include 
    4. #include 
    5.  
    6. #include "vtkAutoInit.h" 
    7. //VTK_MODULE_INIT(vtkRenderingOpenGL2);
    8. //VTK_MODULE_INIT(vtkInteractionStyle);
    9. //#define vtkRenderingCore_AUTOINIT 2(vtkRenderingOpenGL2, vtkInteractionStyle)
    10.  
    11.  
    12.  
    13. #include  
    14. #include 
    15. #include 
    16. #include //pcd 读写类相关的头文件。
    17. #include 
    18. #include 
    19. #include 
    20. #include 
    21. #include 
    22.  
    23. using namespace pcl;
    24. using namespace pcl::io;
    25. using namespace std;
    26.  
    27. int testpointcloudToPcd()
    28. {
    29. 	vtkObject::GlobalWarningDisplayOff();
    30. 	pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud);
    31. 	char strfilepath[256] = "D:\\PCL\\rabbit.pcd";
    32.  
    33. 	//第一种读入方法较多场合如此
    34. 	if (-1 == pcl::io::loadPCDFile(strfilepath, *cloud_in)) {
    35. 		cout << "error input!" << endl;
    36. 		return -1;
    37. 	}
    38.  
    39. 	//输出源点云点坐标
    40. 	std::cout << "Saved " << cloud_in->size() << " data points to input:" << std::endl;
    41.  
    42. 	//for (auto& point : *cloud_in)
    43. 		//std::cout << point << std::endl;
    44.  
    45.  
    46. 	//目标点云由输入点云来构造
    47. 	pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud);
    48. 	*cloud_out = *cloud_in;
    49. 	std::cout << "size:" << cloud_out->size() << std::endl;
    50. 	for (auto& point : *cloud_out)
    51. 	{
    52. 		point.x += 0.7f;
    53. 		point.y += 0.7f;
    54. 		point.z += 1.0f;
    55. 	}
    56.  
    57.  
    58.  
    59. 	//输出目标点云
    60. 	std::cout << "Transformed " << cloud_in->size() << " data points:" << std::endl;
    61. 	//for (auto& point : *cloud_out)
    62. 		//std::cout << point << std::endl;
    63.  
    64. 	pcl::io::savePCDFileASCII("D:\\PCL\\rabbit_trans.pcd", *cloud_out);
    65.  
    66. 	pcl::IterativeClosestPoint icp;
    67. 	icp.setInputSource(cloud_in);
    68. 	icp.setInputTarget(cloud_out);
    69.  
    70. 	pcl::PointCloud Final;
    71. 	icp.align(Final);
    72.     std::cout << "has converged:" << icp.hasConverged() << " score: " <<
    73.     icp.getFitnessScore() << std::endl;
    74.  
    75. 	std::cout << "Final data points size: " << Final.size() << std::endl;
    76. 	pcl::io::savePCDFileASCII("D:\\PCL\\Final.pcd", Final);
    77.  
    78.  
    79. 	//输出变换矩阵
    80. 	std::cout << icp.getFinalTransformation() << std::endl;
    81. }
    82.  
    83. int _tmain(int argc, _TCHAR* argv[])
    84. {
    85. 	testpointcloudToPcd();
    86. 	return 0;
    87. }

     目标点云由输入点云变换得到，同时也保存到了本地，方便下次直接加载。运行结果如下：

    理论上目标点云是由源点云x平移0.7，y平移0.7，z平移1获得。实际计算结果是如下矩阵的最后一列。和理论变换值还是有一些差距的。

     输入源点云，目标点云，Final点云的部分点坐标做了一个比较。可以看到源点云经过变换后，已经和目标点云很对齐了。

    这里上传下上面的三份点云文件。

    链接：https://pan.baidu.com/s/1LUB9jLOG1eIq8JT4wheqHw 
    提取码：pfsy 
     

    三. 小实验1

    读取两份点云，在窗口中拆分显示, 测试点云来自于mirrors / pointcloudlibrary / data · GitCode

    1. #include   // for pcl::make_shared
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6.  
    7. #include 
    8. #include 
    9.  
    10. #include 
    11.  
    12. #include 
    13. #include 
    14. #include 
    15.  
    16. #include 
    17.  
    18.  
    19. #include 
    20.  
    21. using pcl::visualization::PointCloudColorHandlerGenericField;
    22. using pcl::visualization::PointCloudColorHandlerCustom;
    23.  
    24. //convenient typedefs
    25. typedef pcl::PointXYZ PointT;
    26. typedef pcl::PointCloud PointCloud;
    27. typedef pcl::PointNormal PointNormalT;
    28. typedef pcl::PointCloud PointCloudWithNormals;
    29.  
    30. int main()
    31. {
    32.     PointCloud::Ptr cloud_src(new PointCloud);
    33. 	PointCloud::Ptr cloud_tgt(new PointCloud);
    34. 	
    35. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
    36. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
    37.  
    38. 	std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
    39. 	std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
    40.  
    41. 	pcl::VoxelGrid grid;
    42. 	PointCloud::Ptr src(new PointCloud);
    43. 	PointCloud::Ptr tgt(new PointCloud);
    44. 	
    45. 	grid.setLeafSize(0.05, 0.05, 0.05);
    46. 	grid.setInputCloud(cloud_src);
    47. 	grid.filter(*src);
    48. 	std::cout << "src size: " << src->size() << std::endl;
    49.  
    50. 	grid.setInputCloud(cloud_tgt);
    51. 	grid.filter(*tgt);
    52. 	std::cout << "target size: " << tgt->size() << std::endl;
    53.  
    54.  
    55. 	pcl::visualization::PCLVisualizer* p;
    56. 	int vp_1, vp_2;
    57.  
    58. 	p = new pcl::visualization::PCLVisualizer("view");
    59. 	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
    60. 	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
    61.  
    62. 	PointCloudColorHandlerCustom src_h(cloud_src, 255, 0, 0);
    63. 	PointCloudColorHandlerCustom tgt_h(cloud_tgt, 0, 255, 0);
    64. 	
    65. 	p->addPointCloud(cloud_src, src_h, "vp1_src", vp_1);
    66. 	p->addPointCloud(cloud_tgt, tgt_h, "vp1_target", vp_1);
    67.  
    68. 	p->addPointCloud(cloud_tgt, tgt_h, "vp2_target", vp_2);
    69.  
    70. 	p->spin();
    71.  
    72.  
    73.  
    74. 	return (0);
    75. }

    运行结果如下：

    四. 小实验2

    上面main函数中的代码修改为如下：

    1. PointCloud::Ptr cloud_src(new PointCloud);
    2. 	PointCloud::Ptr cloud_tgt(new PointCloud);
    3. 	
    4. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
    5. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
    6.  
    7. 	std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
    8. 	std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
    9.  
    10. 	pcl::VoxelGrid grid;
    11. 	PointCloud::Ptr src(new PointCloud);
    12. 	PointCloud::Ptr tgt(new PointCloud);
    13. 	
    14. 	grid.setLeafSize(0.05, 0.05, 0.05);
    15. 	grid.setInputCloud(cloud_src);
    16. 	grid.filter(*src);
    17. 	std::cout << "src size: " << src->size() << std::endl;
    18.  
    19. 	grid.setInputCloud(cloud_tgt);
    20. 	grid.filter(*tgt);
    21. 	std::cout << "target size: " << tgt->size() << std::endl;
    22.  
    23.  
    24. 	// Compute surface normals and curvature
    25. 	PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
    26. 	PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
    27. 	
    28. 	pcl::NormalEstimation norm_est;
    29. 	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
    30. 	norm_est.setSearchMethod(tree);
    31. 	norm_est.setKSearch(30);
    32. 	
    33. 	norm_est.setInputCloud(src);
    34. 	norm_est.compute(*points_with_normals_src);
    35. 	pcl::copyPointCloud(*src, *points_with_normals_src);
    36. 	
    37. 	norm_est.setInputCloud(tgt);
    38. 	norm_est.compute(*points_with_normals_tgt);
    39. 	pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
    40.  
    41.  
    42. 	pcl::visualization::PCLVisualizer* p;
    43. 	int vp_1, vp_2;
    44.  
    45. 	p = new pcl::visualization::PCLVisualizer("view");
    46. 	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
    47. 	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
    48.  
    49.  
    50. 	PointCloudColorHandlerCustom src_h(points_with_normals_src, 255, 0, 0);
    51. 	PointCloudColorHandlerCustom tgt_h(points_with_normals_tgt, 0, 255, 0);
    52. 	
    53.  
    54. 	p->addPointCloud(points_with_normals_src, src_h, "vp1_src", vp_1);
    55. 	p->addPointCloud(points_with_normals_tgt, tgt_h, "vp1_target", vp_1);
    56.  
    57. 	p->addPointCloud(points_with_normals_tgt, tgt_h, "vp2_target", vp_2);
    58.  
    59. 	p->spin();
    60.  
    61.  
    62.  
    63. 	return (0);

    运行结果如下：

     若注释掉上面代码中copyPointCloud部分两句

    1. 	//pcl::copyPointCloud(*src, *points_with_normals_src);
    2. 	
    3. 	norm_est.setInputCloud(tgt);
    4. 	norm_est.compute(*points_with_normals_tgt);
    5. 	//pcl::copyPointCloud(*tgt, *points_with_normals_tgt);

    运行结果如下：

     五. 小实验3

    对上面两个点云进行配准，迭代次数设为30。

    1. #include   // for pcl::make_shared
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6.  
    7. #include 
    8. #include 
    9.  
    10. #include 
    11.  
    12. #include 
    13. #include 
    14. #include 
    15.  
    16. #include 
    17.  
    18.  
    19. #include 
    20.  
    21. using pcl::visualization::PointCloudColorHandlerGenericField;
    22. using pcl::visualization::PointCloudColorHandlerCustom;
    23.  
    24. //convenient typedefs
    25. typedef pcl::PointXYZ PointT;
    26. typedef pcl::PointCloud PointCloud;
    27. typedef pcl::PointNormal PointNormalT;
    28. typedef pcl::PointCloud PointCloudWithNormals;
    29.  
    30. // Define a new point representation for < x, y, z, curvature >
    31. class MyPointRepresentation : public pcl::PointRepresentation < PointNormalT>
    32. {
    33. 	using pcl::PointRepresentation::nr_dimensions_;
    34. public:
    35. 	MyPointRepresentation()
    36.     {
    37. 		// Define the number of dimensions
    38. 	    nr_dimensions_ = 4;
    39.     }
    40. 	
    41.     // Override the copyToFloatArray method to define our feature vector
    42. 	virtual void copyToFloatArray(const PointNormalT & p, float* out) const
    43.     {
    44. 	// < x, y, z, curvature >
    45. 	    out[0] = p.x;
    46. 		out[1] = p.y;
    47. 		out[2] = p.z;
    48. 		out[3] = p.curvature;
    49. 	}
    50. };
    51.  
    52. int main()
    53. {
    54. 	PointCloud::Ptr cloud_src(new PointCloud);
    55. 	PointCloud::Ptr cloud_tgt(new PointCloud);
    56.  
    57. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
    58. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
    59.  
    60. 	std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
    61. 	std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
    62.  
    63. 	pcl::VoxelGrid grid;
    64. 	PointCloud::Ptr src(new PointCloud);
    65. 	PointCloud::Ptr tgt(new PointCloud);
    66.  
    67. 	grid.setLeafSize(0.05, 0.05, 0.05);
    68. 	grid.setInputCloud(cloud_src);
    69. 	grid.filter(*src);
    70. 	std::cout << "src size: " << src->size() << std::endl;
    71.  
    72. 	grid.setInputCloud(cloud_tgt);
    73. 	grid.filter(*tgt);
    74. 	std::cout << "target size: " << tgt->size() << std::endl;
    75.  
    76.  
    77. 	// Compute surface normals and curvature
    78. 	PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
    79. 	PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
    80.  
    81. 	pcl::NormalEstimation norm_est;
    82. 	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
    83. 	norm_est.setSearchMethod(tree);
    84. 	norm_est.setKSearch(30);
    85.  
    86. 	norm_est.setInputCloud(src);
    87. 	norm_est.compute(*points_with_normals_src);
    88. 	pcl::copyPointCloud(*src, *points_with_normals_src);
    89.  
    90. 	norm_est.setInputCloud(tgt);
    91. 	norm_est.compute(*points_with_normals_tgt);
    92. 	pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
    93.  
    94.  
    95. 	//
    96. 	// Instantiate our custom point representation (defined above) ...
    97. 	MyPointRepresentation point_representation;
    98. 	// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
    99. 	float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
    100. 	point_representation.setRescaleValues(alpha);
    101.  
    102.  
    103. 	// Align
    104. 	pcl::IterativeClosestPointNonLinear reg;
    105. 	reg.setTransformationEpsilon(1e-6);
    106. 	// Set the maximum distance between two correspondences (src<->tgt) to 10cm
    107.     // Note: adjust this based on the size of your datasets
    108. 	reg.setMaxCorrespondenceDistance(0.1);
    109. 	// Set the point representation
    110. 	reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
    111. 	
    112. 	reg.setInputSource(points_with_normals_src);
    113. 	reg.setInputTarget(points_with_normals_tgt);
    114.  
    115. 	PointCloudWithNormals::Ptr reg_result(new PointCloudWithNormals);
    116. 	reg.setMaximumIterations(30);
    117. 	reg.align(*reg_result);
    118. 	Eigen::Matrix4f transform = reg.getFinalTransformation();
    119. 	std::cout << "has converaged: " << reg.hasConverged() << " score: " << reg.getFitnessScore() << std::endl;
    120. 	std::cout << "transform: " << std::endl;
    121. 	std::cout << transform << std::endl;
    122. 	std::cout << std::endl;
    123.  
    124. 	pcl::io::savePCDFileASCII("D:\\PCL\\trans_method1_final.pcd", *reg_result);
    125. 	pcl::io::savePCDFileASCII("D:\\PCL\\trans_method1_out.pcd", *points_with_normals_tgt);
    126.  
    127. 	PointCloudWithNormals::Ptr transform_src(new PointCloudWithNormals);
    128. 	pcl::transformPointCloud(*points_with_normals_src, *transform_src, transform);
    129. 	pcl::io::savePCDFileASCII("D:\\PCL\\trans_method1_tranform_src.pcd", *transform_src);
    130. 	//
    131.  
    132. 	pcl::visualization::PCLVisualizer* p;
    133. 	int vp_1, vp_2;
    134.  
    135. 	p = new pcl::visualization::PCLVisualizer("view");
    136. 	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
    137. 	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
    138.  
    139.  
    140. 	PointCloudColorHandlerCustom src_h(points_with_normals_src, 255, 0, 0);
    141. 	PointCloudColorHandlerCustom tgt_h(points_with_normals_tgt, 0, 255, 0);
    142. 	PointCloudColorHandlerCustom final_h(reg_result, 0, 0, 255);
    143.  
    144. 	p->addPointCloud(points_with_normals_src, src_h, "vp1_src", vp_1);
    145. 	p->addPointCloud(reg_result, final_h, "reg_result", vp_1);
    146.  
    147. 	p->addPointCloud(points_with_normals_src, src_h, "vp1_src_copy", vp_2);
    148. 	p->addPointCloud(points_with_normals_tgt, tgt_h, "vp2_target", vp_2);
    149.  
    150. 	p->spin();
    151.  
    152.  
    153.  
    154. 	return (0);
    155. }

    运行效果如下（左边显示的是对齐后的点云和目标点云，右边是源点云和目标点云），左边两组点云的一致性很好，而右边两组由于未对齐，可看到是错乱分布的。

     源点云到目标点云迭代30次配准的变换矩阵如下：

     由变换矩阵将源点云映射得到的点云和直接通过配准函数获得的对齐后点云，部分数据如下，很接近。

      

    六. 小实验4

    将ICP的最小迭代次数设为1，外面加一个循环执行此过程30次。

    1. #include   // for pcl::make_shared
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6.  
    7. #include 
    8. #include 
    9.  
    10. #include 
    11.  
    12. #include 
    13. #include 
    14. #include 
    15.  
    16. #include 
    17.  
    18.  
    19. #include 
    20.  
    21. using pcl::visualization::PointCloudColorHandlerGenericField;
    22. using pcl::visualization::PointCloudColorHandlerCustom;
    23.  
    24. //convenient typedefs
    25. typedef pcl::PointXYZ PointT;
    26. typedef pcl::PointCloud PointCloud;
    27. typedef pcl::PointNormal PointNormalT;
    28. typedef pcl::PointCloud PointCloudWithNormals;
    29.  
    30. // Define a new point representation for < x, y, z, curvature >
    31. class MyPointRepresentation : public pcl::PointRepresentation < PointNormalT>
    32. {
    33. 	using pcl::PointRepresentation::nr_dimensions_;
    34. public:
    35. 	MyPointRepresentation()
    36.     {
    37. 		// Define the number of dimensions
    38. 	    nr_dimensions_ = 4;
    39.     }
    40. 	
    41.     // Override the copyToFloatArray method to define our feature vector
    42. 	virtual void copyToFloatArray(const PointNormalT & p, float* out) const
    43.     {
    44. 	// < x, y, z, curvature >
    45. 	    out[0] = p.x;
    46. 		out[1] = p.y;
    47. 		out[2] = p.z;
    48. 		out[3] = p.curvature;
    49. 	}
    50. };
    51.  
    52. int main()
    53. {
    54. 	PointCloud::Ptr cloud_src(new PointCloud);
    55. 	PointCloud::Ptr cloud_tgt(new PointCloud);
    56.  
    57. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
    58. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
    59.  
    60. 	std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
    61. 	std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
    62.  
    63. 	pcl::VoxelGrid grid;
    64. 	PointCloud::Ptr src(new PointCloud);
    65. 	PointCloud::Ptr tgt(new PointCloud);
    66.  
    67. 	grid.setLeafSize(0.05, 0.05, 0.05);
    68. 	grid.setInputCloud(cloud_src);
    69. 	grid.filter(*src);
    70. 	std::cout << "src size: " << src->size() << std::endl;
    71.  
    72. 	grid.setInputCloud(cloud_tgt);
    73. 	grid.filter(*tgt);
    74. 	std::cout << "target size: " << tgt->size() << std::endl;
    75.  
    76.  
    77. 	// Compute surface normals and curvature
    78. 	PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
    79. 	PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
    80.  
    81. 	pcl::NormalEstimation norm_est;
    82. 	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
    83. 	norm_est.setSearchMethod(tree);
    84. 	norm_est.setKSearch(30);
    85.  
    86. 	norm_est.setInputCloud(src);
    87. 	norm_est.compute(*points_with_normals_src);
    88. 	pcl::copyPointCloud(*src, *points_with_normals_src);
    89.  
    90. 	norm_est.setInputCloud(tgt);
    91. 	norm_est.compute(*points_with_normals_tgt);
    92. 	pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
    93.  
    94.  
    95. 	//
    96. 	// Instantiate our custom point representation (defined above) ...
    97. 	MyPointRepresentation point_representation;
    98. 	// ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
    99. 	float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
    100. 	point_representation.setRescaleValues(alpha);
    101.  
    102.  
    103. 	// Align
    104. 	pcl::IterativeClosestPointNonLinear reg;
    105. 	reg.setTransformationEpsilon(1e-6);
    106. 	// Set the maximum distance between two correspondences (src<->tgt) to 10cm
    107.     // Note: adjust this based on the size of your datasets
    108. 	reg.setMaxCorrespondenceDistance(0.1);
    109. 	// Set the point representation
    110. 	reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
    111. 	
    112. 	reg.setInputSource(points_with_normals_src);
    113. 	reg.setInputTarget(points_with_normals_tgt);
    114.  
    115. 	//
    116.     // Run the same optimization in a loop and visualize the results
    117. 	Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
    118. 	PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
    119. 	reg.setMaximumIterations(1);
    120.  
    121. 	pcl::visualization::PCLVisualizer* p;
    122. 	p = new pcl::visualization::PCLVisualizer("view");
    123.  
    124. 	for (int i = 0; i < 30; ++i)
    125.     {
    126. 	    PCL_INFO("Iteration Nr. %d.\n", i);
    127. 		
    128. 	    // save cloud for visualization purpose
    129. 	    points_with_normals_src = reg_result;
    130. 	
    131. 	    // Estimate
    132. 	    reg.setInputSource(points_with_normals_src);
    133. 	    reg.align(*reg_result);
    134. 	
    135. 			//accumulate transformation between each Iteration
    136. 	    Ti = reg.getFinalTransformation() * Ti;
    137. 	
    138. 			//if the difference between this transformation and the previous one
    139. 			//is smaller than the threshold, refine the process by reducing
    140. 			//the maximal correspondence distance
    141. 	    if (std::abs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
    142. 	      reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
    143. 	
    144. 	    prev = reg.getLastIncrementalTransformation();
    145. 	
    146. 	    // visualize current state
    147.  
    148. 		p->removePointCloud("source");
    149. 		p->removePointCloud("target");
    150.  
    151. 		PointCloudColorHandlerGenericField src_color_handler(points_with_normals_src, "curvature");
    152. 		if (!src_color_handler.isCapable())
    153. 			PCL_WARN("Cannot create curvature color handler!\n");
    154.  
    155. 		PointCloudColorHandlerGenericField tgt_color_handler(points_with_normals_tgt, "curvature");
    156. 	    if (!tgt_color_handler.isCapable())
    157. 	      PCL_WARN("Cannot create curvature color handler!\n");
    158. 	
    159. 	    	
    160. 	  	p->addPointCloud(points_with_normals_src, src_color_handler, "source");
    161. 	    p->addPointCloud(points_with_normals_tgt, tgt_color_handler, "target");
    162. 	     
    163. 	
    164. 		 p->spinOnce();
    165.  
    166. 	  }
    167. 	  
    168. 	std::cout << "transform: " << std::endl;
    169. 	std::cout << Ti << std::endl;
    170. 	std::cout << std::endl;
    171. 	pcl::io::savePCDFileASCII("D:\\PCL\\trans_method2_final.pcd", *reg_result);
    172.  
    173. 	
    174. 	return (0);
    175. }

    运行结果如下（可看到和上面一次迭代次数设为30的方法对比，结果是一致的，只是这样做的一个好处是可以对齐过程可视化，不像上面黑盒子一样）：

     七. 两个点云的融合（相加）

    1. #include   // for pcl::make_shared
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6.  
    7. #include 
    8. #include 
    9.  
    10. #include 
    11.  
    12. #include 
    13. #include 
    14. #include 
    15.  
    16. #include 
    17.  
    18.  
    19. #include 
    20.  
    21. using pcl::visualization::PointCloudColorHandlerGenericField;
    22. using pcl::visualization::PointCloudColorHandlerCustom;
    23.  
    24. //convenient typedefs
    25. typedef pcl::PointXYZ PointT;
    26. typedef pcl::PointCloud PointCloud;
    27. typedef pcl::PointNormal PointNormalT;
    28. typedef pcl::PointCloud PointCloudWithNormals;
    29.  
    30. int main()
    31. {
    32. 	PointCloud::Ptr cloud_src(new PointCloud);
    33. 	PointCloud::Ptr cloud_tgt(new PointCloud);
    34.  
    35. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0001.pcd", *cloud_src);
    36. 	pcl::io::loadPCDFile("D://PCL//data-master//tutorials//pairwise//capture0005.pcd", *cloud_tgt);
    37.  
    38. 	std::cout << "cloud_src size: " << cloud_src->size() << std::endl;
    39. 	std::cout << "cloud_target size: " << cloud_tgt->size() << std::endl;
    40.  
    41. 	pcl::VoxelGrid grid;
    42. 	PointCloud::Ptr src(new PointCloud);
    43. 	PointCloud::Ptr tgt(new PointCloud);
    44.  
    45. 	grid.setLeafSize(0.05, 0.05, 0.05);
    46. 	grid.setInputCloud(cloud_src);
    47. 	grid.filter(*src);
    48. 	std::cout << "src size: " << src->size() << std::endl;
    49.  
    50. 	grid.setInputCloud(cloud_tgt);
    51. 	grid.filter(*tgt);
    52. 	std::cout << "target size: " << tgt->size() << std::endl;
    53.  
    54.  
    55. 	pcl::visualization::PCLVisualizer* p;
    56. 	int vp_1, vp_2;
    57.  
    58. 	p = new pcl::visualization::PCLVisualizer("view");
    59. 	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
    60. 	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
    61.  
    62. 	PointCloudColorHandlerCustom src_h(cloud_src, 255, 0, 0);
    63. 	PointCloudColorHandlerCustom tgt_h(cloud_tgt, 0, 255, 0);
    64.  
    65. 	p->addPointCloud(cloud_src, src_h, "vp1_src", vp_1);
    66. 	p->addPointCloud(cloud_tgt, tgt_h, "vp1_target", vp_1);
    67.  
    68.  
    69. 	PointCloud::Ptr total(new PointCloud);
    70. 	total = cloud_src;
    71. 	*total += *cloud_tgt;
    72. 	PointCloudColorHandlerCustom total_h(total, 0, 0, 255);
    73.  
    74. 	p->addPointCloud(total, total_h, "vp2_target", vp_2);
    75.  
    76. 	p->spin();
    77.  
    78.  
    79.  
    80. 	return (0);
    81. }

    运行效果如下：

    八. How to incrementally register pairs of clouds

    有了上面的例子后，再去看官网上的这段代码就很容易了

    How to incrementally register pairs of clouds — Point Cloud Library 0.0 documentation

    代码会加载5张点云数据，A,B,C,D,E分别代表这5个点云，然后AB,BC,CD,DE分别两两配准，这样就可以把B,C,D,E都变换到A空间中去， 向A对齐，完毕后将这5个点云数据做融合（相加），这样就可以实现一个拼接或者融合的功能，后续任务就可以基于这份包含更细致信息的点云做文章。

  • 相关阅读:
    外部访问win服务器的mysql数据库
    flink1.13.2版本的对应的hive的Hcatalog的使用记录
    为什么要使用elasticsearch
    React进阶之路（一）-- JSX基础、组件基础
    【微信小程序】flex布局
    云原生赋能智能网联汽车消息处理基础框架构建
    学生个人网页设计作品 HTML+CSS+JavaScript仿小米商城(8页) 学生个人网页模板 简单个人主页成品 个人网页制作 HTML学生个人网站作业设计
    用C语言API(常用)操作MySql数据库
    常见限流算法
    6143. 预算内的最多机器人数目--（每日一难phase2--day7）
  • 原文地址：https://blog.csdn.net/jiugeshao/article/details/126730727