iss关键点检测以及SAC-IA粗配准

一、iss关键点检测

C++

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

int main(int, char** argv)
{
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);//要配准变化的点云
	pcl::PointCloud::Ptr cloud_target(new pcl::PointCloud);//目标点云(不变的）
	if (pcl::io::loadPCDFile("pcd/pig_view1.pcd", *cloud) == -1)
	{
		PCL_ERROR("加载点云失败\n");
	}
	if (pcl::io::loadPCDFile("pcd/pig_view2.pcd", *cloud_target) == -1)
	{
		PCL_ERROR("加载点云失败\n");
	}
	pcl::ISSKeypoint3D iss;
	pcl::PointCloud::Ptr keypoints(new pcl::PointCloud());
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
	iss.setInputCloud(cloud);
	iss.setSearchMethod(tree);
	iss.setNumberOfThreads(8);     //初始化调度器并设置要使用的线程数
	iss.setSalientRadius(5);  // 设置用于计算协方差矩阵的球邻域半径
	iss.setNonMaxRadius(5);   // 设置非极大值抑制应用算法的半径
	iss.setThreshold21(0.95);     // 设定第二个和第一个特征值之比的上限
	iss.setThreshold32(0.95);     // 设定第三个和第二个特征值之比的上限
	iss.setMinNeighbors(6);       // 在应用非极大值抑制算法时，设置必须找到的最小邻居数
	iss.compute(*keypoints);
	cout << "ISS_3D points 的提取结果为 " << keypoints->points.size() << endl;
	//关键点显示
	boost::shared_ptr viewer1(new pcl::visualization::PCLVisualizer("v1"));
	viewer1->setBackgroundColor(255, 255, 255);
	viewer1->setWindowName("ISS Key point extraction");
	pcl::visualization::PointCloudColorHandlerCustom single_color(cloud, 0.0, 255, 0.0);
	viewer1->addPointCloud(cloud, single_color, "sample cloud");
	viewer1->addPointCloud(keypoints, "key cloud");//特征点
	viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "key cloud");
	viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "key cloud");
	while (!viewer1->wasStopped())
	{
		viewer1->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100));
	}



	return 0;
}

关键代码解析：

iss关键点提取

	pcl::ISSKeypoint3D iss;
	pcl::PointCloud::Ptr keypoints(new pcl::PointCloud());
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
	iss.setInputCloud(cloud);
	iss.setSearchMethod(tree);
	iss.setNumberOfThreads(8);     //初始化调度器并设置要使用的线程数
	iss.setSalientRadius(5);  // 设置用于计算协方差矩阵的球邻域半径
	iss.setNonMaxRadius(5);   // 设置非极大值抑制应用算法的半径
	iss.setThreshold21(0.95);     // 设定第二个和第一个特征值之比的上限
	iss.setThreshold32(0.95);     // 设定第三个和第二个特征值之比的上限
	iss.setMinNeighbors(6);       // 在应用非极大值抑制算法时，设置必须找到的最小邻居数
	iss.compute(*keypoints);

1. pcl::ISSKeypoint3D iss;：

   • 创建了 ISS 关键点检测器的对象 iss。
   • pcl::ISSKeypoint3D 是用于 3D 点云的 ISS 关键点检测器。

2. pcl::PointCloud::Ptr keypoints(new pcl::PointCloud());：

   • 创建了一个指向包含关键点的点云的指针 keypoints。
   • pcl::PointCloud::Ptr 定义了指向包含 XYZ 坐标的点云的指针类型。

3. pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());：

   • 创建了一个用于最近邻搜索的 KdTree 数据结构的指针 tree。
   • pcl::search::KdTree::Ptr 定义了指向 KdTree 的指针类型。

4. iss.setInputCloud(cloud);：

   • 设置输入点云，即要检测关键点的点云。

5. iss.setSearchMethod(tree);：

   • 设置关键点检测时用于最近邻搜索的方法，这里是 KdTree。

6. iss.setNumberOfThreads(8);：

   • 初始化调度器并设置要使用的线程数，这里设置了 8 个线程用于计算。增加线程数可以加快算法的运行速度，但是也会增加系统资源的占用。通常，将线程数设置为机器的逻辑处理器数量是一个合理的选择。

