测试环境：
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
