SHOT特征描述符、对应关系可视化以及ICP配准

一、SHOT特征描述符可视化

C++

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d_omp.h>//使用OMP需要添加的头文件
#include <boost/thread/thread.hpp>
#include <pcl/features/shot_omp.h>
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
using namespace std;
int main()
{
	//------------------加载点云数据-----------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1)//需使用绝对路径
	{
		PCL_ERROR("Could not read file\n");
	}

	//--------------------计算法线------------------
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	n.setNumberOfThreads(8);//设置openMP的线程数
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(10);
	n.compute(*normals);//开始进行法向计算

	// ------------------SHOT图像计算------------------
	pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> descr_est;
	pcl::PointCloud<pcl::SHOT352>::Ptr shot_images(new pcl::PointCloud<pcl::SHOT352>);
	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<pcl::Histogram<352>>::Ptr histograms(new pcl::PointCloud<pcl::Histogram<352>>);
	// 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<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> descr_est;
	pcl::PointCloud<pcl::SHOT352>::Ptr shot_images(new pcl::PointCloud<pcl::SHOT352>);
	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<pcl::Histogram<352>>::Ptr histograms(new pcl::PointCloud<pcl::Histogram<352>>);
	// 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);
	}

1. pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> descr_est;

   • 创建了一个用于计算SHOT特征的对象descr_est。这里使用了OMP（OpenMP）多线程加速。

2. pcl::PointCloud<pcl::SHOT352>::Ptr shot_images(new pcl::PointCloud<pcl::SHOT352>);

   • 创建了一个指向pcl::PointCloud<pcl::SHOT352>类型的智能指针shot_images，用于存储计算得到的SHOT特征。

3. descr_est.setNumberOfThreads(4);

   • 设置了并行计算的线程数为4。这个参数控制了在计算SHOT特征时使用的并行线程数量。

4. descr_est.setRadiusSearch(50);

   • 设置了用于计算SHOT特征的搜索半径为50个单位。这个参数决定了在计算每个点的SHOT特征时，应该考虑多远范围内的点。

5. descr_est.setInputCloud(cloud);

   • 设置了输入点云cloud，其中包含了关键点。

6. descr_est.setInputNormals(normals);

   • 设置了输入法线normals，用于计算SHOT描述子。

7. descr_est.setSearchMethod(tree);

   • 设置了搜索方法tree，可能是一种用于加速搜索的数据结构，比如kd-tree。

8. descr_est.compute(*shot_images);

   • 调用了compute函数，开始计算SHOT特征。计算结果将存储在shot_images中。

9. pcl::SHOT352 first_descriptor = shot_images->points[0];

   • 获取第一个点的SHOT描述子。

10. 下面的循环将SHOT特征存储在直方图中：

• 首先创建了一个指向pcl::PointCloud<pcl::Histogram<352>>类型的智能指针histograms，用于存储直方图。
• 然后遍历计算得到的SHOT特征，将每个特征的描述子存储在直方图中。

参数设置的影响：

• 线程数会影响计算的速度和系统资源的利用率。增加线程数可能会加快计算速度，但也会增加系统负担。
• 搜索半径决定了计算SHOT特征时考虑的邻域范围。如果搜索半径设置过小，可能会导致遗漏关键信息；如果设置过大，可能会增加计算的复杂度和耗时。
• 输入点云和法线的质量和数量直接影响了计算得到的SHOT特征的准确性和鲁棒性。
• 使用合适的搜索方法能够加快特征计算的速度，提高效率。

需要根据具体的应用场景和数据特点来选择合适的参数值，以达到较好的计算效果和性能。

结果：

二、SHOT对应关系可视化

C++

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/registration/correspondence_estimation.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/transformation_estimation_svd.h> 


typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::Normal> pointnormal;
typedef pcl::PointCloud<pcl::SHOT352> shotFeature;

shotFeature::Ptr compute_shot_feature(pointcloud::Ptr input_cloud, pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
{
    pointnormal::Ptr normals(new pointnormal);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(input_cloud);
    n.setNumberOfThreads(12);
    n.setSearchMethod(tree);
    n.setKSearch(30);
    n.compute(*normals);

    shotFeature::Ptr shot(new shotFeature);
    pcl::SHOTEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> 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<pcl::PointXYZ>("pcd/pig_view1.pcd", *source_cloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *target_cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    shotFeature::Ptr source_shot = compute_shot_feature(source_cloud, tree);
    shotFeature::Ptr target_shot = compute_shot_feature(target_cloud, tree);
    pcl::registration::CorrespondenceEstimation<pcl::SHOT352, pcl::SHOT352> crude_cor_est;
    boost::shared_ptr<pcl::Correspondences> 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<pcl::PointXYZ, pcl::PointXYZ, float>::Ptr trans(new pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ, float>);

    trans->estimateRigidTransformation(*source_cloud, *target_cloud, *cru_correspondences, Transform);

    cout << "变换矩阵为：\n" << Transform << endl;


    boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("v"));
    viewer->setBackgroundColor(0, 0, 0);
    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target_cloud, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    // 对源点云着色可视化 (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color(source_cloud, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(source_cloud, input_color, "input cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");
    //对应关系可视化
    viewer->addCorrespondences<pcl::PointXYZ>(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 <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/features/shot_omp.h>
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::SHOT352 SHOTT;
typedef pcl::PointCloud<SHOTT> PointCloudshot;
typedef pcl::search::KdTree<PointT> Tree;

// 关键点提取
void extract_keypoint(PointCloud::Ptr& cloud, PointCloud::Ptr& keypoint, Tree::Ptr& tree)
{
    pcl::ISSKeypoint3D<PointT, PointT> 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<PointT, pcl::Normal> n;//OMP加速
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    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<pcl::PointXYZ, pcl::Normal, pcl::SHOT352> 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<PointT> final_h(icp_result, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> 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<pcl::PointXYZ>("pcd/pig_view1.pcd", *cloud) == -1)
    {
        PCL_ERROR("加载点云失败\n");
    }
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *cloud_target) == -1)
    {
        PCL_ERROR("加载点云失败\n");
    }
    visualize_pcd(cloud, cloud_target);
    //关键点
    pcl::PointCloud<PointT>::Ptr keypoints1(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<PointT>::Ptr keypoints2(new pcl::PointCloud<pcl::PointXYZ>);
    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<PointT, PointT, SHOTT> 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<PointT, PointT> 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博客以及本章第一部分已经解释了大部分函数，这里就不赘述了

结果：