0

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:

  1. accumulate some pointclouds to add noisy areas to the octree
  2. load the accumulated pointcloud to a OctreePointCloudChangeDetector member and switch the buffer
  3. 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);
        }
fehlfarbe
  • 1
  • 3
  • Have you tried to use `deleteTree()` instead? – BTables Aug 25 '21 at 17:57
  • @BTables `deleteTree()` prevents memory increase but it also deletes the background cloud octree (?) and the output cloud contains all points from the input cloud. – fehlfarbe Aug 25 '21 at 18:51

0 Answers0