DataFilters.cpp

Go to the documentation of this file.

#include "../utest.h"
#include <ciso646>
#include <cmath>
#include <cstdint>
#include "pointmatcher/Functions.h"
 
using namespace std;
using namespace PointMatcherSupport;
 
//---------------------------
// DataFilter modules
//---------------------------
 
// Utility classes
class DataFilterTest: public IcpHelper
{
public:
        // Will be called for every tests
        virtual void SetUp()
        {
                icp.setDefault();
                // Uncomment for console outputs
                //setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
 
                // We'll test the filters on reading point cloud
                icp.readingDataPointsFilters.clear();
        }
 
        // Will be called for every tests
        virtual void TearDown() {}
 
        void addFilter(string name, PM::Parameters params)
        {
                std::shared_ptr<PM::DataPointsFilter> testedDataPointFilter =
                        PM::get().DataPointsFilterRegistrar.create(name, params);
 
                icp.readingDataPointsFilters.push_back(testedDataPointFilter);
        }
 
        void addFilter(string name)
        {
                std::shared_ptr<PM::DataPointsFilter> testedDataPointFilter =
                        PM::get().DataPointsFilterRegistrar.create(name);
 
                icp.readingDataPointsFilters.push_back(testedDataPointFilter);
        }
 
        DP generateRandomDataPoints(int nbPoints = 100)
        {
                const int dimFeatures = 4;
                const int dimDescriptors = 3;
                const int dimTime = 2;
 
                PM::Matrix randFeat = PM::Matrix::Random(dimFeatures, nbPoints);
                DP::Labels featLabels;
                featLabels.push_back(DP::Label("x", 1));
                featLabels.push_back(DP::Label("y", 1));
                featLabels.push_back(DP::Label("z", 1));
                featLabels.push_back(DP::Label("pad", 1));
 
                PM::Matrix randDesc = PM::Matrix::Random(dimDescriptors, nbPoints);
                DP::Labels descLabels;
                descLabels.push_back(DP::Label("dummyDesc", 3));
 
                PM::Int64Matrix randTimes = PM::Int64Matrix::Random(dimTime, nbPoints);
                DP::Labels timeLabels;
                timeLabels.push_back(DP::Label("dummyTime", 2));
 
                // Construct the point cloud from the generated matrices
                DP pointCloud = DP(randFeat, featLabels, randDesc, descLabels, randTimes, timeLabels);
 
                return pointCloud;
        }
 
        DP validateAngleLimitFilter(const DP& cloud, double thetaMin, double thetaMax, double phiMin, double phiMax, bool removeInside)
    {
        const int nbRows = cloud.features.rows();
        params = PM::Parameters();
        params["thetaMin"] = toParam(thetaMin);
        params["thetaMax"] = toParam(thetaMax);
        params["phiMin"] = toParam(phiMin);
        params["phiMax"] = toParam(phiMax);
        params["removeInside"] = toParam(removeInside);
 
        std::shared_ptr<PM::DataPointsFilter> angleLimitFilter = PM::get().DataPointsFilterRegistrar.create("AngleLimitDataPointsFilter", params);
        const DP filteredCloud = angleLimitFilter->filter(cloud);
 
        EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
        EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
        {
            const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
            NumericType r = point.norm();
            NumericType r_plane = point.head(nbRows-2).norm();
            NumericType theta = PointMatcherSupport::radToDeg(acos(point(nbRows-2) / r));
            NumericType phi = PointMatcherSupport::radToDeg(acos(point(0) / r_plane));
 
            if (point(1) < 0)
                phi = -phi;
            if (removeInside)
            {
                EXPECT_TRUE(theta < thetaMin || theta > thetaMax || phi < phiMin || phi > phiMax);
            }
            else
            {
                EXPECT_GE(theta, thetaMin);
                EXPECT_LE(theta, thetaMax);
                EXPECT_GE(phi, phiMin);
                EXPECT_LE(phi, phiMax);
            }
        }
        return filteredCloud;
    }
};
 
TEST_F(DataFilterTest, IdentityDataPointsFilter)
{
        // build test cloud
        DP ref2DCopy(ref2D);
 
        // apply and checked
        addFilter("IdentityDataPointsFilter");
        icp.readingDataPointsFilters.apply(ref2DCopy);
        EXPECT_TRUE(ref2D == ref2DCopy);
}
 
