SACSegmentation点云分割

 1#include <iostream>
 2#include <pcl/io/pcd_io.h>
 3#include <pcl/point_types.h>
 4#include <pcl/point_cloud.h>
 5#include <pcl/visualization/pcl_visualizer.h>
 6#include <pcl/filters/extract_indices.h>
 7#include <pcl/segmentation/sac_segmentation.h>
 8#include <vtkPoints.h>
 9#include <vtkPolyData.h>
10#include <vtkActor.h>
11#include <vtkRenderer.h>
12#include <vtkRenderWindow.h>
13#include <vtkPolyDataMapper.h>
14#include <vtkInteractorStyle.h>
15#include <vtkRenderWindowInteractor.h>
16#include <vtkProperty.h>
17#include <vtkCellArray.h>
18#include <vtkInteractorStyleTrackballCamera.h>
19
20VTK_MODULE_INIT(vtkRenderingOpenGL);
21VTK_MODULE_INIT(vtkInteractionStyle);
22
23// VTK绘制剩下的点云
24void drawSegment(pcl::PointIndices &indices, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
25{
26    vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
27    vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
28    for (int i: (indices).indices) {
29        vtkIdType pid[1];
30        pid[0] = points->InsertNextPoint(cloud->at(i).x, cloud->at(i).y,
31                                         cloud->at(i).z);
32        vertices->InsertNextCell(1, pid);
33    }
34    vtkPolyData *polyData = vtkPolyData::New();
35    polyData->SetPoints(points);
36    polyData->SetVerts(vertices);
37
38    vtkPolyDataMapper *mapper = vtkPolyDataMapper::New();
39    mapper->SetInputData(polyData);
40
41    vtkActor *actor = vtkActor::New();
42    actor->SetMapper(mapper);
43    actor->GetProperty()->SetColor(1.0, 0.0, 0.0);
44    actor->GetProperty()->SetPointSize(3);
45
46    vtkRenderer *renderer = vtkRenderer::New();
47    renderer->AddActor(actor);
48    renderer->SetBackground(.0, .0, .0);//set background
49
50    vtkRenderWindow *renderwind = vtkRenderWindow::New();
51    renderwind->AddRenderer(renderer);
52
53    vtkInteractorStyleTrackballCamera *style = vtkInteractorStyleTrackballCamera::New();
54
55    vtkRenderWindowInteractor *renderwindIt = vtkRenderWindowInteractor::New();
56    renderwindIt->SetRenderWindow(renderwind);
57    renderwindIt->SetInteractorStyle(style);
58
59    renderwind->Render();
60    renderwindIt->Start();
61}
62
63int main()
64{
65    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>);
66    std::string fileName = getenv("HOME") + std::string("/data_set/table_scene_lms400.pcd");
67    std::cout << fileName << std::endl;
68    pcl::io::loadPCDFile(fileName, *cloud_source);
69    // 模型系数
70    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
71    // 点索引数组
72    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
73    pcl::SACSegmentation<pcl::PointXYZ> sac;
74    sac.setInputCloud(cloud_source);
75    // 提取方法
76    sac.setMethodType(pcl::SAC_RANSAC);
77    // 提取的物体模型
78    sac.setModelType(pcl::SACMODEL_PLANE);
79    sac.setDistanceThreshold(0.01);
80    sac.setMaxIterations(100);
81    sac.setProbability(0.95);
82    sac.segment(*inliers, *coefficients);
83    // 拟合系数
84    std::cout << *coefficients << std::endl;
85
86    // 绘制分割后的点云
87    drawSegment(*inliers, cloud_source);
88}

header:
seq: 0 stamp: 0 frame_id:
values[]
  values[0]:   -0.00691321
  values[1]:   -0.875345
  values[2]:   -0.48345
  values[3]:   -1.17686