Spin Image自旋图像描述符可视化以及ICP配准

一、Spin Image自旋图像描述符可视化

C++

#include 
#include 
#include 
#include 
#include //使用OMP需要添加的头文件
#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(6);//设置openMP的线程数
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setRadiusSearch(0.3);//半径搜素
	n.compute(*normals);//开始进行法向计算

	// ------------------spin image图像计算------------------
	pcl::SpinImageEstimation > spin_image_descriptor(8, 0.05, 10);
	//三个数字分别表示：旋转图像分辨率；最小允许输入点与搜索曲面点之间的法线夹角的余弦值，以便在支撑中保留该点；
	//小支持点数，以正确估计自旋图像。如果在某个点支持包含的点较少，则抛出异常
	spin_image_descriptor.setInputCloud(cloud);
	spin_image_descriptor.setInputNormals(normals);
	// 使用法线计算的KD树
	spin_image_descriptor.setSearchMethod(tree);
	pcl::PointCloud >::Ptr spin_images(new pcl::PointCloud >);
	spin_image_descriptor.setRadiusSearch(40);
	// 计算spin image图像
	spin_image_descriptor.compute(*spin_images);
	cout << "SI output points.size (): " << spin_images->points.size() << endl;

	// 显示和检索第一点的自旋图像描述符向量。
	pcl::Histogram<153> first_descriptor = spin_images->points[0];
	cout << first_descriptor << endl;

	//========自旋图像描述符可视化=============================

	pcl::visualization::PCLPlotter plotter;
	plotter.addFeatureHistogram(*spin_images, 600); //设置的横坐标长度，该值越大，则显示的越细致
	plotter.setWindowName("Spin Image");
	plotter.plot();

	return 0;
}

关键代码解析：

    pcl::SpinImageEstimation > spin_image_descriptor(8, 0.05, 10);
	//三个数字分别表示：旋转图像分辨率；最小允许输入点与搜索曲面点之间的法线夹角的余弦值，以便在支撑中保留该点；
	//小支持点数，以正确估计自旋图像。如果在某个点支持包含的点较少，则抛出异常
	spin_image_descriptor.setInputCloud(cloud);
	spin_image_descriptor.setInputNormals(normals);
	// 使用法线计算的KD树
	spin_image_descriptor.setSearchMethod(tree);
	pcl::PointCloud >::Ptr spin_images(new pcl::PointCloud >);
	spin_image_descriptor.setRadiusSearch(40);
	// 计算spin image图像
	spin_image_descriptor.compute(*spin_images);

1. pcl::SpinImageEstimation > spin_image_descriptor(8, 0.05, 10);

   • 这行代码创建了一个 Spin Image 特征估计器对象。
   • 第一个参数 8 是旋转图像分辨率，指定了生成的 spin image 的分辨率。这个值越大，生成的 spin image 的分辨率就越高，但计算开销也会增加。
   • 第二个参数 0.05 是最小允许输入点与搜索曲面点之间的法线夹角的余弦值。这个值控制了在计算 spin image 时保留点的支撑度。值越大，要求点与法线的夹角越小，以保留更多的点。较小的值会过滤掉与法线夹角较大的点，从而减少噪声的影响。
   • 第三个参数 10 是小支持点数，指定了在估计 spin image 时，支持点所需的最小点数。如果某个点支持包含的点数少于这个值，将抛出异常。

2. spin_image_descriptor.setInputCloud(cloud);

   • 这行代码设置输入点云数据。

3. spin_image_descriptor.setInputNormals(normals);

   • 这行代码设置输入点云的法线数据。

4. spin_image_descriptor.setSearchMethod(tree);

   • 这行代码设置了法线计算的 KD 树搜索方法。KD 树是一种数据结构，用于快速查找最近邻点。

5. pcl::PointCloud >::Ptr spin_images(new pcl::PointCloud >);

   • 这行代码创建了一个指向 pcl::PointCloud > 类型对象的指针，用于存储计算得到的 spin image。