TEST_F(DataFilterTest, RemoveNaNDataPointsFilter)
{
        // build test cloud
        DP ref2DCopy(ref2D);
        int goodCount(0);
        const NumericType nan(std::numeric_limits<NumericType>::quiet_NaN());
        for (int i(0); i < ref2DCopy.features.cols(); ++i)
        {
                if (rand() % 3 == 0)
                {
                        ref2DCopy.features(rand() % ref2DCopy.features.rows(), i) = nan;
                }
                else
                        ++goodCount;
        }
 
        // apply and checked
        addFilter("RemoveNaNDataPointsFilter");
        icp.readingDataPointsFilters.apply(ref2DCopy);
        EXPECT_TRUE(ref2DCopy.features.cols() == goodCount);
}
 
TEST_F(DataFilterTest, MaxDistDataPointsFilter)
{
        // Max dist has been selected to not affect the points
        params = PM::Parameters();
        params["dim"] = "0";
        params["maxDist"] = toParam(6.0);
 
        // Filter on x axis
        params["dim"] = "0";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxDistDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on y axis
        params["dim"] = "1";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxDistDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on z axis (not existing)
        params["dim"] = "2";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxDistDataPointsFilter", params);
        EXPECT_ANY_THROW(validate2dTransformation());
        validate3dTransformation();
 
        // Filter on a radius
        params["dim"] = "-1";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxDistDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Parameter outside valid range
        params["dim"] = "3";
        //TODO: specify the exception, move that to GenericTest
        EXPECT_ANY_THROW(addFilter("MaxDistDataPointsFilter", params));
 
}
 