7. iss.setSalientRadius(5);：

   • 设置用于计算协方差矩阵的球形邻域的半径。较小的半径会导致更细小的特征被检测到，而较大的半径则会导致更大的特征被检测到。因此，这个参数影响着检测到的关键点的大小和密度。

8. iss.setNonMaxRadius(5);：

   • 设置非极大值抑制应用算法的半径，用于去除冗余的关键点。较小的半径会保留更多的关键点，而较大的半径则会删除更多的冗余点。因此，这个参数影响着最终检测到的关键点的数量和分布。

9. iss.setThreshold21(0.95);：

   • 设定第二个和第一个特征值之比的上限，用于判断关键点的稳定性。较高的阈值会导致更严格的特征点筛选，从而只保留最稳定的关键点。较低的阈值可能会导致更多的关键点被保留，但可能包含一些噪音或不稳定的点。

10. iss.setThreshold32(0.95);：

    • 设定第三个和第二个特征值之比的上限，同样用于判断关键点的稳定性。同样，较高的阈值会导致更严格的特征点筛选，较低的阈值可能会导致更多的关键点被保留。

11. iss.setMinNeighbors(6);：

    • 在应用非极大值抑制算法时，设置必须找到的最小邻居数，以确保关键点的孤立性。每次获得关键点，都会把邻域内除关键点外的点删除，较大的值会导致更多的关键点被删除，因为只有周围邻域内具有较少邻居的点才会被保留。较小的值可能会导致更多的关键点被保留，但也可能包含一些噪音点。

12. iss.compute(*keypoints);：

    • 执行关键点检测操作，计算关键点并将结果存储在 keypoints 中。

这段代码利用 ISS 关键点检测器，通过设置不同的参数，计算输入点云的关键点，并将结果存储在 keypoints 中。这些关键点通常代表了输入点云中的显著结构。

结果：

二、iss关键点检测以及初始对齐

C++

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include   
#include 
#include 
using namespace std;
void extract_keypoint(pcl::PointCloud::Ptr& cloud, pcl::PointCloud::Ptr& keypoint)
{
	pcl::ISSKeypoint3D iss;
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
	iss.setInputCloud(cloud);
	iss.setSearchMethod(tree);
	iss.setNumberOfThreads(8);     //初始化调度器并设置要使用的线程数
	iss.setSalientRadius(5);  // 设置用于计算协方差矩阵的球邻域半径
	iss.setNonMaxRadius(5);   // 设置非极大值抑制应用算法的半径
	iss.setThreshold21(0.95);     // 设定第二个和第一个特征值之比的上限
	iss.setThreshold32(0.95);     // 设定第三个和第二个特征值之比的上限
	iss.setMinNeighbors(6);       // 在应用非极大值抑制算法时，设置必须找到的最小邻居数
	iss.compute(*keypoint);
}
pcl::PointCloud::Ptr compute_fpfh_feature(pcl::PointCloud::Ptr& cloud, pcl::PointCloud::Ptr& keypoint)
{
	pcl::search::KdTree::Ptr tree;
	pcl::PointCloud::Ptr normals(new pcl::PointCloud);
	pcl::NormalEstimation n;
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	int i = 0;
	for (auto p : cloud->points)
	{
		for (auto k : keypoint->points)
		{
			if (k.x == p.x && k.y == p.y && k.z == p.z)
			{
				inliers->indices.push_back(i);
			}
		}
		i++;
	}
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(10);
	n.compute(*normals);
	pcl::PointCloud::Ptr fpfh(new pcl::PointCloud);
	pcl::FPFHEstimationOMP f;
	f.setIndices(inliers);
	f.setNumberOfThreads(8);
	f.setInputCloud(cloud);
	f.setInputNormals(normals);
	f.setSearchMethod(tree);
	f.setRadiusSearch(50);
	f.compute(*fpfh);
	return fpfh;
}
pcl::PointCloud::Ptr sac_align(pcl::PointCloud::Ptr& cloud, pcl::PointCloud::Ptr s_k, pcl::PointCloud::Ptr t_k, pcl::PointCloud::Ptr sk_fpfh, pcl::PointCloud::Ptr tk_fpfh)
{
	pcl::SampleConsensusInitialAlignment scia;
	scia.setInputSource(s_k);
	scia.setInputTarget(t_k);
	scia.setSourceFeatures(sk_fpfh);
	scia.setTargetFeatures(tk_fpfh);
	scia.setMinSampleDistance(7);///参数：设置采样点之间的最小距离，满足的被当做采样点
	scia.setNumberOfSamples(100);设置每次迭代设置采样点的个数(这个参数多可以增加配准精度)
	scia.setCorrespondenceRandomness(6);//设置选择随机特征对应点时要使用的邻域点个数。值越大，特征匹配的随机性就越大
	pcl::PointCloud::Ptr sac_result(new pcl::PointCloud);
	scia.align(*sac_result);
	pcl::transformPointCloud(*cloud, *sac_result, scia.getFinalTransformation());
	return sac_result;
}


