测试环境:
pcl==1.12.1
python-pcl==0.3.1
python==3.7
代码:
# -*- coding: utf-8 -*-
# http://pointclouds.org/documentation/tutorials/planar_segmentation.php#planar-segmentation
import pcl
import numpy as np
import random
def main():
# pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
#
# // Fill in the cloud data
# cloud->width = 15;
# cloud->height = 1;
# cloud->points.resize (cloud->width * cloud->height);
#
# // Generate the data
# for (size_t i = 0; i < cloud->points.size (); ++i)
# {
# cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
# cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
# cloud->points[i].z = 1.0;
# }
#
# // Set a few outliers
# cloud->points[0].z = 2.0;
# cloud->points[3].z = -2.0;
# cloud->points[6].z = 4.0;
###
cloud = pcl.PointCloud()
points = np.zeros((15, 3), dtype=np.float32)
RAND_MAX = 1024.0
for i in range(0, 15):
points[i][0] = 1024 * random.random() / (RAND_MAX + 1.0)
points[i][1] = 1024 * random.random() / (RAND_MAX + 1.0)
points[i][2] = 1.0
points[0][2] = 2.0
points[3][2] = -2.0
points[6][2] = 4.0
cloud.from_array(points)
# std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;
# for (size_t i = 0; i < cloud->points.size (); ++i)
# std::cerr << " " << cloud->points[i].x << " "
# << cloud->points[i].y << " "
# << cloud->points[i].z << std::endl;
#
print('Point cloud data: ' + str(cloud.size) + ' points')
for i in range(0, cloud.size):
print('x: ' + str(cloud[i][0]) + ', y : ' +
str(cloud[i][1]) + ', z : ' + str(cloud[i][2]))
# pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
# pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
# // Create the segmentation object
# pcl::SACSegmentation<pcl::PointXYZ> seg;
# // Optional
# seg.setOptimizeCoefficients (true);
# // Mandatory
# seg.setModelType (pcl::SACMODEL_PLANE);
# seg.setMethodType (pcl::SAC_RANSAC);
# seg.setDistanceThreshold (0.01);
#
# seg.setInputCloud (cloud);
# seg.segment (*inliers, *coefficients);
###
# http://www.pcl-users.org/pcl-SACMODEL-CYLINDER-is-not-working-td4037530.html
# NG?
# seg = cloud.make_segmenter()
# seg.set_optimize_coefficients(True)
# seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
# seg.set_method_type(pcl.SAC_RANSAC)
# seg.set_distance_threshold(0.01)
# indices, coefficients = seg.segment()
seg = cloud.make_segmenter_normals(ksearch=50)
seg.set_optimize_coefficients(True)
seg.set_model_type(pcl.SACMODEL_NORMAL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.01)
seg.set_normal_distance_weight(0.01)
seg.set_max_iterations(100)
indices, coefficients = seg.segment()
# if (inliers->indices.size () == 0)
# {
# PCL_ERROR ("Could not estimate a planar model for the given dataset.");
# return (-1);
# }
# std::cerr << "Model coefficients: " << coefficients->values[0] << " "
# << coefficients->values[1] << " "
# << coefficients->values[2] << " "
# << coefficients->values[3] << std::endl;
###
if len(indices) == 0:
print('Could not estimate a planar model for the given dataset.')
exit(0)
print('Model coefficients: ' + str(coefficients[0]) + ' ' + str(
coefficients[1]) + ' ' + str(coefficients[2]) + ' ' + str(coefficients[3]))
# 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;
###
print('Model inliers: ' + str(len(indices)))
for i in range(0, len(indices)):
print(str(indices[i]) + ', x: ' + str(cloud[indices[i]][0]) + ', y : ' +
str(cloud[indices[i]][1]) + ', z : ' + str(cloud[indices[i]][2]))
if __name__ == "__main__":
# import cProfile
# cProfile.run('main()', sort='time')
main()
运行结果:
Point cloud data: 15 points
x: 0.4073253273963928, y : 0.18234382569789886, z : 2.0
x: 0.6126348376274109, y : 0.07198140025138855, z : 1.0
x: 0.20239822566509247, y : 0.7896735072135925, z : 1.0
x: 0.019813423976302147, y : 0.8002557158470154, z : -2.0
x: 0.560012936592102, y : 0.6460093855857849, z : 1.0
x: 0.11512751877307892, y : 0.29834282398223877, z : 1.0
x: 0.5930672883987427, y : 0.4660494923591614, z : 4.0
x: 0.326516330242157, y : 0.2036980241537094, z : 1.0
x: 0.0727834403514862, y : 0.2508666515350342, z : 1.0
x: 0.6440120935440063, y : 0.9432345628738403, z : 1.0
x: 0.3259447515010834, y : 0.9963557124137878, z : 1.0
x: 0.7400149703025818, y : 0.9620276689529419, z : 1.0
x: 0.16831167042255402, y : 0.2292098104953766, z : 1.0
x: 0.69353848695755, y : 0.7191517353057861, z : 1.0
x: 0.30966588854789734, y : 0.47624409198760986, z : 1.0
Model coefficients: 0.7691919207572937 -0.6329861283302307 -0.08759189397096634 0.18918786942958832
Model inliers: 4
5, x: 0.11512751877307892, y : 0.29834282398223877, z : 1.0
6, x: 0.5930672883987427, y : 0.4660494923591614, z : 4.0
8, x: 0.0727834403514862, y : 0.2508666515350342, z : 1.0
9, x: 0.6440120935440063, y : 0.9432345628738403, z : 1.0