TEST_F(DataFilterTest, MinDistDataPointsFilter)
{
        // Min dist has been selected to not affect the points too much
        params = PM::Parameters();
        params["dim"] = "0";
        params["minDist"] = toParam(0.05);
 
        // Filter on x axis
        params["dim"] = "0";
        icp.readingDataPointsFilters.clear();
        addFilter("MinDistDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on y axis
        params["dim"] = "1";
        icp.readingDataPointsFilters.clear();
        addFilter("MinDistDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        //TODO: move that to specific 2D test
        // Filter on z axis (not existing)
        params["dim"] = "2";
        icp.readingDataPointsFilters.clear();
        addFilter("MinDistDataPointsFilter", params);
        EXPECT_ANY_THROW(validate2dTransformation());
        validate3dTransformation();
 
        // Filter on a radius
        params["dim"] = "-1";
        icp.readingDataPointsFilters.clear();
        addFilter("MinDistDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
}
 
TEST_F(DataFilterTest, MaxQuantileOnAxisDataPointsFilter)
{
        // Ratio has been selected to not affect the points too much
        string ratio = "0.95";
        params = PM::Parameters();
        params["dim"] = "0";
        params["ratio"] = ratio;
 
        // Filter on x axis
        params["dim"] = "0";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxQuantileOnAxisDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on y axis
        params["dim"] = "1";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxQuantileOnAxisDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on z axis (not existing)
        params["dim"] = "2";
        icp.readingDataPointsFilters.clear();
        addFilter("MaxQuantileOnAxisDataPointsFilter", params);
        EXPECT_ANY_THROW(validate2dTransformation());
        validate3dTransformation();
}
 
 
 
TEST_F(DataFilterTest, SurfaceNormalDataPointsFilter)
{
        // This filter create descriptor, so parameters should'nt impact results
        params = PM::Parameters();
        params["knn"] =  "5";
        params["epsilon"] =  "0.1";
        params["keepNormals"] =  "1";
        params["keepDensities"] =  "1";
        params["keepEigenValues"] =  "1";
        params["keepEigenVectors"] =  "1" ;
        params["keepMatchedIds"] =  "1" ;
        // FIXME: the parameter keepMatchedIds seems to do nothing...
 
        addFilter("SurfaceNormalDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // TODO: standardize how filter are tested:
        // 1- impact on number of points
        // 2- impact on descriptors
        // 3- impact on ICP (that's what we test now)
}
 
TEST_F(DataFilterTest, MaxDensityDataPointsFilter)
{
        // Ratio has been selected to not affect the points too much
        vector<double> ratio = {100, 1000, 5000};
 
        for(unsigned i=0; i < ratio.size(); i++)
        {
                icp.readingDataPointsFilters.clear();
                params = PM::Parameters();
                params["knn"] = "5";
                params["epsilon"] = "0.1";
                params["keepNormals"] = "0";
                params["keepDensities"] = "1";
                params["keepEigenValues"] = "0";
                params["keepEigenVectors"] = "0" ;
                params["keepMatchedIds"] = "0" ;
 
                addFilter("SurfaceNormalDataPointsFilter", params);
 
                params = PM::Parameters();
                params["maxDensity"] = toParam(ratio[i]);
 
                addFilter("MaxDensityDataPointsFilter", params);
 
                // FIXME BUG: the density in 2D is not well computed
                //validate2dTransformation();
 
                //double nbInitPts = data2D.features.cols();
                //double nbRemainingPts = icp.getPrefilteredReadingPtsCount();
                //EXPECT_TRUE(nbRemainingPts < nbInitPts);
 
                validate3dTransformation();
 
                double nbInitPts = data3D.features.cols();
                double nbRemainingPts = icp.getPrefilteredReadingPtsCount();
                EXPECT_TRUE(nbRemainingPts < nbInitPts);
        }
}
 
TEST_F(DataFilterTest, SamplingSurfaceNormalDataPointsFilter)
{
        // This filter create descriptor AND subsample
        params = PM::Parameters();
        params["knn"] = "5";
        params["averageExistingDescriptors"] = "1";
        params["keepNormals"] = "1";
        params["keepDensities"] = "1";
        params["keepEigenValues"] = "1";
        params["keepEigenVectors"] = "1";
 
        addFilter("SamplingSurfaceNormalDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
}
 
//TODO: this filter is broken, fix it!
/*
TEST_F(DataFilterTest, ElipsoidsDataPointsFilter)
{
        // This filter creates descriptor AND subsamples
        params = PM::Parameters();
        params["knn"] = "5";
        params["averageExistingDescriptors"] = "1";
        params["keepNormals"] = "1";
        params["keepDensities"] = "1";
        params["keepEigenValues"] = "1";
        params["keepEigenVectors"] = "1";
        params["maxBoxDim"] = "inf";
        params["maxTimeWindow"] = "10";
        params["minPlanarity"] = "0";
        params["keepMeans"] = "1";
        params["keepCovariances"] = "1";
        params["keepWeights"] = "1";
        params["keepShapes"] = "1";
        params["keepIndices"] = "1";
 
        addFilter("ElipsoidsDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
}
*/
 
TEST_F(DataFilterTest, GestaltDataPointsFilter)
{
        // This filter creates descriptor AND subsamples
        params = PM::Parameters();
        params["knn"] = "5";
        params["averageExistingDescriptors"] = "1";
        params["keepNormals"] = "1";
        params["keepEigenValues"] = "1";
        params["keepEigenVectors"] = "1";
        params["maxBoxDim"] = "1";
        params["keepMeans"] = "1";
        params["keepCovariances"] = "1";
        params["keepGestaltFeatures"] = "1";
        params["radius"] = "1";
        params["ratio"] = "0.5";
 
        addFilter("GestaltDataPointsFilter", params);
        validate3dTransformation();
}
 
TEST_F(DataFilterTest, OrientNormalsDataPointsFilter)
{
        // Used to create normal for reading point cloud
        std::shared_ptr<PM::DataPointsFilter> extraDataPointFilter;
        extraDataPointFilter = PM::get().DataPointsFilterRegistrar.create(
                        "SurfaceNormalDataPointsFilter");
        icp.readingDataPointsFilters.push_back(extraDataPointFilter);
        addFilter("ObservationDirectionDataPointsFilter");
        addFilter("OrientNormalsDataPointsFilter", {
                {"towardCenter", toParam(false)}
        }
        );
        validate2dTransformation();
        validate3dTransformation();
}
 
 
TEST_F(DataFilterTest, RandomSamplingDataPointsFilter)
{
        for(const double prob : {0.8, 0.85, 0.9, 0.95})
        {
                for(const unsigned int samplingMethod : {0, 1})
                {
                        // Try to avoid too low value for the reduction to avoid under sampling
                        params = PM::Parameters();
                        params["prob"] = toParam(prob);
                        params["randomSamplingMethod"] = toParam(samplingMethod);
 
                        icp.readingDataPointsFilters.clear();
                        addFilter("RandomSamplingDataPointsFilter", params);
                        validate2dTransformation();
                        validate3dTransformation();
                }
        }
}
 
TEST_F(DataFilterTest, FixStepSamplingDataPointsFilter)
{
        vector<unsigned> steps = {1, 2, 3};
        for(unsigned i=0; i<steps.size(); i++)
        {
                // Try to avoid too low value for the reduction to avoid under sampling
                params = PM::Parameters();
                params["startStep"] = toParam(steps[i]);
 
                icp.readingDataPointsFilters.clear();
                addFilter("FixStepSamplingDataPointsFilter", params);
                validate2dTransformation();
                validate3dTransformation();
        }
}
 
TEST_F(DataFilterTest, MaxPointCountDataPointsFilter)
{
        DP cloud = ref3D;
 
        const size_t maxCount = 1000;
 
        params = PM::Parameters();
        params["seed"] = "42";
        params["maxCount"] = toParam(maxCount);
 
        std::shared_ptr<PM::DataPointsFilter> maxPtsFilter =
                        PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
 
        DP filteredCloud = maxPtsFilter->filter(cloud);
 
        //Check number of points
        EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
        EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
        EXPECT_EQ(filteredCloud.getNbPoints(), maxCount);
 
        //Same seed should result same filtered cloud
        DP filteredCloud2 = maxPtsFilter->filter(cloud);
 
        EXPECT_TRUE(filteredCloud == filteredCloud2);
 
        //Different seeds should not result same filtered cloud but same number
        params.clear();
        params["seed"] = "1";
        params["maxCount"] = toParam(maxCount);
 
        std::shared_ptr<PM::DataPointsFilter> maxPtsFilter2 =
                        PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
 
        DP filteredCloud3 = maxPtsFilter2->filter(cloud);
 
        EXPECT_FALSE(filteredCloud3 == filteredCloud2);
 
        EXPECT_EQ(filteredCloud3.getNbPoints(), maxCount);
 
        EXPECT_EQ(filteredCloud3.getNbPoints(), filteredCloud2.getNbPoints());
        EXPECT_EQ(filteredCloud3.getDescriptorDim(), filteredCloud2.getDescriptorDim());
        EXPECT_EQ(filteredCloud3.getTimeDim(), filteredCloud2.getTimeDim());
 
        //Validate transformation
        icp.readingDataPointsFilters.clear();
        addFilter("MaxPointCountDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
}
 
TEST_F(DataFilterTest, OctreeGridDataPointsFilter)
{
        const unsigned int nbPts = 60000;
        const DP cloud = generateRandomDataPoints(nbPts);
        params = PM::Parameters();
 
        std::shared_ptr<PM::DataPointsFilter> octreeFilter;
 
        for(const int meth : {0,1,2,3})
                for(const size_t maxData : {1,5})
                        for(const NumericType maxSize : {0.,0.05})
                        {
                                params.clear();
                                params["maxPointByNode"] = toParam(maxData);
                                params["maxSizeByNode"] = toParam(maxSize);
                                params["samplingMethod"] = toParam(meth);
                                params["buildParallel"] = "1";
 
                                octreeFilter = PM::get().DataPointsFilterRegistrar.create("OctreeGridDataPointsFilter", params);
 
                                const DP filteredCloud = octreeFilter->filter(cloud);
 
                                if(maxData==1 and maxSize==0.)
                                {
                                        // 1/pts by octants + validate parallel build
                                        // the number of point should not change
                                        // the sampling methods should not change anything
                                        //Check number of points
                                        EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
                                        EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
                                        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
                                        EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
                                }
                                else
                                {
                                        //Check number of points
                                        EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
                                }
                                //Validate transformation
                                icp.readingDataPointsFilters.clear();
                                addFilter("OctreeGridDataPointsFilter", params);
                                validate2dTransformation();
                                validate3dTransformation();
                        }
}
 
TEST_F(DataFilterTest, NormalSpaceDataPointsFilter)
{
        const size_t nbPts = 60000;
        DP cloud = generateRandomDataPoints(nbPts);
        params = PM::Parameters();
 
        //const size_t nbPts2D = ref2D.getNbPoints();
        const size_t nbPts3D = ref3D.getNbPoints();
 
        std::shared_ptr<PM::DataPointsFilter> nssFilter;
 
        //Compute normals
        auto paramsNorm = PM::Parameters();
                        paramsNorm["knn"] = "5";
                        paramsNorm["epsilon"] = "0.1";
                        paramsNorm["keepNormals"] = "1";
        std::shared_ptr<PM::DataPointsFilter> normalFilter = PM::get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", paramsNorm);
 
        normalFilter->inPlaceFilter(cloud);
 
        //Evaluate filter
        std::vector<size_t> samples = {/* 2*nbPts2D/3, nbPts2D,*/ 1500, 5000, nbPts, nbPts3D};
        for(const NumericType epsilon : {M_PI/6., M_PI/32., M_PI/64.})
                for(const size_t nbSample : samples)
                {
                        icp.readingDataPointsFilters.clear();
 
                        params.clear();
                        params["epsilon"] = toParam(epsilon);
                        params["nbSample"] = toParam(nbSample);
 
                        nssFilter = PM::get().DataPointsFilterRegistrar.create("NormalSpaceDataPointsFilter", params);
 
                        addFilter("SurfaceNormalDataPointsFilter", paramsNorm);
                        addFilter("NormalSpaceDataPointsFilter", params);
 
                        const DP filteredCloud = nssFilter->filter(cloud);
 
                        /*
                        if(nbSample <= nbPts2D)
                        {
                                validate2dTransformation();
                                EXPECT_LE(filteredCloud.getNbPoints(), nbPts2D);
                                continue;
                        }
                        else if (nbSample == nbPts3D)
                        {
                                EXPECT_EQ(filteredCloud.getNbPoints(), nbPts3D);
                        }
                        else */
                        if (nbSample == nbPts)
                        {
                                //Check number of points
                                EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
                                EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
                                EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
                                EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
                        }
 
                        validate3dTransformation();
                        EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
                }
}
 
TEST_F(DataFilterTest, CovarianceSamplingDataPointsFilter)
{
        const size_t nbPts = 60000;
        DP cloud = generateRandomDataPoints(nbPts);
        params = PM::Parameters();
 
        const size_t nbPts3D = ref3D.getNbPoints();
 
        std::shared_ptr<PM::DataPointsFilter> covsFilter;
 
        //Compute normals
        auto paramsNorm = PM::Parameters();
                        paramsNorm["knn"] = "5";
                        paramsNorm["epsilon"] = "0.1";
                        paramsNorm["keepNormals"] = "1";
        std::shared_ptr<PM::DataPointsFilter> normalFilter = PM::get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", paramsNorm);
 
        normalFilter->inPlaceFilter(cloud);
 
        //Evaluate filter
        std::vector<size_t> samples = {500, 1500, 5000, nbPts, nbPts3D};
        for(const size_t nbSample : samples)
        {
                icp.readingDataPointsFilters.clear();
 
                params.clear();
                params["nbSample"] = toParam(nbSample);
 
                covsFilter = PM::get().DataPointsFilterRegistrar.create("CovarianceSamplingDataPointsFilter", params);
 
                addFilter("SurfaceNormalDataPointsFilter", paramsNorm);
                addFilter("CovarianceSamplingDataPointsFilter", params);
 
                const DP filteredCloud = covsFilter->filter(cloud);
 
                if (nbSample == nbPts3D)
                {
                        EXPECT_EQ(filteredCloud.getNbPoints(), nbPts3D);
                }
                else if (nbSample == nbPts)
                {
                        //Check number of points
                        EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
                        EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
                        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
                        EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
                }
 
                validate3dTransformation();
                EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
        }
}
 
TEST_F(DataFilterTest, VoxelGridDataPointsFilter)
{
        // Test with point cloud
        DP cloud = generateRandomDataPoints();
 
        params = PM::Parameters();
        params["vSizeX"] = "0.5";
        params["vSizeY"] = "0.5";
        params["vSizeZ"] = "0.5";
        params["useCentroid"] = toParam(true);
        params["averageExistingDescriptors"] = toParam(true);
 
        std::shared_ptr<PM::DataPointsFilter> voxelFilter =
                        PM::get().DataPointsFilterRegistrar.create("VoxelGridDataPointsFilter", params);
 
        DP filteredCloud = voxelFilter->filter(cloud);
 
        EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
        EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
        // Test with ICP
        vector<bool> useCentroid = {false, true};
        vector<bool> averageExistingDescriptors = {false, true};
        for (unsigned i = 0 ; i < useCentroid.size() ; i++)
        {
                for (unsigned j = 0; j < averageExistingDescriptors.size(); j++)
                {
                        params = PM::Parameters();
                        params["vSizeX"] = "0.02";
                        params["vSizeY"] = "0.02";
                        params["vSizeZ"] = "0.02";
                        params["useCentroid"] = toParam(true);
                        params["averageExistingDescriptors"] = toParam(true);
 
                        icp.readingDataPointsFilters.clear();
                        addFilter("VoxelGridDataPointsFilter", params);
                        validate2dTransformation();
                }
        }
 
        for (unsigned i = 0 ; i < useCentroid.size() ; i++)
        {
                for (unsigned j = 0; j < averageExistingDescriptors.size(); j++)
                {
                        params = PM::Parameters();
                        params["vSizeX"] = "1";
                        params["vSizeY"] = "1";
                        params["vSizeZ"] = "1";
                        params["useCentroid"] = toParam(true);
                        params["averageExistingDescriptors"] = toParam(true);
 
                        icp.readingDataPointsFilters.clear();
                        addFilter("VoxelGridDataPointsFilter", params);
                        validate3dTransformation();
                }
        }
}
 
TEST_F(DataFilterTest, CutAtDescriptorThresholdDataPointsFilter)
{
        // Copied from density ratio above
        vector<double> thresholds = {100, 1000, 5000};
 
        DP ref3Ddensities = ref3D;
        // Adding descriptor "densities"
        icp.readingDataPointsFilters.clear();
        params = PM::Parameters();
        params["knn"] = "5";
        params["epsilon"] = "0.1";
        params["keepNormals"] = "0";
        params["keepDensities"] = "1";
        params["keepEigenValues"] = "0";
        params["keepEigenVectors"] = "0";
        params["keepMatchedIds"] = "0";
 
 
        addFilter("SurfaceNormalDataPointsFilter", params);
        icp.readingDataPointsFilters.apply(ref3Ddensities);
 
        for(unsigned i=0; i < thresholds.size(); i++)
        {
                int belowCount=0;
                int aboveCount=0;
 
                // counting points above and below
                PM::DataPoints::View densities = ref3Ddensities.getDescriptorViewByName("densities");
                for (unsigned j=0; j < densities.cols(); ++j)
                {
                        if (densities(0, j) <= thresholds[i])
                        {
                                ++belowCount;
                        }
                        if (densities(0, j) >= thresholds[i])
                        {
                                ++aboveCount;
                        }
                }
 
                for(bool useLargerThan(true); useLargerThan; useLargerThan=false)
                {
                        DP ref3DCopy = ref3Ddensities;
 
                        icp.readingDataPointsFilters.clear();
                        params = PM::Parameters();
                        params["descName"] = toParam("densities");
                        params["useLargerThan"] = toParam(useLargerThan);
                        params["threshold"] = toParam(thresholds[i]);
 
 
                        addFilter("CutAtDescriptorThresholdDataPointsFilter", params);
                        icp.readingDataPointsFilters.apply(ref3DCopy);
                        if (useLargerThan)
                        {
                                EXPECT_TRUE(ref3DCopy.features.cols() == belowCount);
                        }
                        else
                        {
                                EXPECT_TRUE(ref3DCopy.features.cols() == aboveCount);
                        }
                }
        }
}
 
TEST_F(DataFilterTest, DistanceLimitDataPointsFilter)
{
        params = PM::Parameters();
        params["dim"] = "0";
        params["dist"] = toParam(6.0);
        params["removeInside"] = "0";
 
        // Filter on x axis
        params["dim"] = "0";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on y axis
        params["dim"] = "1";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on z axis (not existing)
        params["dim"] = "2";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        EXPECT_ANY_THROW(validate2dTransformation());
        validate3dTransformation();
 
        // Filter on a radius
        params["dim"] = "-1";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Parameter outside valid range
        params["dim"] = "3";
        //TODO: specify the exception, move that to GenericTest
        EXPECT_ANY_THROW(addFilter("DistanceLimitDataPointsFilter", params));
 
 
        params = PM::Parameters();
        params["dim"] = "0";
        params["dist"] = toParam(0.05);
        params["removeInside"] = "1";
 
        // Filter on x axis
        params["dim"] = "0";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        // Filter on y axis
        params["dim"] = "1";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
 
        //TODO: move that to specific 2D test
        // Filter on z axis (not existing)
        params["dim"] = "2";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        EXPECT_ANY_THROW(validate2dTransformation());
        validate3dTransformation();
 
        // Filter on a radius
        params["dim"] = "-1";
        icp.readingDataPointsFilters.clear();
        addFilter("DistanceLimitDataPointsFilter", params);
        validate2dTransformation();
        validate3dTransformation();
}
 
TEST_F(DataFilterTest, SameFilterInstanceTwice)
{
        params = PM::Parameters();
 
        std::shared_ptr<PM::DataPointsFilter> df = PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
 
        icp.referenceDataPointsFilters.push_back(df);
        icp.readingDataPointsFilters.push_back(df);
}
 
TEST_F(DataFilterTest, RemoveSensorBiasDataPointsFilter)
{
        const size_t nbPts = 6;
        const double expectedErrors_LMS1xx[6] = {0., -0.0015156, -0.059276,
                                                 0., -0.002311, -0.163689};
        const double expectedErrors_HDL32E[6]  = {0., -0.002945, -0.075866,
                                                  0., -0.002998,-0.082777 };
 
        PM::Matrix points = (PM::Matrix(3,6) << 1, 1, 1, 5, 5, 5,
                0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0).finished();
        DP::Labels pointsLabels;
        pointsLabels.push_back(DP::Label("x", 1));
        pointsLabels.push_back(DP::Label("y", 1));
        pointsLabels.push_back(DP::Label("z", 1));
        pointsLabels.push_back(DP::Label("pad", 1));
 
        PM::Matrix desc = (PM::Matrix(4,6) << 0., 0.7854, 1.4835, 0., 0.7854, 1.4835, //0,45,85 degrees
                -1, -1, -1, -5, -5, -5,
                0,  0,  0,  0,  0,  0, // observation : point to sensor
                0,  0,  0,  0,  0,  0).finished();
 
        PM::Matrix desc2 = (PM::Matrix(4,6) << 0., 0.7854, std::nanf(""), 0., 0.7854, M_PI_2, //0,45,90 degrees
                -1, -1, -1, -5, -5, -5,
                0,  0,  0,  0,  0,  0, // observation : point to sensor
                0,  0,  0,  0,  0,  0).finished();
        DP::Labels descLabels;
        descLabels.push_back(DP::Label("incidenceAngles", 1));
        descLabels.push_back(DP::Label("observationDirections", 3));
 
        PM::Int64Matrix randTimes = PM::Int64Matrix::Random(2, nbPts);
        DP::Labels timeLabels;
        timeLabels.push_back(DP::Label("dummyTime", 2));
 
        // Construct the point cloud from the generated matrices
        DP pointCloud = DP(points, pointsLabels, desc, descLabels, randTimes, timeLabels);
 
 
        PM::Parameters parameters;
        parameters["sensorType"] = toParam(0); //LMS_1xx
        std::shared_ptr<PM::DataPointsFilter> removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
        DP resultCloud = removeSensorBiasFilter->filter(pointCloud);
        EXPECT_EQ(pointCloud.getNbPoints(), resultCloud.getNbPoints());
 
        for(std::size_t i = 0; i< nbPts; ++i)
        {
                const double error = pointCloud.features.col(i).norm() - resultCloud.features.col(i).norm();
                EXPECT_NEAR(expectedErrors_LMS1xx[i], error, 1e-3); // below mm
        }
 
        parameters["sensorType"] = toParam(1); //HDL32E
        removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
        resultCloud = removeSensorBiasFilter->filter(pointCloud);
 
        for(std::size_t i = 0; i< nbPts; ++i)
        {
                const double error = pointCloud.features.col(i).norm() - resultCloud.features.col(i).norm();
                EXPECT_NEAR(expectedErrors_HDL32E[i], error, 1e-4); // below mm
        }
 
 
        //test points rejection
        pointCloud = DP(points, pointsLabels, desc2, descLabels, randTimes, timeLabels);
 
        parameters["sensorType"] = toParam(0); //LMS_1xx
        parameters["angleThreshold"] = toParam(30.);
        removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
        resultCloud = removeSensorBiasFilter->filter(pointCloud);
 
        //four points should have been rejected
        EXPECT_EQ(pointCloud.getNbPoints()-4, resultCloud.getNbPoints());
}
 
TEST_F(DataFilterTest, SaliencyDataPointsFilter)
{
        // This filter creates descriptors
        params = PM::Parameters();
        params["k"] = "50";
        params["sigma"] = "1.0";
        params["keepNormals"] = "1";
        params["keepLabels"] = "1";
        params["keepTensors"] = "1";
 
        addFilter("SaliencyDataPointsFilter", params);
        validate3dTransformation();
}
 
TEST_F(DataFilterTest, SpectralDecompositionDataPointsFilter)
{
        using DPFiltersPtr = std::shared_ptr<PM::DataPointsFilter>;
 
        // Test with point cloud
        DP cloud = generateRandomDataPoints(300000);
 
        // This filter creates descriptors
        params = PM::Parameters();
                params["k"] = "50";
                params["sigma"] = "0.1";
                params["radius"] = "0.4";
                params["itMax"] = "15";
                params["keepNormals"] = "1";
                params["keepLabels"] = "1";
                params["keepLambdas"] = "1";
                params["keepTensors"] = "1";
 
        DPFiltersPtr spdf = PM::get().DataPointsFilterRegistrar.create(
                "SpectralDecompositionDataPointsFilter", params
        );
 
        DP filteredCloud = spdf->filter(cloud);
 
        EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
        EXPECT_EQ(cloud.getDescriptorDim()+(3+3+1+3+1+4+7+3), filteredCloud.getDescriptorDim());
        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
        params = PM::Parameters();
                params["k"] = "50";
                params["sigma"] = "1.";
                params["radius"] = "2.";
                params["itMax"] = "15";
                params["keepNormals"] = "1";
                params["keepLabels"] = "1";
                params["keepLambdas"] = "1";
                params["keepTensors"] = "1";
 
        addFilter("SpectralDecompositionDataPointsFilter", params);
        validate3dTransformation();
}
 
TEST_F(DataFilterTest, AddDescriptorDataPointsFilter)
{
        using DPFiltersPtr = std::shared_ptr<PM::DataPointsFilter>;
 
        // Test with point cloud
        DP cloud = generateRandomDataPoints(100);
 
    std::string descriptorName = "test_descriptor";
    std::size_t descriptorDimension = 3;
    std::vector<NumericType> descriptorValues{2, 3, 4};
 
        // This filter adds a new descriptor
        params = PM::Parameters();
                params["descriptorName"] = descriptorName;
                params["descriptorDimension"] = toParam(descriptorDimension);
                params["descriptorValues"] = toParam(descriptorValues);
 
        DPFiltersPtr addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create(
                "AddDescriptorDataPointsFilter", params
        );
 
        DP filteredCloud = addDescriptorFilter->filter(cloud);
 
        EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
        EXPECT_EQ(cloud.getDescriptorDim()+descriptorDimension, filteredCloud.getDescriptorDim());
        EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
 
    Eigen::Matrix<NumericType, 1, Eigen::Dynamic> row = Eigen::Matrix<NumericType, 1, Eigen::Dynamic>::Ones(cloud.getNbPoints());
    EXPECT_EQ(filteredCloud.descriptorLabels.back().text, descriptorName);
    EXPECT_EQ(filteredCloud.descriptorLabels.back().span, descriptorDimension);
    for(unsigned i = 0; i < descriptorDimension; ++i)
    {
        EXPECT_EQ(filteredCloud.descriptors.row(filteredCloud.descriptors.rows()-3+i), row*descriptorValues[i]);
    }
 
 
    descriptorValues = std::vector<NumericType>{-2, -3, -4};
    params["descriptorValues"] = toParam(descriptorValues);
 
        addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create(
                "AddDescriptorDataPointsFilter", params
        );
        DP filteredCloudOvewriteParams = addDescriptorFilter->filter(filteredCloud);
    EXPECT_EQ(filteredCloudOvewriteParams.descriptorLabels.back().text, descriptorName);
    EXPECT_EQ(filteredCloudOvewriteParams.descriptorLabels.back().span, descriptorDimension);
    for(unsigned i = 0; i < descriptorDimension; ++i)
    {
        EXPECT_EQ(filteredCloudOvewriteParams.descriptors.row(filteredCloud.descriptors.rows()-3+i), row*descriptorValues[i]);
    }
 
 
    descriptorValues = std::vector<NumericType>{-2, -3, -4, -5};
    params["descriptorDimension"] = toParam(4);
    params["descriptorValues"] = toParam(descriptorValues);
        addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create(
                "AddDescriptorDataPointsFilter", params
        );
    EXPECT_THROW(addDescriptorFilter->filter(filteredCloud), std::runtime_error);
 
        params = PM::Parameters();
                params["descriptorName"] = "my_descriptor";
                params["descriptorDimension"] = toParam(descriptorDimension);
                params["descriptorValues"] = "[2, 3, 4]";
 
        addFilter("AddDescriptorDataPointsFilter", params);
        validate3dTransformation();
}
 
 
TEST_F(DataFilterTest, AngleLimitDataPointsFilter)
{
    const unsigned int nbPts = 10000;
        const DP cloud = generateRandomDataPoints(nbPts);
 
        // Remove negative x-axis: thetaMin = -180, thetaMax = 180, phiMin = -180/2, phiMax = 180/2, removeInside = 0
        DP filteredCloud = validateAngleLimitFilter(cloud, -180, 180, -90, 90, false);
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
    {
        const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
        EXPECT_GT(point(0), 0.0);
    }
 
        // Remove positive x-axis: thetaMin = -180, thetaMax = 180, phiMin = -180/2, phiMax = 180/2, removeInside = 1
        filteredCloud = validateAngleLimitFilter(cloud, -180, 180, -90, 90, true);
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
    {
        const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
        EXPECT_LT(point(0), 0.0);
    }
 
        // Remove negative y-axis: thetaMin = -180, thetaMax = 180, phiMin = 0, phiMax = 180, removeInside = 0
        filteredCloud = validateAngleLimitFilter(cloud, -180, 180, 0, 180, false);
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
    {
        const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
        EXPECT_GT(point(1), 0.0);
    }
 
        // Remove positive y-axis: thetaMin = -180, thetaMax = 180, phiMin = 0, phiMax = 180, removeInside = 1
        filteredCloud = validateAngleLimitFilter(cloud, -180, 180, 0, 180, true);
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
    {
        const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
        EXPECT_LT(point(1), 0.0);
    }
 
        // // Remove negative z-axis: thetaMin = -90, thetaMax = 90, phiMin = -180, phiMax = 180, removeInside = 0
        filteredCloud = validateAngleLimitFilter(cloud, -90, 90, -180, 180, false);
 
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
    {
        const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
        EXPECT_GT(point(2), 0.0);
    }
 
        // // Remove positive z-axis: thetaMin = -90, thetaMax = 90, phiMin = -180, phiMax = 180, removeInside = 1
        filteredCloud = validateAngleLimitFilter(cloud, -90, 90, -180, 180, true);
        for (unsigned i = 0; i < filteredCloud.getNbPoints(); ++i)
    {
        const Eigen::Vector3d point = filteredCloud.features.col(i).head(3);
        EXPECT_LT(point(2), 0.0);
    }
}