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
|
# -*- coding: utf-8 -*-
from __future__ import print_function
# This Code Base
# http://ros-robot.blogspot.jp/2011/08/pclapi-point-cloud-library-pcl-pcl-api.html
import numpy as np
import pcl
import random
import pcl.pcl_visualization
# pcl::PointCloud<pcl::PointXYZRGB> cloud;
cloud = pcl.PointCloud_PointXYZRGB()
# Fill in the cloud data
# cloud.width = 15;
# cloud.height = 10;
# cloud.points.resize (cloud.width * cloud.height)
# cloud.resize (np.array([15, 10], dtype=np.float))
# points = np.zeros((10, 15, 4), dtype=np.float32)
points = np.zeros((150, 4), dtype=np.float32)
RAND_MAX = 1.0
# Generate the data
for i in range(0, 75):
# set Point Plane
points[i][0] = 1024 * random.random () / (RAND_MAX + 1.0)
points[i][1] = 1024 * random.random () / (RAND_MAX + 1.0)
points[i][2] = 0.1 * random.random () / (RAND_MAX + 1.0)
points[i][3] = 255 << 16 | 255 << 8 | 255
for i in range(75, 150):
# set Point Randomize
points[i][0] = 1024 * random.random () / (RAND_MAX + 1.0)
points[i][1] = 1024 * random.random () / (RAND_MAX + 1.0)
points[i][2] = 1024 * random.random () / (RAND_MAX + 1.0)
points[i][3] = 255 << 16 | 255 << 8 | 255
# Set a few outliers
points[0][2] = 2.0;
points[3][2] = -2.0;
points[6][2] = 4.0;
print (cloud)
for i in range(0, 150):
print (points[i][0], points[i][1], points[i][2], points[i][3])
cloud.from_array(points)
# Create the segmentation object
# pcl::SACSegmentation<pcl::PointXYZRGB> seg
seg = cloud.make_segmenter()
# Optional
seg.set_optimize_coefficients (True)
# Mandatory
seg.set_model_type (pcl.SACMODEL_PLANE)
seg.set_method_type (pcl.SAC_RANSAC)
seg.set_distance_threshold (0.1)
# pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients)
# pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
inliers, model = seg.segment()
# if inliers.size
# return
# end
print (model)
# std::cerr << "Model coefficients: " << coefficients->values[0] << " "
# << coefficients->values[1] << " "
# << coefficients->values[2] << " "
# << coefficients->values[3] << std::endl;
#
# std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
# for (size_t i = 0; i < inliers->indices.size (); ++i)
# {
# std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
# << cloud.points[inliers->indices[i]].y << " "
# << cloud.points[inliers->indices[i]].z << std::endl;
# cloud.points[inliers->indices[i]].r = 255;
# cloud.points[inliers->indices[i]].g = 0;
# cloud.points[inliers->indices[i]].b = 0;
# }
for i in inliers:
points[i][3] = 255 << 16 | 255 << 8 | 0
cloud.from_array(points)
#
# pcl::visualization::CloudViewer viewer("Cloud Viewer");
# viewer.showCloud(cloud.makeShared());
# while (!viewer.wasStopped ())
visual = pcl.pcl_visualization.CloudViewer()
visual.ShowColorCloud(cloud)
while True:
visual.WasStopped()
end
|
