[python][pcl]python-pcl案例之基于无组织点云数据的空间变化检测

测试环境：

pcl==1.12.1

python-pcl==0.3.1

python==3.7

代码：

# -*- coding: utf-8 -*-
# Spatial change detection on unorganized point cloud data
# http://pointclouds.org/documentation/tutorials/octree_change.php#octree-change-detection

import pcl
import numpy as np
import random


def main():
    # // Octree resolution - side length of octree voxels
    resolution = 32.0

    # // Instantiate octree-based point cloud change detection class
    # pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);
    # pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );
    # // Generate pointcloud data for cloudA
    # cloudA->width = 128;
    # cloudA->height = 1;
    # cloudA->points.resize (cloudA->width * cloudA->height);
    # for (size_t i = 0; i < cloudA->points.size (); ++i)
    # {
    #     cloudA->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    #     cloudA->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    #     cloudA->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
    # }
    #
    # // Add points from cloudA to octree
    # octree.setInputCloud (cloudA);
    # octree.addPointsFromInputCloud ();
    cloudA = pcl.PointCloud()

    points = np.zeros((128, 3), dtype=np.float32)
    RAND_MAX = 1.0
    for i in range(0, 5):
        points[i][0] = 64.0 * random.random() / RAND_MAX
        points[i][1] = 64.0 * random.random() / RAND_MAX
        points[i][2] = 64.0 * random.random() / RAND_MAX

    cloudA.from_array(points)
    octree = cloudA.make_octreeChangeDetector(resolution)
    octree.add_points_from_input_cloud()
    ###

    # // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
    # octree.switchBuffers ();
    octree.switchBuffers()

    # pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
    cloudB = pcl.PointCloud()

    # // Generate pointcloud data for cloudB
    # cloudB->width = 128;
    # cloudB->height = 1;
    # cloudB->points.resize (cloudB->width * cloudB->height);
    #
    # for (size_t i = 0; i < cloudB->points.size (); ++i)
    # {
    #   cloudB->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    #   cloudB->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    #   cloudB->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
    # }
    # // Add points from cloudB to octree
    #  octree.setInputCloud (cloudB);
    #  octree.addPointsFromInputCloud ();
    points2 = np.zeros((128, 3), dtype=np.float32)
    for i in range(0, 128):
        points2[i][0] = 64.0 * random.random() / RAND_MAX
        points2[i][1] = 64.0 * random.random() / RAND_MAX
        points2[i][2] = 64.0 * random.random() / RAND_MAX

    cloudB.from_array(points2)

    octree.set_input_cloud(cloudB)
    octree.add_points_from_input_cloud()

    # std::vector<int> newPointIdxVector;
    # // Get vector of point indices from octree voxels which did not exist in previous buffer
    # octree.getPointIndicesFromNewVoxels (newPointIdxVector);
    # // Output points
    # std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
    # for (size_t i = 0; i < newPointIdxVector.size (); ++i)
    #   std::cout << i << "# Index:" << newPointIdxVector[i]
    #             << "  Point:" << cloudB->points[newPointIdxVector[i]].x << " "
    #             << cloudB->points[newPointIdxVector[i]].y << " "
    #             << cloudB->points[newPointIdxVector[i]].z << std::endl;
    newPointIdxVector = octree.get_PointIndicesFromNewVoxels()
    print('Output from getPointIndicesFromNewVoxels:')

    cloudB.extract(newPointIdxVector)

    # count = newPointIdxVector.size
    for i in range(0, len(newPointIdxVector)):
        # print(str(i) + '# Index:' + str(newPointIdxVector[i]) + '  Point:' + str(cloudB[i * 3 + 0]) + ' ' + str(cloudB[i * 3 + 1]) + ' ' + str(cloudB[i * 3 + 2]) )
        # print(str(i) + '# Index:' + str(i) + '  Point:' + str(cloudB[i]))
        print(str(i) + '# Index:' + str(newPointIdxVector[i]) + '  Point:' + str(cloudB[newPointIdxVector[i]][0]) + ' ' + str(
            cloudB[newPointIdxVector[i]][1]) + ' ' + str(cloudB[newPointIdxVector[i]][2]))


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

运行结果：

