Spatial change detection on unorganized point cloud data
An octree is a tree-based data structure for organizing sparse 3-D data. In this tutorial we will learn how to use the octree implementation for detecting spatial changes between multiple unorganized point clouds which could vary in size, resolution, density and point ordering. By recursively comparing the tree structures of octrees, spatial changes represented by differences in voxel configuration can be identified. Additionally, we explain how to use the pcl octree “double buffering” technique allows us to efficiently process multiple point clouds over time.
The code:
First, create a file, let’s say, octree_change_detection.cpp and place the following inside it:
1#include <pcl/point_cloud.h>
2#include <pcl/octree/octree_pointcloud_changedetector.h>
3
4#include <iostream>
5#include <vector>
6#include <ctime>
7
8int
9main ()
10{
11 srand ((unsigned int) time (NULL));
12
13 // Octree resolution - side length of octree voxels
14 float resolution = 32.0f;
15
16 // Instantiate octree-based point cloud change detection class
17 pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);
18
19 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );
20
21 // Generate pointcloud data for cloudA
22 cloudA->width = 128;
23 cloudA->height = 1;
24 cloudA->points.resize (cloudA->width * cloudA->height);
25
26 for (std::size_t i = 0; i < cloudA->size (); ++i)
27 {
28 (*cloudA)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
29 (*cloudA)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
30 (*cloudA)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
31 }
32
33 // Add points from cloudA to octree
34 octree.setInputCloud (cloudA);
35 octree.addPointsFromInputCloud ();
36
37 // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
38 octree.switchBuffers ();
39
40 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
41
42 // Generate pointcloud data for cloudB
43 cloudB->width = 128;
44 cloudB->height = 1;
45 cloudB->points.resize (cloudB->width * cloudB->height);
46
47 for (std::size_t i = 0; i < cloudB->size (); ++i)
48 {
49 (*cloudB)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
50 (*cloudB)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
51 (*cloudB)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
52 }
53
54 // Add points from cloudB to octree
55 octree.setInputCloud (cloudB);
56 octree.addPointsFromInputCloud ();
57
58 std::vector<int> newPointIdxVector;
59
60 // Get vector of point indices from octree voxels which did not exist in previous buffer
61 octree.getPointIndicesFromNewVoxels (newPointIdxVector);
62
63 // Output points
64 std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
65 for (std::size_t i = 0; i < newPointIdxVector.size (); ++i){
66 std::cout << i << "# Index:" << newPointIdxVector[i]
67 << " Point:" << (*cloudB)[newPointIdxVector[i]].x << " "
68 << (*cloudB)[newPointIdxVector[i]].y << " "
69 << (*cloudB)[newPointIdxVector[i]].z << std::endl;
70 }
71}
The explanation
Now, let’s discuss the code in detail.
We first instantiate the OctreePointCloudChangeDetector class and define its voxel resolution.
srand ((unsigned int) time (NULL));
// Octree resolution - side length of octree voxels
float resolution = 32.0f;
// Instantiate octree-based point cloud change detection class
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);
Then we create a point cloud instance cloudA which is initialized with random point data. The generated point data is used to build an octree structure.
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 (std::size_t i = 0; i < cloudA->size (); ++i)
{
(*cloudA)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
(*cloudA)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
(*cloudA)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
}
// Add points from cloudA to octree
octree.setInputCloud (cloudA);
octree.addPointsFromInputCloud ();
Point cloud cloudA is our reference point cloud and the octree structure describe its spatial distribution. The class OctreePointCloudChangeDetector inherits from class Octree2BufBase which enables to keep and manage two octrees in the memory at the same time. In addition, it implements a memory pool that reuses already allocated node objects and therefore reduces expensive memory allocation and deallocation operations when generating octrees of multiple point clouds. By calling “octree.switchBuffers()”, we reset the octree class while keeping the previous octree structure in memory.
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers ();
Now we instantiate a second point cloud “cloudB” and fill it with random point data. This point cloud is used to build a new octree structure.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
// Generate pointcloud data for cloudB
cloudB->width = 128;
cloudB->height = 1;
cloudB->points.resize (cloudB->width * cloudB->height);
for (std::size_t i = 0; i < cloudB->size (); ++i)
{
(*cloudB)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
(*cloudB)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
(*cloudB)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
}
// Add points from cloudB to octree
octree.setInputCloud (cloudB);
octree.addPointsFromInputCloud ();
In order to retrieve points that are stored at voxels of the current octree structure (based on cloudB) which did not exist in the previous octree structure (based on cloudA), we can call the method “getPointIndicesFromNewVoxels” which return a vector of the result point indices.
std::vector<int> newPointIdxVector;
// Get vector of point indices from octree voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
Finally, we output the results to the std::cout stream.
// Output points
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
for (std::size_t i = 0; i < newPointIdxVector.size (); ++i){
std::cout << i << "# Index:" << newPointIdxVector[i]
<< " Point:" << (*cloudB)[newPointIdxVector[i]].x << " "
<< (*cloudB)[newPointIdxVector[i]].y << " "
<< (*cloudB)[newPointIdxVector[i]].z << std::endl;
Compiling and running the program
Add the following lines to your CMakeLists.txt file:
1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
2
3project(octree_change_detection)
4
5find_package(PCL 1.2 REQUIRED)
6
7include_directories(${PCL_INCLUDE_DIRS})
8link_directories(${PCL_LIBRARY_DIRS})
9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (octree_change_detection octree_change_detection.cpp)
12target_link_libraries (octree_change_detection ${PCL_LIBRARIES})
After you have made the executable, you can run it. Simply do:
$ ./octree_change_detection
You will see something similar to:
Output from getPointIndicesFromNewVoxels:
0# Index:11 Point:5.56047 56.5082 10.2807
1# Index:34 Point:1.27106 63.8973 14.5316
2# Index:102 Point:6.42197 60.7727 14.7087
3# Index:105 Point:5.64673 57.736 25.7479
4# Index:66 Point:22.8585 56.4647 63.9779
5# Index:53 Point:52.0745 14.9643 63.5844
Another example application: OpenNI change viewer
The pcl visualization component contains an openNI change detector example. It displays grabbed point clouds from the OpenNI interface and displays detected spatial changes in red.
Simply execute:
$ cd visualization/tools
$ ./openni_change_viewer
And you should see something like this:
Conclusion
This octree-based change detection enables to analyse “unorganized” point clouds for spatial changes.
Additional Details
“Unorganized” point clouds are characterized by non-existing point references between points from different point clouds due to varying size, resolution, density and/or point ordering. In case of “organized” point clouds often based on a single 2D depth/disparity images with fixed width and height, a differential analysis of the corresponding 2D depth data might be faster.