[python][pcl]python-pcl案例之八叉树的空间划分和搜索操作

最新推荐文章于 2024-07-21 22:27:18 发布

FL1623863129

最新推荐文章于 2024-07-21 22:27:18 发布

阅读量315

点赞数

分类专栏： python-pcl 文章标签： python 开发语言

本文链接：https://blog.csdn.net/FL1623863129/article/details/131492163

版权

python-pcl 专栏收录该内容

14 篇文章 10 订阅

订阅专栏

测试环境：

pcl==1.12.1

python-pcl==0.3.1

python==3.7

代码：

# -*- coding: utf-8 -*-
# Spatial Partitioning and Search Operations with Octrees
# http://pointclouds.org/documentation/tutorials/octree.php#octree-search

import pcl
import numpy as np
import random


def main():
    # pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    cloud = pcl.PointCloud()

    ##
    # // Generate pointcloud data
    # cloud->width = 1000;
    # cloud->height = 1;
    # cloud->points.resize (cloud->width * cloud->height);
    #
    # for (size_t i = 0; i < cloud->points.size (); ++i)
    # {
    #     cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    #     cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    #     cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
    # }
    #
    points = np.zeros((1000, 3), dtype=np.float32)
    RAND_MAX = 1024.0
    for i in range(0, 1000):
        points[i][0] = 1024 * random.random() / RAND_MAX
        points[i][1] = 1024 * random.random() / RAND_MAX
        points[i][2] = 1024 * random.random() / RAND_MAX

    cloud.from_array(points)

    # pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
    # octree.setInputCloud (cloud);
    # octree.addPointsFromInputCloud ();

    # resolution = 128.0
    # x,y,z Area Filter
    resolution = 0.2
    octree = cloud.make_octreeSearch(resolution)
    octree.add_points_from_input_cloud()

    # pcl::PointXYZ searchPoint;
    # searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    # searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    # searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
    searchPoint = pcl.PointCloud()
    searchPoints = np.zeros((1, 3), dtype=np.float32)
    # searchPoints[0][0] = 1024 * random.random () / (RAND_MAX + 1.0)
    # searchPoints[0][1] = 1024 * random.random () / (RAND_MAX + 1.0)
    # searchPoints[0][2] = 1024 * random.random () / (RAND_MAX + 1.0)
    searchPoints[0][0] = 1024 * random.random() / (RAND_MAX + 1.0)
    searchPoints[0][1] = 1024 * random.random() / (RAND_MAX + 1.0)
    searchPoints[0][2] = 1024 * random.random() / (RAND_MAX + 1.0)

    searchPoint.from_array(searchPoints)

    ##
    # // Neighbors within voxel search
    # std::vector<int> pointIdxVec;
    #
    #   if (octree.voxelSearch (searchPoint, pointIdxVec))
    #   {
    #     std::cout << "Neighbors within voxel search at (" << searchPoint.x
    #      << " " << searchPoint.y
    #      << " " << searchPoint.z << ")"
    #      << std::endl;
    #
    #     for (size_t i = 0; i < pointIdxVec.size (); ++i)
    #    std::cout << "    " << cloud->points[pointIdxVec[i]].x
    #        << " " << cloud->points[pointIdxVec[i]].y
    #        << " " << cloud->points[pointIdxVec[i]].z << std::endl;
    #   }
    ind = octree.VoxelSearch(searchPoint)

    print('Neighbors within voxel search at (' + str(searchPoint[0][0]) + ' ' + str(
        searchPoint[0][1]) + ' ' + str(searchPoint[0][2]) + ')')
    # for i in range(0, ind.size):
    for i in range(0, ind.size):
        print('index = ' + str(ind[i]))
        print('(' + str(cloud[ind[i]][0]) + ' ' +
              str(cloud[ind[i]][1]) + ' ' + str(cloud[ind[i]][2]))

    ##
    # // K nearest neighbor search
    # std::vector<int> pointIdxNKNSearch;
    # std::vector<float> pointNKNSquaredDistance;
    #
    # std::cout << "K nearest neighbor search at (" << searchPoint.x
    #           << " " << searchPoint.y
    #           << " " << searchPoint.z
    #           << ") with K=" << K << std::endl;
    K = 10
    print('K nearest neighbor search at (' + str(searchPoint[0][0]) + ' ' + str(
        searchPoint[0][1]) + ' ' + str(searchPoint[0][2]) + ') with K=' + str(K))

    # if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    # {
    #   for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
    #     std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x
    #               << " " << cloud->points[ pointIdxNKNSearch[i] ].y
    #               << " " << cloud->points[ pointIdxNKNSearch[i] ].z
    #               << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    # }
    # // Neighbors within radius search
    [ind, sqdist] = octree.nearest_k_search_for_cloud(searchPoint, K)
    # if nearest_k_search_for_cloud
    for i in range(0, ind.size):
        print('(' + str(cloud[ind[0][i]][0]) + ' ' + str(cloud[ind[0][i]][1]) + ' ' + str(
            cloud[ind[0][i]][2]) + ' (squared distance: ' + str(sqdist[0][i]) + ')')

    ##
    # std::vector<int> pointIdxRadiusSearch;
    # std::vector<float> pointRadiusSquaredDistance;
    # float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
    # std::cout << "Neighbors within radius search at (" << searchPoint.x
    #     << " " << searchPoint.y
    #     << " " << searchPoint.z
    #     << ") with radius=" << radius << std::endl;
    #
    radius = 256.0 * random.random() / (RAND_MAX + 1.0)
    print('Neighbors within radius search at (' + str(searchPoint[0][0]) + ' ' + str(
        searchPoint[0][1]) + ' ' + str(searchPoint[0][2]) + ') with radius=' + str(radius))

    # if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    # {
    #   for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
    #        std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x
    #                  << " " << cloud->points[ pointIdxRadiusSearch[i] ].y
    #                  << " " << cloud->points[ pointIdxRadiusSearch[i] ].z
    #                  << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    # }
    ###
    # [ind, sqdist] = octree.radius_search_for_cloud (searchPoint, radius)
    # Exception ignored in: 'pcl._pcl.to_point_t'
    # [ind, sqdist] = octree.radius_search (searchPoint, radius, 10)
    searchPoints = (searchPoint[0][0], searchPoint[0][1], searchPoint[0][2])
    [ind, sqdist] = octree.radius_search(searchPoints, radius, 10)

    # Function radius_search
    for i in range(0, ind.size):
        print('(' + str(cloud[ind[i]][0]) + ' ' + str(cloud[ind[i]][1]) + ' ' +
              str(cloud[ind[i]][2]) + ' (squared distance: ' + str(sqdist[i]) + ')')

    # Function radius_search_for_cloud
    # for i in range(0, ind.size):
    #    print ('(' + str(cloud[ind[0][i]][0]) + ' ' + str(cloud[ind[0][i]][1]) + ' ' + str(cloud[ind[0][i]][2]) + ' (squared distance: ' + str(sqdist[0][i]) + ')')