int main(int, char** argv)
{

	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);//要配准变化的点云
	pcl::PointCloud::Ptr cloud_target(new pcl::PointCloud);//目标点云(不变的）
	if (pcl::io::loadPCDFile("pcd/pig_view1.pcd", *cloud) == -1)
	{
		PCL_ERROR("加载点云失败\n");
	}
	if (pcl::io::loadPCDFile("pcd/pig_view2.pcd", *cloud_target) == -1)
	{
		PCL_ERROR("加载点云失败\n");
	}

	pcl::PointCloud::Ptr s_k(new pcl::PointCloud);
	pcl::PointCloud::Ptr t_k(new pcl::PointCloud);
	extract_keypoint(cloud, s_k);
	extract_keypoint(cloud_target, t_k);
	cout << "提取关键点完成！" << endl;
	pcl::PointCloud::Ptr sk_fpfh = compute_fpfh_feature(cloud, s_k);
	pcl::PointCloud::Ptr tk_fpfh = compute_fpfh_feature(cloud_target, t_k);
	cout << "计算特征完成！" << endl;
	pcl::PointCloud::Ptr result(new pcl::PointCloud);
	result = sac_align(cloud, s_k, t_k, sk_fpfh, tk_fpfh);
	cout << "初始对齐完成！" << endl;
	boost::shared_ptrviewer1(new pcl::visualization::PCLVisualizer("v2"));
	viewer1->setBackgroundColor(255, 255, 255);  //设置背景颜色为黑色
	viewer1->setWindowName("Coarse registration");
	// 对目标点云着色可视化 (red).
	pcl::visualization::PointCloudColorHandlerCustomtarget_color(cloud_target, 255, 0, 0);
	viewer1->addPointCloud(cloud_target, target_color, "target cloud");
	viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
	// 对源点云着色可视化 (green).
	pcl::visualization::PointCloudColorHandlerCustominput_color(result, 0, 255, 0);
	viewer1->addPointCloud(result, input_color, "input cloud");
	viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");

	while (!viewer1->wasStopped())
	{
		viewer1->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}


	return 0;
}

注意：这部分代码运行很慢需要几分钟

关键代码解析：

法线计算

pcl::search::KdTreepcl::PointXYZ::Ptr tree;
pcl::PointCloudpcl::Normal::Ptr normals(new pcl::PointCloudpcl::Normal);
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(10);
n.compute(*normals);

1. pcl::search::KdTree::Ptr tree;

   这一行声明了一个指向 kd-tree 数据结构的指针 tree，该 kd-tree 是专门用于处理 3D 点云数据（pcl::PointXYZ）的。Kd 树通常用于高效地进行最近邻搜索。

2. pcl::PointCloud::Ptr normals(new pcl::PointCloud);

   在这一行，声明并初始化了一个法向量点云的指针 normals。法向量是垂直于三维点表面的向量，通常用于表面重建和特征提取等应用中。

3. n.setInputCloud(cloud);

   这一行设置了法向量估计过程的输入点云。它假设有一个名为 cloud 的点云变量包含了三维点。

4. n.setSearchMethod(tree);

   这里设置了在法向量估计过程中用于查找邻居点的搜索方法。它使用了之前声明的 kd 树 tree。