6. spin_image_descriptor.setRadiusSearch(40);

   • 这行代码设置了搜索半径，指定了在计算 spin image 时用于搜索最近邻点的半径范围。只有位于这个半径范围内的点才会被考虑在内。这个值越大，搜索的范围就越广，但也会增加计算开销。

7. spin_image_descriptor.compute(*spin_images);

   • 这行代码执行 spin image 的计算，并将结果存储在 spin_images 中。

参数的设置会直接影响到计算得到的 spin image 的质量和计算效率。调整这些参数可以根据具体的应用场景和需求进行优化，以获得更好的特征描述符。

	// 显示和检索第一点的自旋图像描述符向量。
	pcl::Histogram<153> first_descriptor = spin_images->points[0];
	cout << first_descriptor << endl;

	//========自旋图像描述符可视化=============================

	pcl::visualization::PCLPlotter plotter;
	plotter.addFeatureHistogram(*spin_images, 600); //设置的横坐标长度，该值越大，则显示的越细致
	plotter.setWindowName("Spin Image");
	plotter.plot();

1. pcl::Histogram<153> first_descriptor = spin_images->points[0];

   • 这行代码获取了计算得到的自旋图像描述符向量中的第一个描述符，并将其存储在 first_descriptor 变量中。
   • pcl::Histogram<153> 表示自旋图像描述符的数据类型，其中 153 是描述符的维度。

2. pcl::visualization::PCLPlotter plotter;

   • 这行代码创建了一个 PCLPlotter 对象，用于绘制可视化图形。

3. plotter.addFeatureHistogram(*spin_images, 600);

   • 这行代码将自旋图像描述符向量添加到绘图器中进行可视化。
   • *spin_images 是指向自旋图像描述符向量的指针。
   • 600 是设置的横坐标长度，该值越大，则显示的越细致。

4. plotter.setWindowName("Spin Image");

   • 这行代码设置绘图窗口的标题为 "Spin Image"。

5. plotter.plot();

   • 这行代码执行绘图操作，显示自旋图像描述符的可视化结果。

参数的设置会直接影响到可视化结果的显示效果。例如，通过调整横坐标长度可以改变柱状图的分辨率，使其更细致或更粗略。可根据需要调整这些参数以获得更好的可视化效果。

结果：

 

二、 Spin Image结合ICP配准

C++

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
typedef pcl::Histogram<153> HisT;
typedef pcl::PointCloud PointCloudHis;
typedef pcl::VFHSignature308 VFHT;
typedef pcl::PointCloud PointCloudVFH;
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 computeKeyPointsSpinimage(PointCloud::Ptr& cloud_in, PointCloud::Ptr& key_cloud, PointCloudHis::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.setSearchMethod(tree);
    //n.setKSearch(10);
    n.setRadiusSearch(0.3);
    n.compute(*normals);


    pcl::SpinImageEstimation spin_image_descriptor;
    spin_image_descriptor.setInputCloud(key_cloud);
    spin_image_descriptor.setInputNormals(normals);
    spin_image_descriptor.setSearchMethod(tree);
    spin_image_descriptor.setRadiusSearch(40); // 设置搜索半径
    spin_image_descriptor.compute(*dsc);
}

//Histogram<153>转换成VFHSignature308
void histogramVFHSignature(PointCloudHis::Ptr& dsc, PointCloudVFH::Ptr feature)
{
    pcl::VFHSignature308 midpoint;
    for (int i = 0; i < 308; i++) {
        midpoint.histogram[i] = 0;
    }
    for (int j = 0; j < dsc->size(); ++j)
    {
        for (int i = 0; i < 153; i++)
        {
            midpoint.histogram[i] = dsc->points[j].histogram[i];
        }
        feature->push_back(midpoint);
    }
}

// 点云可视化
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));
    }
}

