9-1_采樣一致性過濾點雲

9-1_采樣一致性過濾點雲

程式說明:

範例程式:

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
    // --------------------------------------------
    // 回傳viewer的指標，表示內部顯示的結果更新
    // --------------------------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->initCameraParameters();
    return (viewer);
}

int main(int argc, char** argv)
{
    // 點雲的初始化
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

    // 先產生3000的點為範例
    cloud->width = 3000;
    cloud->height = 1;
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
       if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
       {
          cloud->points[i].x = 1 * rand() / (RAND_MAX + 1.0); //(隨機數)
          cloud->points[i].y = 1 * rand() / (RAND_MAX + 1.0); //(隨機數)

          if (i % 5 == 0)
             cloud->points[i].z = 1 * rand() / (RAND_MAX + 1.0);
          else if (i % 2 == 0)
             cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x) //x^2 + y^2 + z^2 = 1 (上圓)
             - (cloud->points[i].y * cloud->points[i].y));
          else
             cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x) //x^2 + y^2 + z^2 = 1 (下圓)
             - (cloud->points[i].y * cloud->points[i].y));
       }
       else
       {
          cloud->points[i].x = 1 * rand() / (RAND_MAX + 1.0); //(隨機數)
          cloud->points[i].y = 1 * rand() / (RAND_MAX + 1.0); //(隨機數)

          if (i % 2 == 0)
             cloud->points[i].z = 1 * rand() / (RAND_MAX + 1.0); // 隨機點
          else
             cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y); // x+y+z = 0;
       }
    }

    std::vector inliers;

    // 此部分有平面的model 和球形的model
    pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
       model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
       model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
    if (pcl::console::find_argument(argc, argv, "-f") >= 0) //平面
    {
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
       ransac.setDistanceThreshold(0.01);
       ransac.computeModel();
       ransac.getInliers(inliers);
    }
    else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) //球形
    {
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
       ransac.setDistanceThreshold(0.01);
       ransac.computeModel();
       ransac.getInliers(inliers);
    }

    // 將在inlinear的點雲複製出來
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

    // 建立視覺化的物件
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

    if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
       viewer = simpleVis(final);
    else
       viewer = simpleVis(cloud);


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

    return 0;
}