5. n.setKSearch(10);

   这一行将参数 KSearch 设置为 10。KSearch 参数定义了在估计法向量时要考虑的邻居点的数量。在这种情况下，它被设置为 10，意味着每个点的法向量将基于其 10 个最近邻点来计算。

6. n.compute(*normals);

   最后，这一行根据之前设置的参数和输入，计算出了法向量。计算出的法向量被存储在 normals 点云中。

关于参数设置：

• setKSearch：该参数确定在估计法向量时要考虑的邻居点的数量。较高的值可能会导致更平滑的法向量，但可能会减少对表面细节的敏感性。

感兴趣区域的索引集合

pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	int i = 0;
	for (auto p : cloud->points)
	{
		for (auto k : keypoint->points)
		{
			if (k.x == p.x && k.y == p.y && k.z == p.z)
			{
				inliers->indices.push_back(i);
			}
		}
		i++;
	}

1. pcl::PointIndices::Ptr inliers(new pcl::PointIndices());：

   • 创建了一个指向 pcl::PointIndices 类型对象的智能指针 inliers，用于存储符合条件的点的索引。

2. int i = 0;：

   • 初始化一个整数变量 i，用于追踪点云中的索引。

3. for (auto p : cloud->points)：

   • 遍历源点云 cloud 中的每个点，其中 cloud 应该是一个点云对象，通过 points 成员来访问其中的点。

4. for (auto k : keypoint->points)：

   • 在源点云的每个点迭代中，再次遍历关键点云 keypoint 中的每个点，其中 keypoint 也应该是一个点云对象，通过 points 成员来访问其中的点。

5. if (k.x == p.x && k.y == p.y && k.z == p.z)：

   • 检查当前源点云 cloud 中的点 p 是否与关键点云 keypoint 中的点 k 具有相同的坐标（x、y、z）。

6. inliers->indices.push_back(i);：

   • 如果源点云 cloud 中的点 p 是关键点云 keypoint 中的一个点，则将该点在源点云中的索引 i 添加到 inliers 中，以标记为内点（inliers）。

7. i++;：

   • 更新索引值，以便跟踪源点云中的下一个点的索引。

这段代码的目的是找到源点云中与关键点云中的点具有相同坐标的点，并将其索引存储在 inliers 中。这样可以用于后续的处理，比如在配准过程中确定内点或者在其他算法中过滤或识别特定的点。

fpfh特征计算 

	pcl::PointCloud::Ptr fpfh(new pcl::PointCloud);
	pcl::FPFHEstimationOMP f;
	f.setIndices(inliers);
	f.setNumberOfThreads(8);
	f.setInputCloud(cloud);
	f.setInputNormals(normals);
	f.setSearchMethod(tree);
	f.setRadiusSearch(50);
	f.compute(*fpfh);

1. pcl::PointCloud::Ptr fpfh(new pcl::PointCloud);：

   • 这行代码创建了一个指向点云中 FPFH 特征的指针 fpfh，该特征是一个包含 33 个元素的向量。
   • pcl::PointCloud::Ptr 定义了指向 FPFH 特征的指针类型。

2. pcl::FPFHEstimationOMP f;：

   • 这行代码创建了一个 FPFH 估计器对象 f。
   • pcl::FPFHEstimationOMP 是一个用于并行计算 FPFH 特征的类。
   • 模板参数  指定了输入点云数据类型（pcl::PointXYZ）、法线数据类型（pcl::Normal）和输出 FPFH 特征的数据类型（pcl::FPFHSignature33）。

3. f.setIndices(inliers);：

   • 这行代码设置了要用于计算 FPFH 特征的点的索引。inliers 可能是一个点云中感兴趣区域的索引集合，可以选择性地计算特定区域的特征。

4. f.setNumberOfThreads(8);：

   • 这行代码设置了并行计算时要使用的线程数。在这个例子中，设置了 8 个线程。这有助于提高算法的运行速度，尤其是处理大型点云时。

5. f.setInputCloud(cloud);：

   • 这行代码设置了输入点云数据，即要计算特征的点云。

6. f.setInputNormals(normals);：

   • 这行代码设置了点云的法线数据，用于估计每个点的表面法线。

7. f.setSearchMethod(tree);：

   • 这行代码设置了搜索方法，用于确定每个点的邻居点。
   • tree 可能是一个用于快速最近邻搜索的数据结构，例如 KD 树。