void visualize_his(PointCloudHis::Ptr& dsc)
{
    pcl::visualization::PCLPlotter plotter;
    plotter.addFeatureHistogram(*dsc, 600); //设置的横坐标长度，该值越大，则显示的越细致
    plotter.setWindowName("Spin Image");
    plotter.plot();
}
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描述符计算特征
    PointCloudHis::Ptr source_features(new PointCloudHis);
    PointCloudHis::Ptr target_features(new PointCloudHis);
    computeKeyPointsSpinimage(cloud, keypoints1, source_features, tree);
    computeKeyPointsSpinimage(cloud_target, keypoints2, target_features, tree);
    cout << "SpinImage完成！" << endl;

    //========自旋图像描述符可视化=============================
    // 显示和检索第一点的自旋图像描述符向量。
    pcl::Histogram<153> first_descriptor_src = source_features->points[0];
    pcl::Histogram<153> first_descriptor_tgt = target_features->points[0];
    cout << "source_features第一点的自旋图像描述符向量：" << endl << first_descriptor_src << endl;
    cout << "target_features第一点的自旋图像描述符向量：" << endl << first_descriptor_tgt << endl;
    visualize_his(source_features);
    visualize_his(target_features);



    //Histogram<153>转换成VFHSignature308
    PointCloudVFH::Ptr source_feature(new PointCloudVFH);
    PointCloudVFH::Ptr target_feature(new PointCloudVFH);
    histogramVFHSignature(source_features, source_feature);
    histogramVFHSignature(target_features, target_feature);
    cout << "Histogram<153>转换VFHSignature308完成！" << endl;

    //SAC配准
    pcl::SampleConsensusInitialAlignment scia;
    scia.setInputSource(keypoints1);
    scia.setInputTarget(keypoints2);
    scia.setSourceFeatures(source_feature);
    scia.setTargetFeatures(target_feature);
    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;
}

关键代码解析：

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;
typedef pcl::Histogram<153> HisT;
typedef pcl::PointCloud PointCloudHis;
typedef pcl::VFHSignature308 VFHT;
typedef pcl::PointCloud PointCloudVFH;
typedef pcl::search::KdTree Tree;

 

1. typedef pcl::PointXYZ PointT;

   • 这行代码定义了一个名为 PointT 的新类型，它实际上是 pcl::PointXYZ 的别名。pcl::PointXYZ 是 PCL 中表示三维点的结构体，包含了 x、y 和 z 坐标。

2. typedef pcl::PointCloud PointCloud;

   • 这行代码定义了一个名为 PointCloud 的新类型，它是 pcl::PointCloud 的别名。pcl::PointCloud 是 PCL 中用于表示点云的类模板，而 PointT 则是点云中包含的点的类型。

3. typedef pcl::Histogram<153> HisT;

   • 这行代码定义了一个名为 HisT 的新类型，它是 pcl::Histogram<153> 的别名。pcl::Histogram 通常用于表示直方图，而这里的 <153> 表示直方图的大小为 153。

4. typedef pcl::PointCloud PointCloudHis;

   • 这行代码定义了一个名为 PointCloudHis 的新类型，它是 pcl::PointCloud 的别名。这表示一个点云，其中每个点都是类型为 HisT 的直方图。

5. typedef pcl::VFHSignature308 VFHT;

   • 这行代码定义了一个名为 VFHT 的新类型，它是 pcl::VFHSignature308 的别名。pcl::VFHSignature308 是 PCL 中用于表示视点特征直方图（VFH）的类型，这里的 308 表示 VFH 的长度为 308。

6. typedef pcl::PointCloud PointCloudVFH;

   • 这行代码定义了一个名为 PointCloudVFH 的新类型，它是 pcl::PointCloud 的别名。这表示一个点云，其中每个点都是类型为 VFHT 的 VFH 特征。

7. typedef pcl::search::KdTree Tree;

   • 这行代码定义了一个名为 Tree 的新类型，它是 pcl::search::KdTree 的别名。pcl::search::KdTree 是 PCL 中用于进行点云搜索的类，这里指定了 PointT 作为搜索的点类型。

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);
}

