I want to subtract a background pointcloud from my current pointcloud in a ROS nodelet to filter moving objects.
My Pointcloud2 subscriber has different steps:
- accumulate some pointclouds to add noisy areas to the octree
- load the accumulated pointcloud to a
OctreePointCloudChangeDetector
member and switch the buffer - add the current pointcloud to the second buffer, find new voxels, publish all points in new voxels and reset the buffer via
OctreePointCloudChangeDetector::deleteCurrentBuffer()
for the next pointcloud
The algorithm works great for my use case but it seems there is a memory leak or wrong usage of OctreePointCloudChangeDetector::deleteCurrentBuffer()
because the nodelet allocates more memory with every cycle and even after deleting the current buffer the OctreePointCloudChangeDetector::getBranchCount()
shows that the number of branches is increasing.
Do I have to reset something else or is OctreePointCloudChangeDetector only useable for one cycle and I have to build a new background octree for every cycle (which is very slow)?
The important part of my Pointcloud2 subscriber looks like this:
// output cloud
pcl::PointCloud<pcl::PointXYZI>::Ptr outputPtr(new pcl::PointCloud<pcl::PointXYZI>);
// if size not reached add to accumulated background else subtract background from current
if (background_count < param_background_count_max) {
NODELET_INFO_STREAM("Add cloud " << background_count << "/" << param_background_count_max << " to background");
// add cloud to background
*background += (*inputPtr);
background_count++;
} else {
NODELET_INFO_STREAM("Subtract background from cloud");
// create background octree if not already initialized
if (!octree_initialized) {
// set octree resolution
octree.setResolution(param_voxel_size);
// Add points from background to octree
octree.setInputCloud(background);
octree.addPointsFromInputCloud();
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree.switchBuffers();
// initialized!
octree_initialized = true;
NODELET_INFO_STREAM("Initialized background octree with resolution of " << octree.getResolution());
} else {
// delete last cloud from current buffer
octree.deleteCurrentBuffer();
}
// Add points from current cloud to octree
octree.setInputCloud(inputPtr);
octree.addPointsFromInputCloud();
NODELET_INFO_STREAM("octree branches: " << octree.getBranchCount()
<< " tree depth " << octree.getTreeDepth());
// Get vector of point indices from octree voxels which did not exist in previous buffer
std::vector<int> indicesVec;
octree.getPointIndicesFromNewVoxels(indicesVec);
pcl::PointIndicesPtr indices(new pcl::PointIndices());
indices->indices = std::move(indicesVec);
// filter by indices
pcl::ExtractIndices<pcl::PointXYZI> sor;
sor.setInputCloud(inputPtr);
sor.setIndices(indices);
sor.setNegative(false);
sor.filter(*outputPtr);
}