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

jjm20022024-02-19 13:51

一、3DSC特征描述符可视化

C++

cpp 复制代码 
#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 <pcl/features/spin_image.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/3dsc.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);//开始进行法向计算

	// ------------------3DSC图像计算------------------
	pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> 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<pcl::Histogram<1980>>::Ptr histograms(new pcl::PointCloud<pcl::Histogram<1980>>);
	// 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;
}

关键代码解析：

cpp 复制代码 
    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);

  1. pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;：

    • pcl::NormalEstimationOMP 是一个用于估计点云法线的类，它利用了 OpenMP 进行多线程加速。
    • <pcl::PointXYZ, pcl::Normal> 指定输入点云类型为 pcl::PointXYZ，输出法线类型为 pcl::Normal。

  2. pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);：

    • 创建了一个指向存储法线的点云的指针。

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

    • 创建了一个 KdTree 对象，用于近邻搜索。
    • pcl::search::KdTree<pcl::PointXYZ> 指定了 KdTree 使用的点类型。

  4. n.setNumberOfThreads(8);：

    • 设置 OpenMP 的线程数为 8。这指定了在计算法线时要使用的并行线程数量。通常，设置为计算机的可用核心数或稍少一些是合理的。

  5. n.setInputCloud(cloud);：

    • 设置输入点云，即需要估计法线的点云。

  6. n.setSearchMethod(tree);：

    • 设置法线估计中使用的搜索方法，这里使用了建立好的 KdTree。

  7. n.setKSearch(10);：

    • 设置用于估计每个点法线的最近邻点的数量。这里设置为 10，表示每个点的法线估计将使用其最近的 10 个邻居。

  8. n.compute(*normals);：

    • 执行法线估计。
    • 估计的法线将存储在 normals 指向的点云中。

参数设置及其影响：

  • setNumberOfThreads(int num_threads)：通过设置并行计算线程数，可以加快法线估计的速度。但是，设置的线程数应该根据计算机的硬件配置来调整，过多的线程可能会造成资源浪费。
  • setKSearch(int k)：决定了估计每个点法线时考虑的最近邻点的数量。更大的值将考虑更多的邻居，这可能导致更平滑的法线估计，但也可能增加计算时间。
cpp 复制代码 
	pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> 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);

  1. pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> sc;：

    • 创建了一个 pcl::ShapeContext3DEstimation 类的对象 sc，用于计算形状上下文。
    • pcl::PointXYZ 指定了输入点云的类型。
    • pcl::Normal 指定了输入点云的法线类型。
    • pcl::ShapeContext1980 指定了形状上下文的类型。

  2. sc.setInputCloud(cloud);：

    • 设置输入点云，即待计算形状上下文的点云。

  3. sc.setInputNormals(normals);：

    • 设置输入点云的法线。

  4. sc.setSearchMethod(tree);：

    • 设置用于形状上下文估计的搜索方法，这里使用了之前建立的 KdTree。

  5. sc.setMinimalRadius(4);：

    • 设置形状上下文估计中的最小半径。这个参数决定了形状上下文描述符的大小。较小的值可能导致更精细的形状描述，但也可能增加计算开销。

  6. sc.setPointDensityRadius(8);：

    • 设置用于计算点云形状上下文时的点密度半径。这个参数控制着在形状上下文计算中用于描述点云局部形状的密度。较大的值将考虑更广泛的区域，可能导致更全局的形状描述。

  7. pcl::PointCloud< pcl::ShapeContext1980>::Ptr dsc_images(new pcl::PointCloud< pcl::ShapeContext1980>);：

    • 创建了一个指向存储形状上下文描述符的点云的指针。

  8. sc.setRadiusSearch(40);：

    • 设置用于形状上下文计算的搜索半径。这个参数决定了在形状上下文计算中考虑的邻居点的数量。较大的值将考虑更广泛的邻域，但可能会增加计算时间。

  9. sc.compute(*dsc_images);：

    • 执行形状上下文计算。
    • 计算得到的形状上下文描述符将存储在 dsc_images 指向的点云中。

参数设置及其影响：

  • setMinimalRadius(double radius)：设置形状上下文计算中的最小半径。较小的值可能导致更精细的形状描述，但也可能增加计算开销。
  • setPointDensityRadius(double radius)：设置用于计算形状上下文时的点密度半径。较大的值将考虑更广泛的区域，可能导致更全局的形状描述。
  • setRadiusSearch(double radius)：设置形状上下文计算中的搜索半径。较大的值将考虑更广泛的邻域，但可能会增加计算时间。