1. void extract_keypoint(PointCloud::Ptr& cloud, PointCloud::Ptr& keypoint, Tree::Ptr& tree)

   • 这是一个函数定义，用于从输入的点云 cloud 中提取关键点，并将结果存储在 keypoint 中。tree 是 KD 树的搜索方法。

2. pcl::ISSKeypoint3D iss;

   • 创建了一个 ISS 关键点检测器对象。PointT 应该是点云中点的类型。

3. iss.setInputCloud(cloud);

   • 设置输入点云数据。

4. iss.setSearchMethod(tree);

   • 设置搜索方法，这里使用了提供的 KD 树。

5. iss.setNumberOfThreads(8);

   • 设置初始化调度器并设置要使用的线程数。这个参数用于控制并行计算的线程数量。设置为 8 表示使用 8 个线程，从而提高计算效率。

6. iss.setSalientRadius(5);

   • 设置用于计算协方差矩阵的球邻域半径。这个值影响关键点的计算，较大的值可能导致检测到更大的关键点。

7. iss.setNonMaxRadius(5);

   • 设置非极大值抑制应用算法的半径。这个值影响检测到的关键点之间的最小距离，较大的值可能导致保留更少的关键点。

8. iss.setThreshold21(0.95);

   • 设置第二个和第一个特征值之比的上限。这个参数影响关键点的选择，控制特征值之间的比率。

9. iss.setThreshold32(0.95);

   • 设置第三个和第二个特征值之比的上限。同样，这个参数也影响关键点的选择，控制特征值之间的比率。

10. iss.setMinNeighbors(6);

    • 在应用非极大值抑制算法时，设置必须找到的最小邻居数。这个参数控制关键点周围邻居的数量，可能影响非极大值抑制的效果。

11. iss.compute(*keypoint);

    • 执行关键点的计算，并将结果存储在 keypoint 中。

void computeKeyPointsSpinimage(PointCloud::Ptr& cloud_in, PointCloud::Ptr& key_cloud, PointCloudHis::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.setSearchMethod(tree);
    //n.setKSearch(10);
    n.setRadiusSearch(0.3);
    n.compute(*normals);


    pcl::SpinImageEstimation spin_image_descriptor;
    spin_image_descriptor.setInputCloud(key_cloud);
    spin_image_descriptor.setInputNormals(normals);
    spin_image_descriptor.setSearchMethod(tree);
    spin_image_descriptor.setRadiusSearch(40); // 设置搜索半径
    spin_image_descriptor.compute(*dsc);
}

1. void computeKeyPointsSpinimage(PointCloud::Ptr& cloud_in, PointCloud::Ptr& key_cloud, PointCloudHis::Ptr& dsc, Tree::Ptr& tree)

   • 这是一个函数声明，它接受四个参数：
     • cloud_in：输入的点云，类型为 PointCloud::Ptr，表示指向点云的指针。
     • key_cloud：关键点的点云，类型为 PointCloud::Ptr，表示指向关键点点云的指针。
     • dsc：描述子的点云，类型为 PointCloudHis::Ptr，表示指向描述子点云的指针。
     • tree：搜索树，类型为 Tree::Ptr，表示指向搜索树对象的指针。

2. pcl::NormalEstimationOMP n;

   • 创建了一个使用 OpenMP 加速的法线估计对象 n，该对象用于计算点云中每个点的法线。

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

   • 创建了一个指向法线点云的指针 normals，该点云用于存储计算得到的法线信息。

4. n.setNumberOfThreads(6);

   • 设置 OpenMP 的线程数为 6，以便并行计算法线。

5. n.setInputCloud(key_cloud);

   • 将关键点的点云设置为法线估计的输入。

6. n.setSearchSurface(cloud_in);

   • 设置搜索表面为输入点云 cloud_in，这意味着法线估计将在整个输入点云上进行搜索。

