SHOT特征描述符、对应关系可视化以及ICP配准
一、SHOT特征描述符可视化
C++
#include
#include
#include
#include
#include //使用OMP需要添加的头文件
#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);//开始进行法向计算
// ------------------SHOT图像计算------------------
pcl::SHOTEstimationOMP descr_est;
pcl::PointCloud::Ptr shot_images(new pcl::PointCloud);
descr_est.setNumberOfThreads(4);
descr_est.setRadiusSearch(50); //设置搜索半径
descr_est.setInputCloud(cloud); //输入模型的关键点
descr_est.setInputNormals(normals); //输入模型的法线
descr_est.setSearchMethod(tree);
descr_est.compute(*shot_images);
cout << "SHOT图像计算计算完成" << endl;
// 显示和检索第一点的自旋图像描述符向量。
pcl::SHOT352 first_descriptor = shot_images->points[0];
cout << first_descriptor << endl;
pcl::PointCloud>::Ptr histograms(new pcl::PointCloud>);
// Accumulate histograms
for (int i = 0; i < shot_images->size(); ++i) {
pcl::Histogram<352> aggregated_histogram;
for (int j = 0; j < 352; ++j) {
aggregated_histogram.histogram[j] = (*shot_images)[i].descriptor[j];
}
histograms->push_back(aggregated_histogram);
}
pcl::visualization::PCLPlotter plotter;
plotter.addFeatureHistogram(*histograms, 400); //设置的横坐标长度，该值越大，则显示的越细致
plotter.setWindowName("SHOT Image");
plotter.plot();
return 0;
} 关键代码解析：
pcl::SHOTEstimationOMP descr_est;
pcl::PointCloud::Ptr shot_images(new pcl::PointCloud);
descr_est.setNumberOfThreads(4);
descr_est.setRadiusSearch(50); //设置搜索半径
descr_est.setInputCloud(cloud); //输入模型的关键点
descr_est.setInputNormals(normals); //输入模型的法线
descr_est.setSearchMethod(tree);
descr_est.compute(*shot_images);
cout << "SHOT图像计算计算完成" << endl;
// 显示和检索第一点的自旋图像描述符向量。
pcl::SHOT352 first_descriptor = shot_images->points[0];
cout << first_descriptor << endl;
pcl::PointCloud>::Ptr histograms(new pcl::PointCloud>);
// Accumulate histograms
for (int i = 0; i < shot_images->size(); ++i) {
pcl::Histogram<352> aggregated_histogram;
for (int j = 0; j < 352; ++j) {
aggregated_histogram.histogram[j] = (*shot_images)[i].descriptor[j];
}
histograms->push_back(aggregated_histogram);
} -
pcl::SHOTEstimationOMP- 创建了一个用于计算SHOT特征的对象
descr_est。这里使用了OMP（OpenMP）多线程加速。
- 创建了一个用于计算SHOT特征的对象
-
pcl::PointCloud- 创建了一个指向
pcl::PointCloud类型的智能指针shot_images，用于存储计算得到的SHOT特征。
- 创建了一个指向
-
descr_est.setNumberOfThreads(4);- 设置了并行计算的线程数为4。这个参数控制了在计算SHOT特征时使用的并行线程数量。
-
descr_est.setRadiusSearch(50);- 设置了用于计算SHOT特征的搜索半径为50个单位。这个参数决定了在计算每个点的SHOT特征时，应该考虑多远范围内的点。
-
descr_est.setInputCloud(cloud);- 设置了输入点云
cloud，其中包含了关键点。
- 设置了输入点云
-
descr_est.setInputNormals(normals);- 设置了输入法线
normals，用于计算SHOT描述子。
- 设置了输入法线
-
descr_est.setSearchMethod(tree);- 设置了搜索方法
tree，可能是一种用于加速搜索的数据结构，比如kd-tree。
- 设置了搜索方法
-
descr_est.compute(*shot_images);- 调用了
compute函数，开始计算SHOT特征。计算结果将存储在shot_images中。
- 调用了
-
pcl::SHOT352 first_descriptor = shot_images->points[0];- 获取第一个点的SHOT描述子。
-
下面的循环将SHOT特征存储在直方图中：
- 首先创建了一个指向
pcl::PointCloud类型的智能指针histograms，用于存储直方图。 - 然后遍历计算得到的SHOT特征，将每个特征的描述子存储在直方图中。
参数设置的影响：
- 线程数会影响计算的速度和系统资源的利用率。增加线程数可能会加快计算速度，但也会增加系统负担。
- 搜索半径决定了计算SHOT特征时考虑的邻域范围。如果搜索半径设置过小，可能会导致遗漏关键信息；如果设置过大，可能会增加计算的复杂度和耗时。
- 输入点云和法线的质量和数量直接影响了计算得到的SHOT特征的准确性和鲁棒性。
- 使用合适的搜索方法能够加快特征计算的速度，提高效率。
需要根据具体的应用场景和数据特点来选择合适的参数值，以达到较好的计算效果和性能。
结果：
二、SHOT对应关系可视化
C++
#include
#include
#include
#include
#include
#include
#include
#include
#include
typedef pcl::PointCloud pointcloud;
typedef pcl::PointCloud pointnormal;
typedef pcl::PointCloud shotFeature;
shotFeature::Ptr compute_shot_feature(pointcloud::Ptr input_cloud, pcl::search::KdTree::Ptr tree)
{
pointnormal::Ptr normals(new pointnormal);
pcl::NormalEstimationOMP n;
n.setInputCloud(input_cloud);
n.setNumberOfThreads(12);
n.setSearchMethod(tree);
n.setKSearch(30);
n.compute(*normals);
shotFeature::Ptr shot(new shotFeature);
pcl::SHOTEstimationOMP descr_est;
descr_est.setRadiusSearch(50); //设置搜索半径
descr_est.setInputCloud(input_cloud); //输入模型的关键点
descr_est.setInputNormals(normals); //输入模型的法线
descr_est.compute(*shot); //计算描述子
return shot;
}
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::search::KdTree::Ptr tree(new pcl::search::KdTree());
shotFeature::Ptr source_shot = compute_shot_feature(source_cloud, tree);
shotFeature::Ptr target_shot = compute_shot_feature(target_cloud, tree);
pcl::registration::CorrespondenceEstimation crude_cor_est;
boost::shared_ptr cru_correspondences(new pcl::Correspondences);
crude_cor_est.setInputSource(source_shot);
crude_cor_est.setInputTarget(target_shot);
crude_cor_est.determineCorrespondences(*cru_correspondences, 0.1);
Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity();
pcl::registration::TransformationEstimationSVD::Ptr trans(new pcl::registration::TransformationEstimationSVD);
trans->estimateRigidTransformation(*source_cloud, *target_cloud, *cru_correspondences, Transform);
cout << "变换矩阵为：\n" << Transform << endl;
boost::shared_ptrviewer(new pcl::visualization::PCLVisualizer("v"));
viewer->setBackgroundColor(0, 0, 0);
// 对目标点云着色可视化 (red).
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");
// 对源点云着色可视化 (green).
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(source_cloud, target_cloud, *cru_correspondences, "correspondence");
//viewer->initCameraParameters();
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
关键代码解析：
我之前在iss关键点检测以及SAC-IA粗配准-CSDN博客
和Spin Image自旋图像描述符可视化以及ICP配准-CSDN博客以及本章第一部分已经解释了大部分函数，这里就不赘述了
结果：
运行速度很慢，可以适当修改参数
三、SHOT结合ICP配准
C++
#include
#include
#include
#include
#include
#include
#include
#include
#include
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
typedef pcl::SHOT352 SHOTT;
typedef pcl::PointCloud PointCloudshot;
typedef pcl::search::KdTree Tree;
// 关键点提取
void extract_keypoint(PointCloud::Ptr& cloud, PointCloud::Ptr& keypoint, Tree::Ptr& tree)
{
pcl::ISSKeypoint3D iss;
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);
}
// 法线计算和 计算特征点的Spinimage描述子
void computeKeyPointsShot(PointCloud::Ptr& cloud_in, PointCloud::Ptr& key_cloud, PointCloudshot::Ptr& dsc, Tree::Ptr& tree)
{
pcl::NormalEstimationOMP n;//OMP加速
pcl::PointCloud::Ptr normals(new pcl::PointCloud);
n.setNumberOfThreads(6);//设置openMP的线程数
n.setInputCloud(key_cloud);
n.setSearchSurface(cloud_in);
n.setKSearch(30);
//n.setRadiusSearch(0.3);
n.compute(*normals);
//[pcl::SHOTEstimation::computeFeature] The local reference frame is not valid! Aborting description of point with index 1689
pcl::SHOTEstimationOMP descr_est;
descr_est.setNumberOfThreads(4);
descr_est.setRadiusSearch(110); //设置搜索半径
descr_est.setInputCloud(key_cloud); //输入模型的关键点
descr_est.setInputNormals(normals); //输入模型的法线
descr_est.setSearchMethod(tree);
descr_est.compute(*dsc);
}
// 点云可视化
void visualize_pcd(PointCloud::Ptr icp_result, PointCloud::Ptr cloud_target)
{
//创建初始化目标
pcl::visualization::PCLVisualizer viewer("registration Viewer");
pcl::visualization::PointCloudColorHandlerCustom final_h(icp_result, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom tgt_h(cloud_target, 255, 0, 0);
viewer.setBackgroundColor(0, 0, 0);
viewer.addPointCloud(cloud_target, tgt_h, "tgt cloud");
viewer.addPointCloud(icp_result, final_h, "final cloud");
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
int main()
{
// 加载源点云和目标点云
PointCloud::Ptr cloud(new PointCloud);
PointCloud::Ptr cloud_target(new 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");
}
visualize_pcd(cloud, cloud_target);
//关键点
pcl::PointCloud::Ptr keypoints1(new pcl::PointCloud);
pcl::PointCloud::Ptr keypoints2(new pcl::PointCloud);
Tree::Ptr tree(new Tree);
extract_keypoint(cloud, keypoints1, tree);
extract_keypoint(cloud_target, keypoints2, tree);
cout << "iss完成！" << endl;
cout << "cloud的关键点的个数：" << keypoints1->size() << endl;
cout << "cloud_target的关键点的个数：" << keypoints2->size() << endl;
// 使用SpinImage描述符计算特征
PointCloudshot::Ptr source_features(new PointCloudshot);
PointCloudshot::Ptr target_features(new PointCloudshot);
computeKeyPointsShot(cloud, keypoints1, source_features, tree);
computeKeyPointsShot(cloud_target, keypoints2, target_features, tree);
cout << "FPFH完成！" << endl;
//SAC配准
pcl::SampleConsensusInitialAlignment scia;
scia.setInputSource(keypoints1);
scia.setInputTarget(keypoints2);
scia.setSourceFeatures(source_features);
scia.setTargetFeatures(target_features);
scia.setMinSampleDistance(7); // 设置样本之间的最小距离
scia.setNumberOfSamples(100); // 设置每次迭代计算中使用的样本数量（可省）,可节省时间
scia.setCorrespondenceRandomness(6);// 在选择随机特征对应时，设置要使用的邻居的数量;
PointCloud::Ptr sac_result(new PointCloud);
scia.align(*sac_result);
Eigen::Matrix4f sac_trans;
sac_trans = scia.getFinalTransformation();
cout << "SAC配准完成！" << endl;
//icp配准
PointCloud::Ptr icp_result(new PointCloud);
pcl::IterativeClosestPoint icp;
icp.setInputSource(keypoints1);
icp.setInputTarget(keypoints2);
icp.setMaxCorrespondenceDistance(20);
icp.setMaximumIterations(35); // 最大迭代次数
icp.setTransformationEpsilon(1e-10); // 两次变化矩阵之间的差值
icp.setEuclideanFitnessEpsilon(0.01);// 均方误差
icp.align(*icp_result, sac_trans);
cout << "ICP配准完成！" << endl;
// 输出配准结果
std::cout << "ICP converged: " << icp.hasConverged() << std::endl;
std::cout << "Transformation matrix:\n" << icp.getFinalTransformation() << std::endl;
Eigen::Matrix4f icp_trans;
icp_trans = icp.getFinalTransformation();
cout << icp_trans << endl;
使用创建的变换对未过滤的输入点云进行变换
pcl::transformPointCloud(*cloud, *icp_result, icp_trans);
visualize_pcd(icp_result, cloud_target);
return 0;
}
关键代码解析：
我之前在iss关键点检测以及SAC-IA粗配准-CSDN博客
和Spin Image自旋图像描述符可视化以及ICP配准-CSDN博客以及本章第一部分已经解释了大部分函数，这里就不赘述了
结果：