Output from getPointIndicesFromNewVoxels:
0# Index:50  Point:11.972423553466797 1.8964799642562866 38.286407470703125
1# Index:63  Point:13.539965629577637 1.2798662185668945 20.3869571685791
2# Index:66  Point:11.864972114562988 6.050127029418945 30.22968292236328
3# Index:79  Point:10.80513858795166 5.733692646026611 21.694900512695312
4# Index:92  Point:13.254244804382324 5.490572452545166 38.35628128051758
5# Index:31  Point:1.399954080581665 6.5627851486206055 59.77663803100586
6# Index:69  Point:6.051963806152344 8.723193168640137 47.4965705871582
7# Index:81  Point:9.219134330749512 10.29308795928955 52.557762145996094
8# Index:2  Point:11.686635971069336 43.0211067199707 2.6150615215301514
9# Index:15  Point:12.163130760192871 23.522417068481445 6.489630699157715
10# Index:77  Point:7.4891276359558105 41.68091583251953 12.935272216796875
11# Index:4  Point:5.716355323791504 37.78592300415039 43.62761688232422
12# Index:16  Point:3.167928457260132 20.018701553344727 29.55776596069336
13# Index:40  Point:4.608023643493652 21.787654876708984 15.523554801940918
14# Index:58  Point:10.125600814819336 15.933192253112793 22.04520606994629
15# Index:84  Point:11.526062965393066 27.955076217651367 37.99164962768555
16# Index:108  Point:10.02737045288086 29.09772300720215 20.512739181518555
17# Index:109  Point:3.084271192550659 20.944135665893555 21.518613815307617
18# Index:120  Point:5.55586051940918 23.939077377319336 19.712251663208008
19# Index:127  Point:4.852753162384033 31.202938079833984 38.0097541809082
20# Index:27  Point:5.1185078620910645 60.477569580078125 8.828771591186523
21# Index:42  Point:0.9534367322921753 51.08786392211914 6.395307540893555
22# Index:45  Point:0.8131439089775085 52.32958984375 5.981511116027832
23# Index:46  Point:7.095189094543457 61.94698715209961 4.2420735359191895
24# Index:105  Point:6.555032730102539 61.077632904052734 13.5862455368042
25# Index:123  Point:14.773215293884277 47.00944900512695 2.0186405181884766
26# Index:12  Point:14.735007286071777 58.55983352661133 42.63286590576172
27# Index:13  Point:12.096949577331543 53.69136428833008 34.9669075012207
28# Index:91  Point:4.499029636383057 53.8192024230957 29.055084228515625
29# Index:95  Point:4.944021224975586 57.4706916809082 20.636672973632812
30# Index:112  Point:10.80593490600586 58.96677017211914 36.17029571533203
31# Index:55  Point:5.961886405944824 34.91774368286133 62.891109466552734
32# Index:72  Point:0.5914714932441711 23.037723541259766 52.610755920410156
33# Index:94  Point:7.45672082901001 21.772293090820312 61.54426956176758
34# Index:118  Point:5.897623062133789 28.23716926574707 61.058128356933594
35# Index:125  Point:1.8563398122787476 37.02959060668945 56.08918380737305
36# Index:97  Point:46.033416748046875 7.087563991546631 1.0569145679473877
37# Index:21  Point:43.512813568115234 4.418891429901123 16.364845275878906
38# Index:22  Point:37.22151184082031 9.431781768798828 42.97357177734375
39# Index:24  Point:40.16318130493164 1.7755177021026611 18.956172943115234
40# Index:29  Point:23.022014617919922 9.17255687713623 15.644731521606445
41# Index:103  Point:17.698322296142578 0.22400006651878357 17.39834976196289
42# Index:124  Point:22.623138427734375 10.886869430541992 18.776472091674805
43# Index:7  Point:54.98961639404297 9.106148719787598 10.35471248626709
44# Index:56  Point:48.773651123046875 10.421725273132324 0.17998644709587097
45# Index:101  Point:52.49997329711914 10.05856990814209 15.622115135192871
46# Index:67  Point:28.932676315307617 7.106484413146973 56.863624572753906
47# Index:70  Point:33.90690994262695 3.9379823207855225 47.71257019042969
48# Index:88  Point:47.58229446411133 10.7798490524292 51.920555114746094
49# Index:98  Point:16.057056427001953 7.660116195678711 53.91740417480469
50# Index:1  Point:47.33275604248047 54.9852294921875 28.96578025817871
51# Index:5  Point:45.66539764404297 58.903873443603516 39.416358947753906
52# Index:28  Point:23.649169921875 53.32499313354492 23.905054092407227
53# Index:34  Point:36.85371780395508 62.36407470703125 39.98997497558594
54# Index:37  Point:35.166770935058594 51.478309631347656 18.695758819580078
55# Index:61  Point:32.91502380371094 62.786067962646484 27.32115936279297
56# Index:74  Point:35.40948486328125 45.162593841552734 35.38079833984375
57# Index:93  Point:24.023229598999023 50.614051818847656 38.55327606201172
58# Index:96  Point:31.08929443359375 47.06435775756836 27.249977111816406
59# Index:116  Point:19.513914108276367 62.72019577026367 43.8797492980957
60# Index:117  Point:29.41721534729004 45.48991012573242 35.05834197998047
61# Index:126  Point:24.33269691467285 63.25983428955078 45.53131866455078
62# Index:17  Point:54.645050048828125 13.41910171508789 5.3019185066223145
63# Index:82  Point:63.2945671081543 20.938364028930664 12.441509246826172
64# Index:10  Point:50.855804443359375 30.97215461730957 23.455347061157227
65# Index:25  Point:58.95404052734375 13.06283187866211 30.566320419311523
66# Index:41  Point:53.29911804199219 27.371829986572266 16.288183212280273
67# Index:43  Point:61.57455062866211 26.00208854675293 34.39857482910156
68# Index:54  Point:62.070159912109375 22.1309871673584 14.310153007507324
69# Index:64  Point:55.58402633666992 19.69698143005371 38.05708312988281
70# Index:83  Point:63.747413635253906 41.72088623046875 43.209651947021484
71# Index:100  Point:54.178131103515625 37.363407135009766 27.988304138183594
72# Index:107  Point:53.347015380859375 33.00093078613281 36.92409896850586
73# Index:14  Point:51.606876373291016 51.62226104736328 9.026566505432129
74# Index:19  Point:58.94906234741211 61.41598892211914 12.996980667114258
75# Index:73  Point:58.16590118408203 51.89847946166992 0.5953369140625
76# Index:85  Point:61.61116027832031 50.886051177978516 5.005516052246094
77# Index:11  Point:32.77092742919922 41.12651443481445 61.173858642578125
78# Index:23  Point:33.76865005493164 26.893245697021484 62.152286529541016
79# Index:26  Point:23.125234603881836 17.90355110168457 61.54076385498047
80# Index:32  Point:35.11180114746094 14.158604621887207 55.57609558105469
81# Index:35  Point:29.81991958618164 26.031063079833984 60.866966247558594
82# Index:71  Point:28.69194984436035 22.044692993164062 47.41803741455078
83# Index:75  Point:32.57098388671875 30.87228012084961 60.75761795043945
84# Index:102  Point:33.65158462524414 25.498821258544922 55.05491256713867
85# Index:104  Point:38.8353157043457 21.003772735595703 50.575355529785156
86# Index:121  Point:17.31923484802246 27.67675018310547 56.93491744995117
87# Index:6  Point:46.288177490234375 52.3199577331543 54.75199508666992
88# Index:9  Point:31.036348342895508 54.268836975097656 56.902671813964844
89# Index:33  Point:43.259029388427734 43.618995666503906 62.211219787597656
90# Index:36  Point:31.63159942626953 50.39216232299805 57.794124603271484
91# Index:78  Point:39.373619079589844 59.80958557128906 47.219749450683594
92# Index:39  Point:50.18341064453125 23.2985897064209 49.241512298583984
93# Index:44  Point:58.1259651184082 32.48925018310547 61.03275680541992
94# Index:52  Point:60.27777862548828 33.700557708740234 51.230934143066406
95# Index:86  Point:53.71144104003906 39.02741622924805 48.724979400634766
96# Index:119  Point:48.55701446533203 31.179954528808594 51.34789276123047
97# Index:18  Point:52.60405349731445 53.203529357910156 59.918251037597656
98# Index:115  Point:48.702049255371094 44.258689880371094 61.852664947509766