7. n.setSearchMethod(tree);

   • 设置法线估计所用的搜索方法为 tree，即使用了之前定义的搜索树。

8. n.setRadiusSearch(0.3);

   • 设置法线估计的搜索半径为 0.3，这表示法线将在距离每个点不超过 0.3 的邻域内计算。

9. n.compute(*normals);

   • 执行法线估计，计算每个关键点的法线，并将结果存储在 normals 指向的点云中。

10. pcl::SpinImageEstimation spin_image_descriptor;

    • 创建了一个 Spin Image 描述子估计对象 spin_image_descriptor，用于计算关键点的 Spin Image 描述子。

11. spin_image_descriptor.setInputCloud(key_cloud);

    • 将关键点的点云设置为 Spin Image 描述子估计的输入。

12. spin_image_descriptor.setInputNormals(normals);

    • 将之前计算得到的法线设置为 Spin Image 描述子估计的输入法线。

13. spin_image_descriptor.setSearchMethod(tree);

    • 设置 Spin Image 描述子估计所用的搜索方法为 tree，即使用之前定义的搜索树。

14. spin_image_descriptor.setRadiusSearch(40);

    • 设置 Spin Image 描述子估计的搜索半径为 40，这表示描述子将在距离每个关键点不超过 40 的邻域内计算。

15. spin_image_descriptor.compute(*dsc);

    • 执行 Spin Image 描述子的计算，计算每个关键点的描述子，并将结果存储在 dsc 指向的点云中。

这些参数的设置会影响计算结果的精度、计算速度以及描述子的数量和质量。例如，搜索半径的选择将影响到所考虑的点的数量，从而影响描述子的丰富程度和计算的时间。增加线程数可能会加快法线估计的速度，但也可能导致系统资源的过度使用。

void histogramVFHSignature(PointCloudHis::Ptr& dsc, PointCloudVFH::Ptr feature)
{
    pcl::VFHSignature308 midpoint;
    for (int i = 0; i < 308; i++) {
        midpoint.histogram[i] = 0;
    }
    for (int j = 0; j < dsc->size(); ++j)
    {
        for (int i = 0; i < 153; i++)
        {
            midpoint.histogram[i] = dsc->points[j].histogram[i];
        }
        feature->push_back(midpoint);
    }
}

1. 由于SpinImage不能直接应用于配准，需要转换成VFH

2. void histogramVFHSignature(PointCloudHis::Ptr& dsc, PointCloudVFH::Ptr feature)

   • 这是一个函数声明，它接受两个参数：
     • dsc：描述子的点云，类型为 PointCloudHis::Ptr，表示指向描述子点云的指针。
     • feature：VFH 特征的点云，类型为 PointCloudVFH::Ptr，表示指向 VFH 特征点云的指针。

3. pcl::VFHSignature308 midpoint;

   • 创建了一个长度为 308 的 VFH 签名对象 midpoint，用于存储计算得到的 VFH 特征。

4. for (int i = 0; i < 308; i++) { midpoint.histogram[i] = 0; }

   • 将 midpoint 对象中的直方图初始化为零，确保所有的直方图元素都被清零。

5. for (int j = 0; j < dsc->size(); ++j)

   • 循环遍历描述子点云中的每个点。

6. for (int i = 0; i < 153; i++) { midpoint.histogram[i] = dsc->points[j].histogram[i]; }

   • 将描述子点云中第 j 个点的直方图值复制到 midpoint 对象的直方图中。这个循环的长度是 153，因为 VFH 特征的长度为 308，而描述子长度为 153。

7. feature->push_back(midpoint);

   • 将存储在 midpoint 中的 VFH 特征添加到 feature 点云中。

参数设置和影响：

