24 icp.readingDataPointsFilters.clear();
32 std::shared_ptr<PM::DataPointsFilter> testedDataPointFilter =
33 PM::get().DataPointsFilterRegistrar.create(name, params);
35 icp.readingDataPointsFilters.push_back(testedDataPointFilter);
40 std::shared_ptr<PM::DataPointsFilter> testedDataPointFilter =
41 PM::get().DataPointsFilterRegistrar.create(name);
43 icp.readingDataPointsFilters.push_back(testedDataPointFilter);
48 const int dimFeatures = 4;
49 const int dimDescriptors = 3;
50 const int dimTime = 2;
52 PM::Matrix randFeat = PM::Matrix::Random(dimFeatures, nbPoints);
57 featLabels.push_back(
DP::Label(
"pad", 1));
59 PM::Matrix randDesc = PM::Matrix::Random(dimDescriptors, nbPoints);
61 descLabels.push_back(
DP::Label(
"dummyDesc", 3));
65 timeLabels.push_back(
DP::Label(
"dummyTime", 2));
68 DP pointCloud =
DP(randFeat, featLabels, randDesc, descLabels, randTimes, timeLabels);
80 addFilter(
"IdentityDataPointsFilter");
81 icp.readingDataPointsFilters.apply(ref2DCopy);
90 const float nan(std::numeric_limits<float>::quiet_NaN());
91 for (
int i(0); i < ref2DCopy.
features.cols(); ++i)
102 addFilter(
"RemoveNaNDataPointsFilter");
103 icp.readingDataPointsFilters.apply(ref2DCopy);
112 params[
"maxDist"] =
toParam(6.0);
116 icp.readingDataPointsFilters.clear();
117 addFilter(
"MaxDistDataPointsFilter", params);
118 validate2dTransformation();
119 validate3dTransformation();
123 icp.readingDataPointsFilters.clear();
124 addFilter(
"MaxDistDataPointsFilter", params);
125 validate2dTransformation();
126 validate3dTransformation();
130 icp.readingDataPointsFilters.clear();
131 addFilter(
"MaxDistDataPointsFilter", params);
133 validate3dTransformation();
136 params[
"dim"] =
"-1";
137 icp.readingDataPointsFilters.clear();
138 addFilter(
"MaxDistDataPointsFilter", params);
139 validate2dTransformation();
140 validate3dTransformation();
154 params[
"minDist"] =
toParam(0.05);
158 icp.readingDataPointsFilters.clear();
159 addFilter(
"MinDistDataPointsFilter", params);
160 validate2dTransformation();
161 validate3dTransformation();
165 icp.readingDataPointsFilters.clear();
166 addFilter(
"MinDistDataPointsFilter", params);
167 validate2dTransformation();
168 validate3dTransformation();
173 icp.readingDataPointsFilters.clear();
174 addFilter(
"MinDistDataPointsFilter", params);
176 validate3dTransformation();
179 params[
"dim"] =
"-1";
180 icp.readingDataPointsFilters.clear();
181 addFilter(
"MinDistDataPointsFilter", params);
182 validate2dTransformation();
183 validate3dTransformation();
190 string ratio =
"0.95";
193 params[
"ratio"] = ratio;
197 icp.readingDataPointsFilters.clear();
198 addFilter(
"MaxQuantileOnAxisDataPointsFilter", params);
199 validate2dTransformation();
200 validate3dTransformation();
204 icp.readingDataPointsFilters.clear();
205 addFilter(
"MaxQuantileOnAxisDataPointsFilter", params);
206 validate2dTransformation();
207 validate3dTransformation();
211 icp.readingDataPointsFilters.clear();
212 addFilter(
"MaxQuantileOnAxisDataPointsFilter", params);
214 validate3dTransformation();
224 params[
"epsilon"] =
"0.1";
225 params[
"keepNormals"] =
"1";
226 params[
"keepDensities"] =
"1";
227 params[
"keepEigenValues"] =
"1";
228 params[
"keepEigenVectors"] =
"1" ;
229 params[
"keepMatchedIds"] =
"1" ;
232 addFilter(
"SurfaceNormalDataPointsFilter", params);
233 validate2dTransformation();
234 validate3dTransformation();
245 vector<double> ratio = {100, 1000, 5000};
247 for(
unsigned i=0; i < ratio.size(); i++)
249 icp.readingDataPointsFilters.clear();
252 params[
"epsilon"] =
"0.1";
253 params[
"keepNormals"] =
"0";
254 params[
"keepDensities"] =
"1";
255 params[
"keepEigenValues"] =
"0";
256 params[
"keepEigenVectors"] =
"0" ;
257 params[
"keepMatchedIds"] =
"0" ;
259 addFilter(
"SurfaceNormalDataPointsFilter", params);
262 params[
"maxDensity"] =
toParam(ratio[i]);
264 addFilter(
"MaxDensityDataPointsFilter", params);
273 validate3dTransformation();
276 double nbRemainingPts = icp.getPrefilteredReadingPtsCount();
286 params[
"averageExistingDescriptors"] =
"1";
287 params[
"keepNormals"] =
"1";
288 params[
"keepDensities"] =
"1";
289 params[
"keepEigenValues"] =
"1";
290 params[
"keepEigenVectors"] =
"1";
292 addFilter(
"SamplingSurfaceNormalDataPointsFilter", params);
293 validate2dTransformation();
294 validate3dTransformation();
330 params[
"averageExistingDescriptors"] =
"1";
331 params[
"keepNormals"] =
"1";
332 params[
"keepEigenValues"] =
"1";
333 params[
"keepEigenVectors"] =
"1";
334 params[
"maxBoxDim"] =
"1";
335 params[
"keepMeans"] =
"1";
336 params[
"keepCovariances"] =
"1";
337 params[
"keepGestaltFeatures"] =
"1";
338 params[
"radius"] =
"1";
339 params[
"ratio"] =
"0.5";
341 addFilter(
"GestaltDataPointsFilter", params);
342 validate3dTransformation();
348 std::shared_ptr<PM::DataPointsFilter> extraDataPointFilter;
349 extraDataPointFilter =
PM::get().DataPointsFilterRegistrar.create(
350 "SurfaceNormalDataPointsFilter");
351 icp.readingDataPointsFilters.push_back(extraDataPointFilter);
352 addFilter(
"ObservationDirectionDataPointsFilter");
353 addFilter(
"OrientNormalsDataPointsFilter", {
354 {
"towardCenter",
toParam(
false)}
357 validate2dTransformation();
358 validate3dTransformation();
364 vector<double> prob = {0.80, 0.85, 0.90, 0.95};
365 for(
unsigned i = 0; i < prob.size(); i++)
369 params[
"prob"] =
toParam(prob[i]);
371 icp.readingDataPointsFilters.clear();
372 addFilter(
"RandomSamplingDataPointsFilter", params);
373 validate2dTransformation();
374 validate3dTransformation();
380 vector<unsigned> steps = {1, 2, 3};
381 for(
unsigned i=0; i<steps.size(); i++)
385 params[
"startStep"] =
toParam(steps[i]);
387 icp.readingDataPointsFilters.clear();
388 addFilter(
"FixStepSamplingDataPointsFilter", params);
389 validate2dTransformation();
390 validate3dTransformation();
398 const size_t maxCount = 1000;
401 params[
"seed"] =
"42";
402 params[
"maxCount"] =
toParam(maxCount);
404 std::shared_ptr<PM::DataPointsFilter> maxPtsFilter =
405 PM::get().DataPointsFilterRegistrar.create(
"MaxPointCountDataPointsFilter", params);
407 DP filteredCloud = maxPtsFilter->filter(cloud);
417 DP filteredCloud2 = maxPtsFilter->filter(cloud);
423 params[
"seed"] =
"1";
424 params[
"maxCount"] =
toParam(maxCount);
426 std::shared_ptr<PM::DataPointsFilter> maxPtsFilter2 =
427 PM::get().DataPointsFilterRegistrar.create(
"MaxPointCountDataPointsFilter", params);
429 DP filteredCloud3 = maxPtsFilter2->filter(cloud);
440 icp.readingDataPointsFilters.clear();
441 addFilter(
"MaxPointCountDataPointsFilter", params);
442 validate2dTransformation();
443 validate3dTransformation();
448 const unsigned int nbPts = 60000;
449 const DP cloud = generateRandomDataPoints(nbPts);
452 std::shared_ptr<PM::DataPointsFilter> octreeFilter;
454 for(
const int meth : {0,1,2,3})
455 for(
const size_t maxData : {1,5})
456 for(
const float maxSize : {0.,0.05})
459 params[
"maxPointByNode"] =
toParam(maxData);
460 params[
"maxSizeByNode"] =
toParam(maxSize);
461 params[
"samplingMethod"] =
toParam(meth);
462 params[
"buildParallel"] =
"1";
464 octreeFilter =
PM::get().DataPointsFilterRegistrar.create(
"OctreeGridDataPointsFilter", params);
466 const DP filteredCloud = octreeFilter->filter(cloud);
468 if(maxData==1 and maxSize==0.)
486 icp.readingDataPointsFilters.clear();
487 addFilter(
"OctreeGridDataPointsFilter", params);
488 validate2dTransformation();
489 validate3dTransformation();
495 const size_t nbPts = 60000;
496 DP cloud = generateRandomDataPoints(nbPts);
502 std::shared_ptr<PM::DataPointsFilter> nssFilter;
506 paramsNorm[
"knn"] =
"5";
507 paramsNorm[
"epsilon"] =
"0.1";
508 paramsNorm[
"keepNormals"] =
"1";
509 std::shared_ptr<PM::DataPointsFilter> normalFilter =
PM::get().DataPointsFilterRegistrar.create(
"SurfaceNormalDataPointsFilter", paramsNorm);
511 normalFilter->inPlaceFilter(cloud);
514 std::vector<size_t> samples = { 1500, 5000, nbPts, nbPts3D};
516 for(
const size_t nbSample : samples)
518 icp.readingDataPointsFilters.clear();
522 params[
"nbSample"] =
toParam(nbSample);
524 nssFilter =
PM::get().DataPointsFilterRegistrar.create(
"NormalSpaceDataPointsFilter", params);
526 addFilter(
"SurfaceNormalDataPointsFilter", paramsNorm);
527 addFilter(
"NormalSpaceDataPointsFilter", params);
529 const DP filteredCloud = nssFilter->filter(cloud);
543 if (nbSample == nbPts)
553 validate3dTransformation();
560 const size_t nbPts = 60000;
561 DP cloud = generateRandomDataPoints(nbPts);
566 std::shared_ptr<PM::DataPointsFilter> covsFilter;
570 paramsNorm[
"knn"] =
"5";
571 paramsNorm[
"epsilon"] =
"0.1";
572 paramsNorm[
"keepNormals"] =
"1";
573 std::shared_ptr<PM::DataPointsFilter> normalFilter =
PM::get().DataPointsFilterRegistrar.create(
"SurfaceNormalDataPointsFilter", paramsNorm);
575 normalFilter->inPlaceFilter(cloud);
578 std::vector<size_t> samples = {500, 1500, 5000, nbPts, nbPts3D};
579 for(
const size_t nbSample : samples)
581 icp.readingDataPointsFilters.clear();
584 params[
"nbSample"] =
toParam(nbSample);
586 covsFilter =
PM::get().DataPointsFilterRegistrar.create(
"CovarianceSamplingDataPointsFilter", params);
588 addFilter(
"SurfaceNormalDataPointsFilter", paramsNorm);
589 addFilter(
"CovarianceSamplingDataPointsFilter", params);
591 const DP filteredCloud = covsFilter->filter(cloud);
593 if (nbSample == nbPts3D)
597 else if (nbSample == nbPts)
607 validate3dTransformation();
615 DP cloud = generateRandomDataPoints();
618 params[
"vSizeX"] =
"0.5";
619 params[
"vSizeY"] =
"0.5";
620 params[
"vSizeZ"] =
"0.5";
621 params[
"useCentroid"] =
toParam(
true);
622 params[
"averageExistingDescriptors"] =
toParam(
true);
624 std::shared_ptr<PM::DataPointsFilter> voxelFilter =
625 PM::get().DataPointsFilterRegistrar.create(
"VoxelGridDataPointsFilter", params);
627 DP filteredCloud = voxelFilter->filter(cloud);
634 vector<bool> useCentroid = {
false,
true};
635 vector<bool> averageExistingDescriptors = {
false,
true};
636 for (
unsigned i = 0 ; i < useCentroid.size() ; i++)
638 for (
unsigned j = 0; j < averageExistingDescriptors.size(); j++)
641 params[
"vSizeX"] =
"0.02";
642 params[
"vSizeY"] =
"0.02";
643 params[
"vSizeZ"] =
"0.02";
644 params[
"useCentroid"] =
toParam(
true);
645 params[
"averageExistingDescriptors"] =
toParam(
true);
647 icp.readingDataPointsFilters.clear();
648 addFilter(
"VoxelGridDataPointsFilter", params);
649 validate2dTransformation();
653 for (
unsigned i = 0 ; i < useCentroid.size() ; i++)
655 for (
unsigned j = 0; j < averageExistingDescriptors.size(); j++)
658 params[
"vSizeX"] =
"1";
659 params[
"vSizeY"] =
"1";
660 params[
"vSizeZ"] =
"1";
661 params[
"useCentroid"] =
toParam(
true);
662 params[
"averageExistingDescriptors"] =
toParam(
true);
664 icp.readingDataPointsFilters.clear();
665 addFilter(
"VoxelGridDataPointsFilter", params);
666 validate3dTransformation();
674 vector<double> thresholds = {100, 1000, 5000};
678 icp.readingDataPointsFilters.clear();
681 params[
"epsilon"] =
"0.1";
682 params[
"keepNormals"] =
"0";
683 params[
"keepDensities"] =
"1";
684 params[
"keepEigenValues"] =
"0";
685 params[
"keepEigenVectors"] =
"0";
686 params[
"keepMatchedIds"] =
"0";
689 addFilter(
"SurfaceNormalDataPointsFilter", params);
690 icp.readingDataPointsFilters.apply(ref3Ddensities);
692 for(
unsigned i=0; i < thresholds.size(); i++)
699 for (
unsigned j=0; j < densities.cols(); ++j)
701 if (densities(0, j) <= thresholds[i])
705 if (densities(0, j) >= thresholds[i])
711 for(
bool useLargerThan(
true); useLargerThan; useLargerThan=
false)
713 DP ref3DCopy = ref3Ddensities;
715 icp.readingDataPointsFilters.clear();
717 params[
"descName"] =
toParam(
"densities");
718 params[
"useLargerThan"] =
toParam(useLargerThan);
719 params[
"threshold"] =
toParam(thresholds[i]);
722 addFilter(
"CutAtDescriptorThresholdDataPointsFilter", params);
723 icp.readingDataPointsFilters.apply(ref3DCopy);
741 params[
"removeInside"] =
"0";
745 icp.readingDataPointsFilters.clear();
746 addFilter(
"DistanceLimitDataPointsFilter", params);
747 validate2dTransformation();
748 validate3dTransformation();
752 icp.readingDataPointsFilters.clear();
753 addFilter(
"DistanceLimitDataPointsFilter", params);
754 validate2dTransformation();
755 validate3dTransformation();
759 icp.readingDataPointsFilters.clear();
760 addFilter(
"DistanceLimitDataPointsFilter", params);
762 validate3dTransformation();
765 params[
"dim"] =
"-1";
766 icp.readingDataPointsFilters.clear();
767 addFilter(
"DistanceLimitDataPointsFilter", params);
768 validate2dTransformation();
769 validate3dTransformation();
779 params[
"dist"] =
toParam(0.05);
780 params[
"removeInside"] =
"1";
784 icp.readingDataPointsFilters.clear();
785 addFilter(
"DistanceLimitDataPointsFilter", params);
786 validate2dTransformation();
787 validate3dTransformation();
791 icp.readingDataPointsFilters.clear();
792 addFilter(
"DistanceLimitDataPointsFilter", params);
793 validate2dTransformation();
794 validate3dTransformation();
799 icp.readingDataPointsFilters.clear();
800 addFilter(
"DistanceLimitDataPointsFilter", params);
802 validate3dTransformation();
805 params[
"dim"] =
"-1";
806 icp.readingDataPointsFilters.clear();
807 addFilter(
"DistanceLimitDataPointsFilter", params);
808 validate2dTransformation();
809 validate3dTransformation();
816 std::shared_ptr<PM::DataPointsFilter> df =
PM::get().DataPointsFilterRegistrar.create(
"MaxPointCountDataPointsFilter", params);
818 icp.referenceDataPointsFilters.push_back(df);
819 icp.readingDataPointsFilters.push_back(df);
824 const size_t nbPts = 6;
825 const double expectedErrors_LMS1xx[6] = {0., -0.0015156, -0.059276,
826 0., -0.002311, -0.163689};
827 const double expectedErrors_HDL32E[6] = {0., -0.002945, -0.075866,
828 0., -0.002998,-0.082777 };
832 0, 0, 0, 0, 0, 0).finished();
834 pointsLabels.push_back(
DP::Label(
"x", 1));
835 pointsLabels.push_back(
DP::Label(
"y", 1));
836 pointsLabels.push_back(
DP::Label(
"z", 1));
837 pointsLabels.push_back(
DP::Label(
"pad", 1));
840 -1, -1, -1, -5, -5, -5,
842 0, 0, 0, 0, 0, 0).finished();
845 -1, -1, -1, -5, -5, -5,
847 0, 0, 0, 0, 0, 0).finished();
849 descLabels.push_back(
DP::Label(
"incidenceAngles", 1));
850 descLabels.push_back(
DP::Label(
"observationDirections", 3));
854 timeLabels.push_back(
DP::Label(
"dummyTime", 2));
857 DP pointCloud =
DP(points, pointsLabels, desc, descLabels, randTimes, timeLabels);
861 parameters[
"sensorType"] =
toParam(0);
862 std::shared_ptr<PM::DataPointsFilter> removeSensorBiasFilter =
PM::get().DataPointsFilterRegistrar.create(
"RemoveSensorBiasDataPointsFilter", parameters);
863 DP resultCloud = removeSensorBiasFilter->filter(pointCloud);
866 for(std::size_t i = 0; i< nbPts; ++i)
868 const double error = pointCloud.
features.col(i).norm() - resultCloud.
features.col(i).norm();
869 EXPECT_NEAR(expectedErrors_LMS1xx[i], error, 1e-3);
872 parameters[
"sensorType"] =
toParam(1);
873 removeSensorBiasFilter =
PM::get().DataPointsFilterRegistrar.create(
"RemoveSensorBiasDataPointsFilter", parameters);
874 resultCloud = removeSensorBiasFilter->filter(pointCloud);
876 for(std::size_t i = 0; i< nbPts; ++i)
878 const double error = pointCloud.
features.col(i).norm() - resultCloud.
features.col(i).norm();
879 EXPECT_NEAR(expectedErrors_HDL32E[i], error, 1e-4);
884 pointCloud =
DP(points, pointsLabels, desc2, descLabels, randTimes, timeLabels);
886 parameters[
"sensorType"] =
toParam(0);
887 parameters[
"angleThreshold"] =
toParam(30.);
888 removeSensorBiasFilter =
PM::get().DataPointsFilterRegistrar.create(
"RemoveSensorBiasDataPointsFilter", parameters);
889 resultCloud = removeSensorBiasFilter->filter(pointCloud);
Subsampling. Filter points beyond a maximum distance measured on a specific axis. ...
Subsampling. Filter points based on distance measured on a specific axis.
unsigned getTimeDim() const
Return the total number of times.
IdentityDataPointsFilter, does nothing.
std::string toParam(const S &value)
Return the a string value using lexical_cast.
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Remove points having NaN as coordinate.
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Maximum number of points.
#define EXPECT_GE(val1, val2)
DP generateRandomDataPoints(int nbPoints=100)
#define EXPECT_ANY_THROW(statement)
Subsampling. Cut points with value of a given descriptor above or below a given threshold.
The name for a certain number of dim.
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Parametrizable::Parameters Parameters
alias
#define EXPECT_GT(val1, val2)
#define EXPECT_NEAR(val1, val2, abs_error)
Functions and classes that are not dependant on scalar type are defined in this namespace.
void addFilter(string name, PM::Parameters params)
Sampling surface normals. First decimate the space until there is at most knn points, then find the center of mass and use the points to estimate nromal using eigen-decomposition.
static const PointMatcher & get()
Return instances.
void addFilter(string name)
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
A dense signed 64-bits matrix.
Data Filter based on Octree representation.
#define EXPECT_EQ(expected, actual)
Gestalt descriptors filter as described in Bosse & Zlot ICRA 2013.
TEST_F(DataFilterTest, IdentityDataPointsFilter)
Surface normals estimation. Find the normal for every point using eigen-decomposition of neighbour po...
#define EXPECT_TRUE(condition)
Subsampling. Filter points beyond a maximum quantile measured on a specific axis. ...
Subsampling. Filter points before a minimum distance measured on a specific axis. ...
Subsampling. Reduce the points number by randomly removing points with a dentsity higher than a tresh...
Matrix features
features of points in the cloud
unsigned getDescriptorDim() const
Return the total number of descriptors.
#define EXPECT_FALSE(condition)
Systematic sampling, with variation over time.
Reorientation of normals.