Intersection of PointClouds using PCL

Question

Let's say I have two different pcl::PointCloud<pcl::PointXYZL> (altough the point type doesn't really matters), c1 and c2.

I'd like to find the intersection of these two pointclouds. By intersection I mean the pointcloud inter constructed such that a point pi from c1 is inserted in inter if (and only if) a point pj exists in c2 and

pi.x == pj.x && pi.y == pj.y && pi.z == pj.z

At the moment I'm using the following functions to achieve this:

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

using namespace pcl;

typedef PointXYZL PointLT;
typedef PointCloud<PointLT> PointLCloudT;

bool contains(PointLCloudT::Ptr c, PointLT p) {
    PointLCloudT::iterator it = c->begin();
    for (; it != c->end(); ++it) {
        if (it->x == p.x && it->y == p.y && it->z == p.z)
            return true;
    }
    return false;
}

PointLCloudT::Ptr intersection(PointLCloudT::Ptr c1,
        PointLCloudT::Ptr c2) {
    PointLCloudT::Ptr inter;
    PointLCloudT::iterator it = c1->begin();
    for (; it != c1->end(); ++it) {
        if (contains(c2, *it))
            inter->push_back(*it);
    }

    return inter;
}

I'd like to know if there's a standard (and possibly more efficient) way of doing this?

I haven't found anything about this in the official documentation, but maybe I'm missing something.

Thank you.

Answer 1

If you're only looking for exact matches, as opposed to approximate matches, you can simply put the points from each point cloud in a std::vector, sort it, then use std::set_intersection to identify the matches.