3DSC特征描述符、对应关系可视化以及ICP配准
一、3DSC特征描述符可视化
C++
#include
#include
#include
#include
#include //使用OMP需要添加的头文件
#include
#include
#include
#include // 直方图的可视化
#include
#include
using namespace std;
int main()
{
//------------------加载点云数据-----------------
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
if (pcl::io::loadPCDFile("pcd/pig_view1.pcd", *cloud) == -1)//需使用绝对路径
{
PCL_ERROR("Could not read file\n");
}
//--------------------计算法线------------------
pcl::NormalEstimationOMP n;//OMP加速
pcl::PointCloud::Ptr normals(new pcl::PointCloud);
//建立kdtree来进行近邻点集搜索
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
n.setNumberOfThreads(8);//设置openMP的线程数
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(10);
n.compute(*normals);//开始进行法向计算
// ------------------3DSC图像计算------------------
pcl::ShapeContext3DEstimation sc;
sc.setInputCloud(cloud);
sc.setInputNormals(normals);
sc.setSearchMethod(tree);
sc.setMinimalRadius(4);
sc.setPointDensityRadius(8);
pcl::PointCloud< pcl::ShapeContext1980>::Ptr dsc_images(new pcl::PointCloud< pcl::ShapeContext1980>);
sc.setRadiusSearch(40);
sc.compute(*dsc_images);
cout << "3DSC图像计算计算完成" << endl;
// 显示和检索第一点的自旋图像描述符向量。
pcl::ShapeContext1980 first_descriptor = dsc_images->points[0];
cout << first_descriptor << endl;
pcl::PointCloud>::Ptr histograms(new pcl::PointCloud>);
// Accumulate histograms
for (int i = 0; i < dsc_images->size(); ++i) {
pcl::Histogram<1980> aggregated_histogram;
for (int j = 0; j < 1980; ++j) {
aggregated_histogram.histogram[j] = (*dsc_images)[i].descriptor[j];
}
histograms->push_back(aggregated_histogram);
}
pcl::visualization::PCLPlotter plotter;
plotter.addFeatureHistogram(*histograms,1980); //设置的横坐标长度，该值越大，则显示的越细致
plotter.setWindowName("3DSC Image");
plotter.plot();
return 0;
} 关键代码解析：
pcl::NormalEstimationOMP n;//OMP加速
pcl::PointCloud::Ptr normals(new pcl::PointCloud);
//建立kdtree来进行近邻点集搜索
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
n.setNumberOfThreads(8);//设置openMP的线程数
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(10);
n.compute(*normals); -
pcl::NormalEstimationOMP：pcl::NormalEstimationOMP是一个用于估计点云法线的类，它利用了 OpenMP 进行多线程加速。pcl::PointXYZ，输出法线类型为pcl::Normal。
-
pcl::PointCloud：- 创建了一个指向存储法线的点云的指针。
-
pcl::search::KdTree：- 创建了一个 KdTree 对象，用于近邻搜索。
pcl::search::KdTree指定了 KdTree 使用的点类型。
-
n.setNumberOfThreads(8);：- 设置 OpenMP 的线程数为 8。这指定了在计算法线时要使用的并行线程数量。通常，设置为计算机的可用核心数或稍少一些是合理的。
-
n.setInputCloud(cloud);：- 设置输入点云，即需要估计法线的点云。
-
n.setSearchMethod(tree);：- 设置法线估计中使用的搜索方法，这里使用了建立好的 KdTree。
-
n.setKSearch(10);：- 设置用于估计每个点法线的最近邻点的数量。这里设置为 10，表示每个点的法线估计将使用其最近的 10 个邻居。
-
n.compute(*normals);：- 执行法线估计。
- 估计的法线将存储在
normals指向的点云中。
参数设置及其影响：
setNumberOfThreads(int num_threads)：通过设置并行计算线程数，可以加快法线估计的速度。但是，设置的线程数应该根据计算机的硬件配置来调整，过多的线程可能会造成资源浪费。setKSearch(int k)：决定了估计每个点法线时考虑的最近邻点的数量。更大的值将考虑更多的邻居，这可能导致更平滑的法线估计，但也可能增加计算时间。
pcl::ShapeContext3DEstimation sc;
sc.setInputCloud(cloud);
sc.setInputNormals(normals);
sc.setSearchMethod(tree);
sc.setMinimalRadius(4);
sc.setPointDensityRadius(8);
pcl::PointCloud< pcl::ShapeContext1980>::Ptr dsc_images(new pcl::PointCloud< pcl::ShapeContext1980>);
sc.setRadiusSearch(40);
// 计算spin image图像
sc.compute(*dsc_images); -
pcl::ShapeContext3DEstimation：- 创建了一个
pcl::ShapeContext3DEstimation类的对象sc，用于计算形状上下文。 pcl::PointXYZ指定了输入点云的类型。pcl::Normal指定了输入点云的法线类型。pcl::ShapeContext1980指定了形状上下文的类型。
- 创建了一个
-
sc.setInputCloud(cloud);：- 设置输入点云，即待计算形状上下文的点云。
-
sc.setInputNormals(normals);：- 设置输入点云的法线。
-
sc.setSearchMethod(tree);：- 设置用于形状上下文估计的搜索方法，这里使用了之前建立的 KdTree。
-
sc.setMinimalRadius(4);：- 设置形状上下文估计中的最小半径。这个参数决定了形状上下文描述符的大小。较小的值可能导致更精细的形状描述，但也可能增加计算开销。
-
sc.setPointDensityRadius(8);：- 设置用于计算点云形状上下文时的点密度半径。这个参数控制着在形状上下文计算中用于描述点云局部形状的密度。较大的值将考虑更广泛的区域，可能导致更全局的形状描述。
-
pcl::PointCloud< pcl::ShapeContext1980>::Ptr dsc_images(new pcl::PointCloud< pcl::ShapeContext1980>);：- 创建了一个指向存储形状上下文描述符的点云的指针。
-
sc.setRadiusSearch(40);：- 设置用于形状上下文计算的搜索半径。这个参数决定了在形状上下文计算中考虑的邻居点的数量。较大的值将考虑更广泛的邻域，但可能会增加计算时间。
-
sc.compute(*dsc_images);：- 执行形状上下文计算。
- 计算得到的形状上下文描述符将存储在
dsc_images指向的点云中。
参数设置及其影响：
setMinimalRadius(double radius)：设置形状上下文计算中的最小半径。较小的值可能导致更精细的形状描述，但也可能增加计算开销。setPointDensityRadius(double radius)：设置用于计算形状上下文时的点密度半径。较大的值将考虑更广泛的区域，可能导致更全局的形状描述。setRadiusSearch(double radius)：设置形状上下文计算中的搜索半径。较大的值将考虑更广泛的邻域，但可能会增加计算时间。
pcl::PointCloud>::Ptr histograms(new pcl::PointCloud>);
// Accumulate histograms
for (int i = 0; i < dsc_images->size(); ++i) {
pcl::Histogram<1980> aggregated_histogram;
for (int j = 0; j < 1980; ++j) {
aggregated_histogram.histogram[j] = (*dsc_images)[i].descriptor[j];
}
histograms->push_back(aggregated_histogram);
} -
pcl::PointCloud：- 创建了一个指向存储直方图的点云的指针。
-
for (int i = 0; i < dsc_images->size(); ++i) {：- 循环遍历形状上下文描述符点云中的每个点。
-
pcl::Histogram<1980> aggregated_histogram;：- 创建了一个用于存储聚合直方图的对象。
-
for (int j = 0; j < 1980; ++j) {：- 循环遍历每个形状上下文描述符的维度。
-
aggregated_histogram.histogram[j] = (*dsc_images)[i].descriptor[j];：- 将形状上下文描述符的每个维度的值赋给聚合直方图中对应维度的值。
-
histograms->push_back(aggregated_histogram);：- 将聚合后的直方图添加到直方图点云中。
参数设置及其影响：
- 这段代码中没有涉及显式的参数设置，但是需要注意的是：
1980表示每个形状上下文描述符的维度。这个值应与前面计算形状上下文时所使用的描述符类型中的维度相匹配。
结果：
注意：运行速度很慢
二、3DSC对应关系可视化
C++
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
typedef pcl::PointCloud pointcloud;
typedef pcl::PointCloud pointnormal;
typedef pcl::PointCloud DSCFeature;
DSCFeature::Ptr compute_pfh_feature(pointcloud::Ptr input_cloud, pcl::search::KdTree::Ptr tree)
{
pointnormal::Ptr normals(new pointnormal);
pcl::NormalEstimationOMP n;
n.setInputCloud(input_cloud);
n.setNumberOfThreads(5);
n.setSearchMethod(tree);
n.setKSearch(10);
n.compute(*normals);
pcl::PointCloud::Ptr dsc_fe_dsc(new pcl::PointCloud());
pcl::ShapeContext3DEstimation sc;
sc.setInputCloud(input_cloud);
sc.setInputNormals(normals);
//kdTree加速
sc.setSearchMethod(tree);
sc.setMinimalRadius(4); // 搜索球面(Rmin)的最小半径值。
sc.setRadiusSearch(40); // 设置用于确定用于特征估计的最近邻居的球体半径。
sc.setPointDensityRadius(8);// 这个半径用于计算局部点密度=这个半径内的点数。
sc.compute(*dsc_fe_dsc);
return dsc_fe_dsc;
}
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);
}
int main(int argc, char** argv)
{
pointcloud::Ptr source_cloud(new pointcloud);
pointcloud::Ptr target_cloud(new pointcloud);
pcl::io::loadPCDFile("pcd/pig_view1.pcd", *source_cloud);
pcl::io::loadPCDFile("pcd/pig_view2.pcd", *target_cloud);
pcl::PointCloud::Ptr s_k(new pcl::PointCloud);
pcl::PointCloud::Ptr t_k(new pcl::PointCloud);
extract_keypoint(source_cloud, s_k);
extract_keypoint(target_cloud, t_k);
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
DSCFeature::Ptr source_pfh = compute_pfh_feature(s_k, tree);
DSCFeature::Ptr target_pfh = compute_pfh_feature(t_k, tree);
pcl::registration::CorrespondenceEstimation crude_cor_est;
boost::shared_ptr cru_correspondences(new pcl::Correspondences);
crude_cor_est.setInputSource(source_pfh);
crude_cor_est.setInputTarget(target_pfh);
crude_cor_est.determineCorrespondences(*cru_correspondences);
Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity();
pcl::registration::TransformationEstimationSVD::Ptr trans(new pcl::registration::TransformationEstimationSVD);
trans->estimateRigidTransformation(*source_cloud, *target_cloud, *cru_correspondences, Transform);
boost::shared_ptrviewer(new pcl::visualization::PCLVisualizer("v1"));
viewer->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustomtarget_color(target_cloud, 255, 0, 0);
viewer->addPointCloud(target_cloud, target_color, "target cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
pcl::visualization::PointCloudColorHandlerCustominput_color(source_cloud, 0, 255, 0);
viewer->addPointCloud(source_cloud, input_color, "input cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");
viewer->addCorrespondences(s_k, t_k, *cru_correspondences, "correspondence");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
关键代码解析：
pcl::registration::CorrespondenceEstimation crude_cor_est;
boost::shared_ptr cru_correspondences(new pcl::Correspondences);
crude_cor_est.setInputSource(source_pfh);
crude_cor_est.setInputTarget(target_pfh);
crude_cor_est.determineCorrespondences(*cru_correspondences);
Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity();
pcl::registration::TransformationEstimationSVD::Ptr trans(new pcl::registration::TransformationEstimationSVD);
trans->estimateRigidTransformation(*source_cloud, *target_cloud, *cru_correspondences, Transform); -
pcl::registration::CorrespondenceEstimation：- 创建了一个形状上下文描述符之间的对应估计对象。这里使用的是形状上下文描述符类型
pcl::ShapeContext1980。 - 参数设置影响：这里没有显式地设置参数，但是你可以根据需要调整匹配算法的参数，如距离阈值、特征匹配方式等，以获得更好的配准效果。
- 创建了一个形状上下文描述符之间的对应估计对象。这里使用的是形状上下文描述符类型
-
boost::shared_ptr：- 创建了一个存储对应关系的指针，用于存储形状上下文描述符之间的对应关系。
-
crude_cor_est.setInputSource(source_pfh);和crude_cor_est.setInputTarget(target_pfh);：- 设置了待配准的源点云和目标点云的形状上下文描述符。
- 参数设置影响：这里的
source_pfh和target_pfh应该是已经计算好的形状上下文描述符，参数的设置会直接影响到配准的准确度和鲁棒性。
-
crude_cor_est.determineCorrespondences(*cru_correspondences);：- 执行对应关系的估计，将对应关系存储到
cru_correspondences中。 - 参数设置影响：在这一步中，配准的结果受到匹配算法的影响，包括特征匹配的算法、距离阈值等参数设置。
- 执行对应关系的估计，将对应关系存储到
-
Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity();：- 创建了一个单位矩阵，用于存储计算得到的变换矩阵。
-
pcl::registration::TransformationEstimationSVD：- 创建了一个利用奇异值分解（SVD）方法估计变换的对象。
- 参数设置影响：这里的
pcl::PointXYZ表示点的类型，可以根据实际情况选择合适的点类型，例如pcl::PointNormal或者pcl::PointXYZRGB等，以确保变换估计的准确性。
-
trans->estimateRigidTransformation(*source_cloud, *target_cloud, *cru_correspondences, Transform);：- 使用估计的对应关系和变换方法计算源点云到目标点云的刚性变换。
- 参数设置影响：这里的参数包括源点云、目标点云、对应关系以及变换矩阵。调整这些参数可以影响配准的结果，如不同的匹配算法、不同的点云特征类型等。
结果：
三、3DSC结合ICP配准
C++
#include
#include
#include
#include
#include
#include
#include
#include // 采取固定数量的点云
#include // 采样一致性
#include // icp配准
#include
#include // 可视化
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
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(7); // 设置用于计算协方差矩阵的球邻域半径
iss.setNonMaxRadius(5); // 设置非极大值抑制应用算法的半径
iss.setThreshold21(0.95); // 设定第二个和第一个特征值之比的上限
iss.setThreshold32(0.95); // 设定第三个和第二个特征值之比的上限
iss.setMinNeighbors(6); // 在应用非极大值抑制算法时，设置必须找到的最小邻居数
iss.compute(*keypoint);
}
// 点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt)
{
//创建初始化目标
pcl::visualization::PCLVisualizer viewer("registration Viewer");
pcl::visualization::PointCloudColorHandlerCustom src_h(pcd_src, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom tgt_h(pcd_tgt, 255, 0, 0);
viewer.setBackgroundColor(0, 0, 0);
viewer.addPointCloud(pcd_src, src_h, "source cloud");
viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(1000));
}
}
// 计算特征点的3DSC描述子
void computeKeyPoints3DSC(PointCloud::Ptr& cloud_in, PointCloud::Ptr& key_cloud, pcl::PointCloud::Ptr& dsc)
{
//------------------计算法线----------------------
pcl::NormalEstimationOMP n;//OMP加速
pcl::PointCloud::Ptr normals(new pcl::PointCloud);
//建立kdtree来进行近邻点集搜索
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
n.setNumberOfThreads(6);//设置openMP的线程数
n.setInputCloud(key_cloud);
n.setSearchSurface(cloud_in);
n.setSearchMethod(tree);
n.setKSearch(20);
//n.setRadiusSearch(0.03);//半径搜素
n.compute(*normals);
cout << "法线计算完毕！！！" << endl;
//-------------------计算3dsc-----------------------
pcl::ShapeContext3DEstimation sc;
sc.setInputCloud(key_cloud);
sc.setInputNormals(normals);
//kdTree加速
sc.setSearchMethod(tree);
sc.setMinimalRadius(4); // 搜索球面(Rmin)的最小半径值。
sc.setRadiusSearch(40); // 设置用于确定用于特征估计的最近邻居的球体半径。
sc.setPointDensityRadius(8);// 这个半径用于计算局部点密度=这个半径内的点数。
sc.compute(*dsc);
cout << "3DSC特征描述子计算完毕！！！" << endl;
}
int main(int argc, char** argv)
{
// 加载点云文件
PointCloud::Ptr source(new PointCloud); // 源点云，待配准
pcl::io::loadPCDFile("pcd/pig_view1.pcd", *source);
PointCloud::Ptr target(new PointCloud); // 目标点云
pcl::io::loadPCDFile("pcd/pig_view2.pcd", *target);
PointCloud::Ptr key_src(new PointCloud);
PointCloud::Ptr key_tgt(new PointCloud);
extract_keypoint(source, key_src);
extract_keypoint(target, key_tgt);
//计算3dsc
pcl::PointCloud::Ptr sps_src(new pcl::PointCloud());
pcl::PointCloud::Ptr sps_tgt(new pcl::PointCloud());
computeKeyPoints3DSC(source, key_src, sps_src);
computeKeyPoints3DSC(target, key_tgt, sps_tgt);
//SAC配准
pcl::SampleConsensusInitialAlignment scia;
scia.setInputSource(key_src);
scia.setInputTarget(key_tgt);
scia.setSourceFeatures(sps_src);
scia.setTargetFeatures(sps_tgt);
scia.setMinSampleDistance(7); // 设置样本之间的最小距离
scia.setNumberOfSamples(100); // 设置每次迭代计算中使用的样本数量（可省）,可节省时间
scia.setCorrespondenceRandomness(6);// 在选择随机特征对应时，设置要使用的邻居的数量;
PointCloud::Ptr sac_result(new PointCloud);
scia.align(*sac_result);
std::cout << "sac has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl;
Eigen::Matrix4f sac_trans;
sac_trans = scia.getFinalTransformation();
std::cout << sac_trans << endl;
//icp配准
PointCloud::Ptr icp_result(new PointCloud);
pcl::IterativeClosestPoint icp;
icp.setInputSource(key_src);
icp.setInputTarget(key_tgt);
icp.setMaxCorrespondenceDistance(20);
icp.setMaximumIterations(35); // 最大迭代次数
icp.setTransformationEpsilon(1e-10); // 两次变化矩阵之间的差值
icp.setEuclideanFitnessEpsilon(0.01);// 均方误差
icp.align(*icp_result, sac_trans);
cout << "ICP has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << endl;
Eigen::Matrix4f icp_trans;
icp_trans = icp.getFinalTransformation();
cout << icp_trans << endl;
//使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud(*source, *icp_result, icp_trans);
//可视化
visualize_pcd(icp_result, target);
return (0);
}
关键代码解析：
我之前在iss关键点检测以及SAC-IA粗配准-CSDN博客
和Spin Image自旋图像描述符可视化以及ICP配准-CSDN博客以及本章第一部分已经解释了大部分函数，这里就不赘述了
结果：
配准结果，程序运行很慢，要等好久好久，可以适当调整参数，加快速度
