CRH直方图和姿态识别代码

来源：互联网发布：淘宝优惠券返利网编辑：程序博客网时间：2024/05/29 15:07
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();    }
阅读全文
0 0