8. f.setRadiusSearch(50);：

   • 这行代码设置了用于计算每个点的邻域半径，即在该半径范围内搜索邻居点来计算 FPFH 特征。半径较小会导致更局部的特征，而较大的半径会导致更全局的特征。

9. f.compute(*fpfh);：

   • 这行代码执行特征计算操作，将计算得到的 FPFH 特征存储在 fpfh 中。
10. 增加 setNumberOfThreads 中的线程数可以提高计算速度，但可能会增加内存和 CPU 开销。调整 setRadiusSearch 的搜索半径可以改变特征的局部或全局性。通过调整 setIndices 来限制计算范围，可以在需要时加快计算速度。

SAC-IA粗配准

pcl::PointCloud::Ptr sac_align(pcl::PointCloud::Ptr& cloud, pcl::PointCloud::Ptr s_k, pcl::PointCloud::Ptr t_k, pcl::PointCloud::Ptr sk_fpfh, pcl::PointCloud::Ptr tk_fpfh)
{
	pcl::SampleConsensusInitialAlignment scia;
	scia.setInputSource(s_k);
	scia.setInputTarget(t_k);
	scia.setSourceFeatures(sk_fpfh);
	scia.setTargetFeatures(tk_fpfh);
	scia.setMinSampleDistance(7);///参数：设置采样点之间的最小距离，满足的被当做采样点
	scia.setNumberOfSamples(100);设置每次迭代设置采样点的个数(这个参数多可以增加配准精度)
	scia.setCorrespondenceRandomness(6);//设置选择随机特征对应点时要使用的邻域点个数。值越大，特征匹配的随机性就越大
	pcl::PointCloud::Ptr sac_result(new pcl::PointCloud);
	scia.align(*sac_result);
	pcl::transformPointCloud(*cloud, *sac_result, scia.getFinalTransformation());
	return sac_result;
}

1. pcl::SampleConsensusInitialAlignment scia;：

   • 创建了 SAC-IA 对象，指定了输入和目标点云的类型以及用于匹配的特征类型（FPFHSignature33）。

2. scia.setInputSource(s_k);：

   • 设置源点云 s_k 作为配准的源。

3. scia.setInputTarget(t_k);：

   • 设置目标点云 t_k 作为配准的目标。

4. scia.setSourceFeatures(sk_fpfh);：

   • 设置源点云的特征，这里使用了之前计算得到的 FPFH 特征 sk_fpfh。

5. scia.setTargetFeatures(tk_fpfh);：

   • 设置目标点云的特征，这里使用了之前计算得到的 FPFH 特征 tk_fpfh。

6. scia.setMinSampleDistance(7);：

   • 设置采样点之间的最小距离，这个参数控制采样点的密度。较小的值可能导致更密集的采样点，从而增加计算复杂性，但提高匹配精度。

7. scia.setNumberOfSamples(100);：

   • 设置每次迭代设置采样点的个数，这个参数决定了每次迭代使用的样本数量。增加样本数量可能会提高匹配的精度，但也增加计算开销。

8. scia.setCorrespondenceRandomness(6);：

   • 设置选择随机特征对应点时要使用的邻域点个数。值越大，特征匹配的随机性就越大，这有助于在存在噪声或遮挡时更鲁棒地进行匹配。

9. pcl::PointCloud::Ptr sac_result(new pcl::PointCloud);：

   • 创建一个用于存储配准结果的点云对象。

10. scia.align(*sac_result);：

• 执行 SAC-IA 算法，将配准结果存储在 sac_result 中。

1. pcl::transformPointCloud(*cloud, *sac_result, scia.getFinalTransformation());：

• 将原始点云 cloud 根据最终的变换矩阵进行变换，得到配准后的点云。

1. return sac_result;：

• 返回配准后的点云结果。

调整这些参数可以影响配准的速度和精度。通常需要根据具体的数据和应用场景进行调试和评估。例如，通过调整 setMinSampleDistance、setNumberOfSamples 和 setCorrespondenceRandomness 可以在速度和精度之间找到平衡。

结果：

NARF关键点提取原理简介-CSDN博客