1. 随机采样一致性算法

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.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>


// 打开3D可视化窗口
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud){
    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, 1, "sample cloud");
    viewer->addCoordinateSystem (1.0);
    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>); // 存储最终点云

    // 填充点云
    cloud->width = 500;
    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 = 1024 * rand() / (RAND_MAX + 1.0f);
            cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
            if (i % 5 == 0)
                cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
            else if(i % 2 == 0)
                cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y));
            else
                cloud->points[i].z = -1 * sqrt(1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y));
        }
        else{
            cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
            cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
            if (i % 5 == 0)
                cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
            else
                cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
        }
    }

    // ---------------------------------------------------------------------
    std::vector<int> inliers; // 存储内点索引

    //创建随机采样一致性对象
    // 针对球模型
    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));

    // 根据命令行参数选择模型
    // 1. 选择平面模型
    if (pcl::console::find_argument(argc, argv, "-s") >= 0){
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
        ransac.setDistanceThreshold (0.01);
        ransac.computeModel();
        ransac.getInliers(inliers);
    }
    // 2. 选择球模型
    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);
    }

    // -----------------------------------------------------------------------------
    // 可视化
    // 复制估算模型的所有局部点到findal中
    pcl::copyPointCloud(*cloud, inliers, *final);

    // 创建可视化对象，并加入原始点云或者所有的局部点云
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
        viewer = simpleVis(final);
    else
        viewer = simpleVis(cloud);

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

./RandomSampleConsensus
20240726153047

./RandomSampleConsensus -s

20240726153358

./RandomSampleConsensus -sf
20240726153322