CRH直方图和姿态识别代码

TCHAR szFilter[] = _T("所有文件(*.*)|*.*|pcd文件(*.pcd)|*.pcd|ply文件(*.ply)|*.ply|xyz文件(*.xyz)|*.xyz|");
    // 构造打开文件对话框   
    CFileDialog fileDlg(TRUE, _T("pcd"), NULL, 0, szFilter, this);
    CString scenefilepath;
    CString modelfilepath;
    std::string scenefile;
    std::string modelfile;
    // Cloud for storing the object.
    pcl::PointCloud<pcl::PointXYZ>::Ptr scenecloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr modelcloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 显示打开文件对话框   
    if (IDOK == fileDlg.DoModal())
    {
        scenefilepath = fileDlg.GetPathName();
        // Load the first file
        //pcl::PCLPointCloud2Ptr cloud;
        // Command line parsing
        bool format = true;
        bool use_camera = true;

        scenefile = WChar2Ansi(scenefilepath.GetBuffer(scenefilepath.GetLength()));
        int pos = scenefile.find_last_of('.');
        std::string extensionName(scenefile.substr(pos + 1));
        if (extensionName == "ply")
        {
            if (pcl::io::loadPLYFile<pcl::PointXYZ>(scenefile.c_str(), *scenecloud) != 0)
            {
                AfxMessageBox(_T("file is error"));
            }
        }
        if (extensionName == "pcd")
        {
            if (pcl::io::loadPCDFile<pcl::PointXYZ>(scenefile.c_str(), *scenecloud) != 0)
            {
                AfxMessageBox(_T("file is error"));
            }
        }
    }
    // 显示打开文件对话框   
    if (IDOK == fileDlg.DoModal())
    {
        modelfilepath = fileDlg.GetPathName();
        // Load the first file
        //pcl::PCLPointCloud2Ptr cloud;
        // Command line parsing
        bool format = true;
        bool use_camera = true;

        modelfile = WChar2Ansi(modelfilepath.GetBuffer(modelfilepath.GetLength()));
        int pos = modelfile.find_last_of('.');
        std::string extensionName(modelfile.substr(pos + 1));
        if (extensionName == "ply")
        {
            if (pcl::io::loadPLYFile<pcl::PointXYZ>(modelfile.c_str(), *modelcloud) != 0)
            {
                AfxMessageBox(_T("file is error"));
            }
        }
        if (extensionName == "pcd")
        {
            if (pcl::io::loadPCDFile<pcl::PointXYZ>(modelfile.c_str(), *modelcloud) != 0)
            {
                AfxMessageBox(_T("file is error"));
            }
        }
    }

    // Object for storing the normals.
    pcl::PointCloud<pcl::Normal>::Ptr scene_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::PointCloud<pcl::Normal>::Ptr model_normals(new pcl::PointCloud<pcl::Normal>);
    // Estimate the normals.
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud(scenecloud);
    normalEstimation.setRadiusSearch(0.03);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    normalEstimation.setSearchMethod(kdtree);
    normalEstimation.compute(*scene_normals);

    normalEstimation.setInputCloud(modelcloud);
    normalEstimation.compute(*model_normals);

    //estimate centroid 
    Eigen::Vector4f scene_centroid;
    Eigen::Vector4f model_centroid;
    pcl::compute3DCentroid<pcl::PointXYZ>(*scenecloud, scene_centroid);
    pcl::compute3DCentroid<pcl::PointXYZ>(*modelcloud, model_centroid);

    //estimate camera roll histogram 
    pcl::CRHEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<90> > crh_;
    pcl::PointCloud<pcl::Histogram<90> >::Ptr scene_crh(new pcl::PointCloud<pcl::Histogram<90> >);
    pcl::PointCloud<pcl::Histogram<90> >::Ptr model_crh(new pcl::PointCloud<pcl::Histogram<90> >);
    crh_.setInputCloud(scenecloud);
    crh_.setInputNormals(scene_normals);
    crh_.setCentroid(scene_centroid);
    crh_.compute(*scene_crh);

    crh_.setInputCloud(modelcloud);
    crh_.setInputNormals(model_normals);
    crh_.setCentroid(model_centroid);
    crh_.compute(*model_crh);

    pcl::visualization::PCLPlotter crh_visualizer;
    crh_visualizer.addFeatureHistogram<pcl::Histogram<90>>(*scene_crh, 90, "scenefeature");
    crh_visualizer.addFeatureHistogram<pcl::Histogram<90>>(*model_crh, 90, "modelfeature");
    crh_visualizer.spin();

    // CRH alignment object.
    pcl::CRHAlignment<pcl::PointXYZ, 90> alignment;
    alignment.setInputAndTargetView(scenecloud, modelcloud);
    // CRHAlignment works with Vector3f, not Vector4f.
    Eigen::Vector3f sceneCentroid3f(scene_centroid[0], scene_centroid[1], scene_centroid[2]);
    Eigen::Vector3f modelCentroid3f(model_centroid[0], model_centroid[1], model_centroid[2]);
    alignment.setInputAndTargetCentroids(sceneCentroid3f, modelCentroid3f);

    // Compute the roll angle(s).
    std::vector<float> angles;
    alignment.computeRollAngle(*scene_crh, *model_crh, angles);

    if (angles.size() > 0)
    {
        int pos = scenefile.find_last_of('.');
        //std::string extensionName(scenefile.substr(pos + 1));
        scenefile = scenefile.substr(0, pos);
        scenefile += ".txt";



        std::ofstream fs;
        fs.open(scenefile.c_str());
        for (int i = 0; i < angles.size(); i++)
        {
            //std::cout << "\t" << angles.at(i) << " degrees." << std::endl
            fs << angles.at(i) << "\n";
        }
        fs.close();
    }