if __name__ == "__main__":
    # import cProfile
    # cProfile.run('main()', sort='time')
    main()

运行结果：

VoxelSearch at (0.37849587202072144 0.19727467000484467 0.3064234256744385)
Neighbors within voxel search at (0.37849587202072144 0.19727467000484467 0.3064234256744385)
index = 45
(0.4282791316509247 0.19563999772071838 0.22944234311580658
index = 364
(0.44496798515319824 0.04221676290035248 0.3575461804866791
index = 388
(0.48459964990615845 0.1802610605955124 0.2799946069717407
index = 907
(0.458575040102005 0.1072542816400528 0.24344711005687714
K nearest neighbor search at (0.37849587202072144 0.19727467000484467 0.3064234256744385) with K=10
(0.335491806268692 0.1623605340719223 0.26550593972206116 (squared distance: 0.0047425875)
(0.3500078320503235 0.19176329672336578 0.38017719984054565 (squared distance: 0.0062815626)
(0.30588388442993164 0.1718531847000122 0.34713444113731384 (squared distance: 0.0075761396)
(0.4282791316509247 0.19563999772071838 0.22944234311580658 (squared distance: 0.008407132)
(0.31789419054985046 0.1155492439866066 0.28539803624153137 (squared distance: 0.010793676)
(0.43619975447654724 0.267613023519516 0.3614135682582855 (squared distance: 0.0113011375)
(0.3559480905532837 0.2263394445180893 0.20286111533641815 (squared distance: 0.012078315)
(0.48459964990615845 0.1802610605955124 0.2799946069717407 (squared distance: 0.012245957)
(0.2636048495769501 0.16589123010635376 0.29033729434013367 (squared distance: 0.01444363)
(0.458575040102005 0.1072542816400528 0.24344711005687714 (squared distance: 0.018482361)
Neighbors within radius search at (0.37849587202072144 0.19727467000484467 0.3064234256744385) with radius=0.14303657495545388
(0.3559480905532837 0.2263394445180893 0.20286111533641815 (squared distance: 0.012078315)
(0.2636048495769501 0.16589123010635376 0.29033729434013367 (squared distance: 0.01444363)
(0.3500078320503235 0.19176329672336578 0.38017719984054565 (squared distance: 0.0062815626)
(0.30588388442993164 0.1718531847000122 0.34713444113731384 (squared distance: 0.0075761396)
(0.335491806268692 0.1623605340719223 0.26550593972206116 (squared distance: 0.0047425875)
(0.31789419054985046 0.1155492439866066 0.28539803624153137 (squared distance: 0.010793676)
(0.4282791316509247 0.19563999772071838 0.22944234311580658 (squared distance: 0.008407132)
(0.48459964990615845 0.1802610605955124 0.2799946069717407 (squared distance: 0.012245957)
(0.458575040102005 0.1072542816400528 0.24344711005687714 (squared distance: 0.018482361)
(0.43619975447654724 0.267613023519516 0.3614135682582855 (squared distance: 0.0113011375)