Comparing pointclouds and extracting the difference

asked 2020-04-19 15:27:30 -0500

lapman gravatar image

Hi, I am finding a way to compare two consecutive pointclouds from a mapping algorithm and extracting the difference between them in order to manipulate it. Both pointcloud are in the same coordinate frame. And assuming that cloud1 is always larger than cloud0. Here is what I have done so far:

const float bad_point = std::numeric_limits<float>::quiet_NaN();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);

pcl::fromROSMsg(*cloud_msg, *cloud1);

if (cloud0->points.size() == 0) {    //init
    *cloud0 = *cloud1;

    return;
}
else {
    if (cloud1->points.size() > cloud0->points.size()) {  
        for (size_t i = 0; i < (cloud0->points.size()); i++) {    //assuming newer points have larger indices
            cloud1->points[i].x = bad_point;
            cloud1->points[i].y = bad_point;
            cloud1->points[i].z = bad_point;    //remove old points
        }

        boost::shared_ptr<std::vector<int>> indices(new std::vector<int>);
        pcl::removeNaNFromPointCloud(*cloud1, *indices);
        pcl::ExtractIndices<pcl::PointXYZ> removePrevPts;
        removePrevPts.setInputCloud(cloud1);
        removePrevPts.setIndices(indices);
        removePrevPts.setNegative(false);
        removePrevPts.filter(*cloud1);
        *cloud0 += *cloud1;   //reconstruct map
    }   
}

However after a few iterations of the code, cloud1 stop updating, so I think that the indices are wrong. I am running Ubuntu 18.04 LTS and ros-melodic.

Any help would be very appreciated!

Thanks in advance

Lapman

edit retag flag offensive close merge delete