pcl-3 pcl结合opencv做svm分类（法向量特征数据）

后续使用了fpfh特征作为训练数据，遇到了一些困难
首先是flann冲突，这个将opcv中的flann都改成了flann2就可以运行
后面在将得到的33特征值进行训练的时候一直内存超限，传输的不太好，到现在还是不行，改了三天还是没有改好，先放这里吧，等后续有时间进行修改，我感觉是传输的问题。

#pragma warning(disable:4996)
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
//点云显示
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>
//数据组织
#include <boost/thread/thread.hpp>
#include <boost/thread/thread_time.hpp>
#include<flann/flann.h>
#include <thread>
#include <pcl/search/kdtree.h>
//
#include <omp.h>
//补充点云特征
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/shot.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/pfh.h>
#include <pcl/features/normal_3d.h>

#include <opencv2/opencv.hpp>



int main() {
    // 读取初始点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDReader reader;
    reader.read("svmtest.pcd", *cloud);

    cout << "初始点云读取完成" << endl;

    // 读取带标签的点云
    pcl::PointCloud<pcl::PointXYZL>::Ptr labeledCloud(new pcl::PointCloud<pcl::PointXYZL>);
    reader.read("svmlearn_xyzl.pcd", *labeledCloud);

    cout << "标签点云读取完成" << endl;

    // 计算法线
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    pcl::NormalEstimation<pcl::PointXYZL, pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud(labeledCloud);
    pcl::search::KdTree<pcl::PointXYZL>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZL>);
    normalEstimation.setSearchMethod(kdtree);
    normalEstimation.setKSearch(20);  // 设置法线估计时近邻点的数量
    normalEstimation.compute(*normals); 
    cout << "发线计算完成" << endl;

     将法线和原始点云拼接起来
    //pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);
    //pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);

     // 将法线和原始点云拼接起来
    //pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointNormal>);

    pcl::PointCloud<pcl::PointXYZLNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZLNormal>);

    cloudWithNormals->resize(cloud->size());
    for (size_t i = 0; i < cloud->size(); ++i) {
        cloudWithNormals->points[i].x = cloud->points[i].x;
        cloudWithNormals->points[i].y = cloud->points[i].y;
        cloudWithNormals->points[i].z = cloud->points[i].z;
        cloudWithNormals->points[i].normal_x = normals->points[i].normal_x;
        cloudWithNormals->points[i].normal_y = normals->points[i].normal_y;
        cloudWithNormals->points[i].normal_z = normals->points[i].normal_z;
    }
    cout << "cloudWithNormals的点云数量为" << cloudWithNormals->size() << endl;
    cout << "法线和原始点云合并完成" << endl;


    // 读取法线和曲率特征

   // pcl::PointCloud<pcl::FPFHSignature33>::Ptr features(new pcl::PointCloud<pcl::FPFHSignature33>);


  // 计算带标签点云的FPFH特征
    pcl::FPFHEstimationOMP<pcl::PointXYZL, pcl::Normal, pcl::FPFHSignature33> fpfh_src;
    fpfh_src.setInputCloud(labeledCloud);
    fpfh_src.setInputNormals(normals);
    fpfh_src.setNumberOfThreads(10);
    pcl::search::KdTree<pcl::PointXYZL>::Ptr kdtree2(new pcl::search::KdTree<pcl::PointXYZL>);
    fpfh_src.setSearchMethod(kdtree2);

    cout << "开始计算点云特征" << endl;

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr features(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_src.setKSearch(20);
    fpfh_src.compute(*features);


    // 开始计算前上锁
    omp_lock_t lock;
    omp_init_lock(&lock);

    // 使用 OpenMP 设置锁
#pragma omp parallel
    {
#pragma omp single
        {
#pragma omp task
            {
                fpfh_src.compute(*features);
            }
        }
    }

    // 计算完成后解锁
    omp_destroy_lock(&lock);

    cout << "读取法线和曲率特征完成" << endl;


   // 准备训练数据和标签
    cv::Mat trainingData(labeledCloud->size(), 33, CV_32FC1);  // 注意特征的维度
    cv::Mat labels(labeledCloud->size(), 1, CV_32SC1);

    std::cout << "labeledCloud size: " << labeledCloud->size() << std::endl;
    std::cout << "features size: " << features->size() << std::endl;

    for (size_t i = 0; i < labeledCloud->size(); ++i)
    {
        // 使用法线和曲率特征
        for (int j = 0; j < 33; ++j)
        {
            if (i < features->size())
            {  // 添加索引范围检查
                trainingData.at<float>(i, j) = features->points[i].histogram[j];
            }
            else 
            {
                std::cerr << "Index out of range for features at i=" << i << " and j=" << j << std::endl;
            }
        }


        // 根据点的标签设置标签数据
        if (i < labeledCloud->size()) {  // 添加索引范围检查
            labels.at<int>(i, 0) = labeledCloud->points[i].label;
        }
        else {
            std::cerr << "Index out of range for labeledCloud at i=" << i << std::endl;
        }
    }

    cout << "根据点的标签设置标签数据完成" << endl;

    // 创建并训练SVM分类器
    cv::Ptr<cv::ml::SVM> svm = cv::ml::SVM::create();
    svm->setType(cv::ml::SVM::C_SVC);
    svm->setKernel(cv::ml::SVM::RBF);
    svm->setC(10);
    svm->setGamma(0.001);
    svm->train(trainingData, cv::ml::ROW_SAMPLE, labels);


    cout << "创建并训练SVM分类器完成，正在开始对点云进行分类" << endl;

    // 对初始点云进行分类
    //cv::Mat testData(cloud->size(), 3, CV_32FC1);

    //for (size_t i = 0; i < cloud->size(); ++i) 
    //{
    //    testData.at<float>(i, 0) = cloud->points[i].x;
    //    testData.at<float>(i, 1) = cloud->points[i].y;
    //    testData.at<float>(i, 2) = cloud->points[i].z;
    //}

    cv::Mat testData(cloud->size(), 33, CV_32FC1);
    for (size_t i = 0; i < cloud->size(); ++i)
    {
        for (int j = 0; j < 33; ++j) {
            testData.at<float>(i, j) = features->points[i].histogram[j];
        }
    }



   cv::Mat predictedLabels;
    /* svm->predict(testData, predictedLabels);*/

    try {
        svm->predict(testData, predictedLabels);
    }
    catch (cv::Exception& e) {
        std::cerr << "OpenCV Exception: " << e.what() << std::endl;
    }

    cout << "正在将分类结果添加到点云中" << endl;

    // 将分类结果添加到点云中
    pcl::PointCloud<pcl::PointXYZL>::Ptr classifiedCloud(new pcl::PointCloud<pcl::PointXYZL>);
    classifiedCloud->resize(cloud->size());

     

    for (size_t i = 0; i < cloud->size(); ++i) {
        classifiedCloud->points[i].x = cloud->points[i].x;
        classifiedCloud->points[i].y = cloud->points[i].y;
        classifiedCloud->points[i].z = cloud->points[i].z;

        // 修正标签值（假设标签是 0 或 1）
        classifiedCloud->points[i].label = static_cast<int>(predictedLabels.at<float>(i, 0)) + 1;
    }

    pcl::PCDWriter writer;

    writer.write("lable.pcd", *classifiedCloud);

    cout << "lable.pcd已完成储存，请查看" << endl;

   
        //----------------------------根据分类标签可视化-----------------------------
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
        pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZL>fildColor(classifiedCloud, "label");
        viewer->setBackgroundColor(0, 0, 0);
        viewer->setWindowName("点云按分类标签显示");
        viewer->addText("Point clouds are shown by label", 50, 50, 0, 1, 0, "v1_text");
        viewer->addPointCloud<pcl::PointXYZL>(classifiedCloud, fildColor, "sample cloud");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");

        while (!viewer->wasStopped())
        {
            viewer->spinOnce(100);
            boost::this_thread::sleep(boost::posix_time::microseconds(100000));
        }


    return 0;
}