• 代码中的参数主要是描述子的长度和 VFH 特征的长度。在这里，描述子长度为 153，而 VFH 特征的长度为 308。这些长度是基于特定的问题和数据集选择的，不同的问题和数据集可能需要不同的长度。
• 对于直方图中的值，这段代码假设描述子的长度是 153，因此循环将从 0 到 152 进行迭代。如果描述子的长度不是 153，那么需要相应地调整循环的长度。
• 由于 VFH 特征是基于描述子计算的，因此描述子的质量和准确性会直接影响到最终的 VFH 特征的质量和准确性。
• 在实际应用中，还需要考虑到 VFH 特征的归一化、特征选择等步骤，以进一步提高特征的性能和鲁棒性。

    //sac配准
    pcl::SampleConsensusInitialAlignment scia;
    scia.setInputSource(keypoints1);
    scia.setInputTarget(keypoints2);
    scia.setSourceFeatures(source_feature);
    scia.setTargetFeatures(target_feature);
    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);

1. pcl::SampleConsensusInitialAlignment scia;: 这里创建了一个用于初始对齐的对象 scia，其中 PointT 是点云中点的类型，VFHT 是用于识别特征的估计方法（例如，VFH 特征）。

2. scia.setInputSource(keypoints1); 和 scia.setInputTarget(keypoints2);: 将待配准的源点云 keypoints1 和目标点云 keypoints2 设置为输入。

3. scia.setSourceFeatures(source_feature); 和 scia.setTargetFeatures(target_feature);: 设置源点云和目标点云的特征。

4. scia.setMinSampleDistance(7);, scia.setNumberOfSamples(100);, scia.setCorrespondenceRandomness(6);: 这些函数设置了 Sample Consensus Initial Alignment（SAC-IA）算法的参数，例如最小样本距离、每次迭代使用的样本数量和对应关系随机性等。

5. PointCloud::Ptr sac_result(new PointCloud);: 创建一个指向点云的指针，用于存储 SAC-IA 算法的结果。

6. scia.align(*sac_result);: 执行 SAC-IA 配准。

7. Eigen::Matrix4f sac_trans; sac_trans = scia.getFinalTransformation();: 获取 SAC-IA 配准的最终变换矩阵。

8. cout << "SAC配准完成！" << endl;: 输出 SAC-IA 配准完成的消息。

9. PointCloud::Ptr icp_result(new PointCloud);: 创建一个指向点云的指针，用于存储 ICP 算法的结果。

10. pcl::IterativeClosestPoint icp;: 创建一个 Iterative Closest Point（ICP）对象 icp。

11. icp.setInputSource(keypoints1); 和 icp.setInputTarget(keypoints2);: 设置 ICP 算法的输入。

12. icp.setMaxCorrespondenceDistance(20);, icp.setMaximumIterations(35);, icp.setTransformationEpsilon(1e-10);, icp.setEuclideanFitnessEpsilon(0.01);: 这些函数设置了 ICP 算法的参数，如最大对应距离、最大迭代次数、变换阈值等。

13. icp.align(*icp_result, sac_trans);: 执行 ICP 配准，其中 sac_trans 是之前执行 SAC-IA 得到的变换矩阵，作为初始猜测。

14. cout << "ICP配准完成！" << endl;: 输出 ICP 配准完成的消息。

15. std::cout << "ICP converged: " << icp.hasConverged() << std::endl;: 输出 ICP 是否收敛的信息。

16. std::cout << "Transformation matrix:\n" << icp.getFinalTransformation() << std::endl;: 输出最终的变换矩阵。

17. Eigen::Matrix4f icp_trans; icp_trans = icp.getFinalTransformation();: 获取 ICP 配准的最终变换矩阵。

18. cout << icp_trans << endl;: 输出变换矩阵。

19. pcl::transformPointCloud(*cloud, *icp_result, icp_trans);: 使用获取的变换矩阵对原始点云进行变换，将变换后的结果存储在 icp_result 中。

结果：

输入点云与输出点云

对输入点云进行Spin Image自旋图像描述符可视化 

 

对输出点云进行Spin Image自旋图像描述符可视化  

配准结果 

 

输出数据