PointCloud-segmentation/main.cpp at master · luoru/PointCloud-segmentation · GitHub

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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
/*
*分割拟合算法测试主程序
*罗中飞，2015.12
*/

#include <iostream>
#include <string>
#include <ctime>

#include "PointIO.h"
#include "Segmentation.h"

#include <pcl/filters/voxel_grid.h>

void pointCloud2pclPointCloud(const td::PointCloud& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& pclCloud)
{
    if(!pclCloud->empty())
    {
        pclCloud.reset();
    }
    for (int i = 0; i < cloud.size(); i++)
    {
        pcl::PointXYZ pt;
        pt.x = cloud[i].x;
        pt.y = cloud[i].y;
        pt.z = cloud[i].z;
        pclCloud->push_back(pt);
    }
}

void pclPointCloud2PointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pclCloud, td::PointCloud& cloud)
{
    if(!cloud.empty())
        cloud.clear();
    for (int i = 0; i < pclCloud->size(); i++)
    {
        td::Point pt;
        pt.x = pclCloud->at(i).x;
        pt.y = pclCloud->at(i).y;
        pt.z = pclCloud->at(i).z;
        cloud.push_back(pt);
    }
}

int main()
{
	std::cout << "------------------------分割拟合程序开始运行---------------------"<< std::endl;
	std::cout << "输入点云文件路径（不能带有空格）"<<std::endl;

	std::string path;
	std::cin >> path;

	td::PointIO ptIO;
	ptIO.open(path);

    td::PointCloud cloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pointCloud2pclPointCloud(ptIO.getPointCloud(), pclCloud);
    std::cout << "输入抽稀网格大小:" << std::endl;
    double leafSize(0);
    std::cin >> leafSize;
    pcl::VoxelGrid<pcl::PointXYZ> filter;
    filter.setInputCloud(pclCloud);
    filter.setLeafSize(leafSize, leafSize, leafSize);
    filter.filter(*filteredCloud);
    pclPointCloud2PointCloud(filteredCloud, cloud);


    td::Segmentation seg;
    seg.setInputCloud(cloud);

    std::cout << "k邻域值(根据实际点云密度进行选择)" << std::endl;
	int k;
	std::cin >> k;
	seg.setKNearest(k);

	/*std::cout << "邻域半径" << std::endl;
	double radius;
	std::cin >> radius;
	seg.setRadius(radius);*/

	std::cout << "平面拟合阈值" << std::endl;
	double threshold;
	std::cin >> threshold;
	seg.setThreshold(threshold);

//	std::cout << "------------------------输入拟合方法---------------------" << std::endl;
//	std::cout << "------------------------0：RANSAC---------------------" << std::endl;
//	std::cout << "------------------------1：BAYSAC---------------------" << std::endl;
//	std::cout << "------------------------2：LMEDS---------------------" << std::endl;
//	std::cout << "------------------------3：BAYLMEDS---------------------" << std::endl;
//	std::cout << "------------------------开始生长拟合---------------------" << std::endl;
//	int me;
//	std::cin >> me;

	clock_t start, end;
	start = clock();
    std::cout << "拟合中..." <<std::endl;
    seg.regionGrow(td::Segmentation::RANSAC, k);
//    seg.multiRansac();
//    seg.singleFitting(td::Segmentation::RANSAC);
	end = clock();
//    std::cout << "拟合时间：" << (double)(end - start) / CLOCKS_PER_SEC *1000.0 << "ms" << std::endl;
//    std::cout << "共拟合" << seg.getModelNum() << "个平面" << std::endl;
//    std::cout << "平均拟合误差为" << seg.getAverageError() << std::endl;


    std::cout << "拟合结束!" << std::endl;

//    std::cout << "平面最少点数" << std::endl;
//    int minN;
//    std::cin >> minN;
//    std::cout << "计算挡板中..." << std::endl;

//    td::Plane normalV;
//    seg.singleFitting(td::Segmentation::RANSAC, normalV);

//    double n1[3];
//    n1[0] = normalV.getA();
//    n1[1] = normalV.getB();
//    n1[2] = normalV.getC();
//    std::vector<td::Plane> pls;
//    pls = seg.getPlaneModels();
//    std::vector<td::PointCloud> indice = seg.getIndices();

//    int tagetNum = 0;
//    for(int i = 0; i < pls.size(); i++)
//    {

//        if(indice[i].size()< minN)
//            continue;
//        double n2[3];
//        n2[0] = pls[i].getA();
//        n2[1] = pls[i].getB();
//        n2[3] = pls[i].getC();
//        double theta(0);
//        theta = n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2];
//        if(theta < 1.0)
//            tagetNum++;
//    }
//    std::cout << "共有 " << tagetNum << "块挡板" << std::endl;

	std::cout << "输入存储点云文件路径（不能带有空格）" << std::endl;
	std::cin >> path;
	seg.outPut(path);
	return 0;
}