RadiusOutlierRemoval<PCLPointCloud2> implementation is slow and confusing

Hi,

Context

I had some troubles with the implementation of the RadiusOutlierRemoval<PCLPointCloud2> filter : it doesn't have the same behavior as the templated RadiusOutlierRemoval<PointT> implementation. I wrote the following unit-test to show the problem :

Code to Reproduce

GTEST_TEST(OutlierRemovalTest, SimpleTest)
{
    using namespace pcl;

    PointCloud<PointXYZ>::Ptr pc_in(new PointCloud<PointXYZ>());
    pc_in->push_back(PointXYZ(1.f, 0.f, 0.f));
    pc_in->push_back(PointXYZ(2.f, 0.f, 0.f));
    pc_in->push_back(PointXYZ(3.f, 0.f, 0.f));

    PCLPointCloud2::Ptr pc2_in(new PCLPointCloud2());
    toPCLPointCloud2(*pc_in, *pc2_in);

    {
        RadiusOutlierRemoval<PointXYZ> filter;
        filter.setInputCloud(pc_in);
        filter.setRadiusSearch(1.1);
        filter.setMinNeighborsInRadius(2);
        PointCloud<PointXYZ> pc_out;
        filter.filter(pc_out);
        EXPECT_EQ(pc_out.size(), 1);
        EXPECT_NEAR(pc_out.at(0).x, 2.f, 1e-4f);
        EXPECT_NEAR(pc_out.at(0).y, 0.f, 1e-4f);
        EXPECT_NEAR(pc_out.at(0).z, 0.f, 1e-4f);
    }

    {
        RadiusOutlierRemoval<PCLPointCloud2> filter;
        filter.setInputCloud(pc2_in);
        filter.setRadiusSearch(1.1);
        // the implementation seems to consider itself as a neigbour, the test only works if
        // we change the next line to filter.setMinNeighborsInRadius(3);
        filter.setMinNeighborsInRadius(2);
        PCLPointCloud2 pc2_out;
        PointCloud<PointXYZ> pc_out;
        filter.filter(pc2_out);
        fromPCLPointCloud2(pc2_out, pc_out);
        EXPECT_EQ(pc_out.size(), 1); // this will fail
        EXPECT_NEAR(pc_out.at(0).x, 2.f, 1e-4f);
        EXPECT_NEAR(pc_out.at(0).y, 0.f, 1e-4f);
        EXPECT_NEAR(pc_out.at(0).z, 0.f, 1e-4f);
    }
}

Current Behavior

This test fails for the RadiusOutlierRemoval<PCLPointCloud2> filter.

Moreover the RadiusOutlierRemoval<PCLPointCloud2> implementation is a lot slower in my test cases : For a dataset of ~12000 points from a 3D Lidar I measured the following timings :

BM_RadiusOutlierRemoval<PointXYZI>             18.8 ms
BM_RadiusOutlierRemoval<PCLPointCloud2>        143 ms

Possible Solution

It is confusing to have two different implementations for the same filter. I think the best solution that doesn't break the API for existing projects would be marking the RadiusOutlierRemoval<PCLPointCloud2> implementation as deprecated, explaining RadiusOutlierRemoval<PointT> should be used instead. If you agree with this change I can make a pull request.