[python][pcl]python-pcl案例之平面模型分割

测试环境：

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