cpp 复制代码 
	pcl::PointCloud<pcl::Histogram<1980>>::Ptr histograms(new pcl::PointCloud<pcl::Histogram<1980>>);
	// 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);
	}

  1. pcl::PointCloud<pcl::Histogram<1980>>::Ptr histograms(new pcl::PointCloud<pcl::Histogram<1980>>);：

    • 创建了一个指向存储直方图的点云的指针。

  2. for (int i = 0; i < dsc_images->size(); ++i) {：

    • 循环遍历形状上下文描述符点云中的每个点。

  3. pcl::Histogram<1980> aggregated_histogram;：

    • 创建了一个用于存储聚合直方图的对象。

  4. for (int j = 0; j < 1980; ++j) {：

    • 循环遍历每个形状上下文描述符的维度。

  5. aggregated_histogram.histogram[j] = (*dsc_images)[i].descriptor[j];：

    • 将形状上下文描述符的每个维度的值赋给聚合直方图中对应维度的值。

  6. histograms->push_back(aggregated_histogram);：

    • 将聚合后的直方图添加到直方图点云中。

参数设置及其影响：

  • 这段代码中没有涉及显式的参数设置，但是需要注意的是：
    • 1980 表示每个形状上下文描述符的维度。这个值应与前面计算形状上下文时所使用的描述符类型中的维度相匹配。

结果：

注意：运行速度很慢

二、3DSC对应关系可视化

C++

cpp 复制代码 
#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/registration/correspondence_estimation.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/transformation_estimation_svd.h> 
#include <pcl/features/3dsc.h>
#include <pcl/keypoints/iss_3d.h>
using namespace std;



typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::Normal> pointnormal;
typedef pcl::PointCloud<pcl::ShapeContext1980> DSCFeature;

DSCFeature::Ptr compute_pfh_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(5);
    n.setSearchMethod(tree);
    n.setKSearch(10);
    n.compute(*normals);


    pcl::PointCloud<pcl::ShapeContext1980>::Ptr dsc_fe_dsc(new pcl::PointCloud<pcl::ShapeContext1980>());
    pcl::ShapeContext3DEstimation<pcl::PointXYZ, pcl::Normal, pcl::ShapeContext1980> 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<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{
    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    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<pcl::PointXYZ>("pcd/pig_view1.pcd", *source_cloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("pcd/pig_view2.pcd", *target_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr s_k(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_k(new pcl::PointCloud<pcl::PointXYZ>);
    extract_keypoint(source_cloud, s_k);
    extract_keypoint(target_cloud, t_k);


    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    DSCFeature::Ptr source_pfh = compute_pfh_feature(s_k, tree);
    DSCFeature::Ptr target_pfh = compute_pfh_feature(t_k, tree);
    pcl::registration::CorrespondenceEstimation<pcl::ShapeContext1980, pcl::ShapeContext1980> crude_cor_est;
    boost::shared_ptr<pcl::Correspondences> 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<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);



    boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("v1"));
    viewer->setBackgroundColor(0, 0, 0);
    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");
    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>(s_k, t_k, *cru_correspondences, "correspondence");
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }


    return 0;
}

关键代码解析：

cpp 复制代码 
    pcl::registration::CorrespondenceEstimation<pcl::ShapeContext1980, pcl::ShapeContext1980> crude_cor_est;
    boost::shared_ptr<pcl::Correspondences> 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<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);

  1. pcl::registration::CorrespondenceEstimation<pcl::ShapeContext1980, pcl::ShapeContext1980> crude_cor_est;：

    • 创建了一个形状上下文描述符之间的对应估计对象。这里使用的是形状上下文描述符类型 pcl::ShapeContext1980。
    • 参数设置影响：这里没有显式地设置参数，但是你可以根据需要调整匹配算法的参数，如距离阈值、特征匹配方式等，以获得更好的配准效果。

  2. boost::shared_ptr<pcl::Correspondences> cru_correspondences(new pcl::Correspondences);：

    • 创建了一个存储对应关系的指针，用于存储形状上下文描述符之间的对应关系。

  3. crude_cor_est.setInputSource(source_pfh); 和 crude_cor_est.setInputTarget(target_pfh);：

    • 设置了待配准的源点云和目标点云的形状上下文描述符。
    • 参数设置影响：这里的 source_pfh 和 target_pfh 应该是已经计算好的形状上下文描述符，参数的设置会直接影响到配准的准确度和鲁棒性。

  4. crude_cor_est.determineCorrespondences(*cru_correspondences);：

    • 执行对应关系的估计，将对应关系存储到 cru_correspondences 中。
    • 参数设置影响：在这一步中，配准的结果受到匹配算法的影响，包括特征匹配的算法、距离阈值等参数设置。

  5. Eigen::Matrix4f Transform = Eigen::Matrix4f::Identity();：

    • 创建了一个单位矩阵，用于存储计算得到的变换矩阵。

  6. pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ, float>::Ptr trans(new pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ, float>);：

    • 创建了一个利用奇异值分解（SVD）方法估计变换的对象。
    • 参数设置影响：这里的 pcl::PointXYZ 表示点的类型，可以根据实际情况选择合适的点类型，例如 pcl::PointNormal 或者 pcl::PointXYZRGB 等，以确保变换估计的准确性。

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

    • 使用估计的对应关系和变换方法计算源点云到目标点云的刚性变换。
    • 参数设置影响：这里的参数包括源点云、目标点云、对应关系以及变换矩阵。调整这些参数可以影响配准的结果，如不同的匹配算法、不同的点云特征类型等。

结果：

三、3DSC结合ICP配准

C++

cpp 复制代码 
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/3dsc.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/random_sample.h> // 采取固定数量的点云
#include <pcl/registration/ia_ransac.h>// 采样一致性
#include <pcl/registration/icp.h>      // icp配准
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h> // 可视化


typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
void extract_keypoint(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& keypoint)
{
	pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	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<PointT> src_h(pcd_src, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<PointT> 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<pcl::ShapeContext1980>::Ptr& dsc)
{
	//------------------计算法线----------------------
	pcl::NormalEstimationOMP<PointT, 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(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<PointT, pcl::Normal, pcl::ShapeContext1980> 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<pcl::ShapeContext1980>::Ptr sps_src(new pcl::PointCloud<pcl::ShapeContext1980>());
	pcl::PointCloud<pcl::ShapeContext1980>::Ptr sps_tgt(new pcl::PointCloud<pcl::ShapeContext1980>());
	computeKeyPoints3DSC(source, key_src, sps_src);
	computeKeyPoints3DSC(target, key_tgt, sps_tgt);

	//SAC配准
	pcl::SampleConsensusInitialAlignment<PointT, PointT, pcl::ShapeContext1980> 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<PointT, PointT> 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博客以及本章第一部分已经解释了大部分函数，这里就不赘述了

结果：

配准结果，程序运行很慢，要等好久好久，可以适当调整参数，加快速度

相关推荐
唐诺3 小时前
几种广泛使用的 C++ 编译器
c++·编译器
冷眼看人间恩怨4 小时前
【Qt笔记】QDockWidget控件详解
c++·笔记·qt·qdockwidget
红龙创客4 小时前
某狐畅游24校招-C++开发岗笔试（单选题）
开发语言·c++
Lenyiin4 小时前
第146场双周赛：统计符合条件长度为3的子数组数目、统计异或值为给定值的路径数目、判断网格图能否被切割成块、唯一中间众数子序列 Ⅰ
c++·算法·leetcode·周赛·lenyiin
yuanbenshidiaos6 小时前
c++---------数据类型
java·jvm·c++
十年一梦实验室6 小时前
【C++】sophus : sim_details.hpp 实现了矩阵函数 W、其导数，以及其逆 (十七)
开发语言·c++·线性代数·矩阵
taoyong0016 小时前
代码随想录算法训练营第十一天-239.滑动窗口最大值
c++·算法
这是我587 小时前
C++打小怪游戏
c++·其他·游戏·visual studio·小怪·大型·怪物
fpcc7 小时前
跟我学c++中级篇——C++中的缓存利用
c++·缓存
呆萌很7 小时前
C++ 集合 list 使用
c++
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02ARM学习（31）编译器对overlay方式的支持03玄机平台应急响应—webshell查杀04DisplayPort AltMode On Type-c协议_Ver2.0(学习笔记)05校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站06Docker 夺命连环 15 问076个主流的工业3D管道设计软件08基于大语言模型（LLM）的合成数据生成、策展和评估的综述09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10树莓派 5 AI 套件（Hailo-8L）使用教程