DataFilters.cpp
Go to the documentation of this file.
1 #include "../utest.h"
2 #include <ciso646>
3 #include <cmath>
4 
5 using namespace std;
6 using namespace PointMatcherSupport;
7 
8 //---------------------------
9 // DataFilter modules
10 //---------------------------
11 
12 // Utility classes
13 class DataFilterTest: public IcpHelper
14 {
15 public:
16  // Will be called for every tests
17  virtual void SetUp()
18  {
19  icp.setDefault();
20  // Uncomment for console outputs
21  //setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
22 
23  // We'll test the filters on reading point cloud
24  icp.readingDataPointsFilters.clear();
25  }
26 
27  // Will be called for every tests
28  virtual void TearDown() {}
29 
30  void addFilter(string name, PM::Parameters params)
31  {
32  std::shared_ptr<PM::DataPointsFilter> testedDataPointFilter =
33  PM::get().DataPointsFilterRegistrar.create(name, params);
34 
35  icp.readingDataPointsFilters.push_back(testedDataPointFilter);
36  }
37 
38  void addFilter(string name)
39  {
40  std::shared_ptr<PM::DataPointsFilter> testedDataPointFilter =
41  PM::get().DataPointsFilterRegistrar.create(name);
42 
43  icp.readingDataPointsFilters.push_back(testedDataPointFilter);
44  }
45 
46  DP generateRandomDataPoints(int nbPoints = 100)
47  {
48  const int dimFeatures = 4;
49  const int dimDescriptors = 3;
50  const int dimTime = 2;
51 
52  PM::Matrix randFeat = PM::Matrix::Random(dimFeatures, nbPoints);
53  DP::Labels featLabels;
54  featLabels.push_back(DP::Label("x", 1));
55  featLabels.push_back(DP::Label("y", 1));
56  featLabels.push_back(DP::Label("z", 1));
57  featLabels.push_back(DP::Label("pad", 1));
58 
59  PM::Matrix randDesc = PM::Matrix::Random(dimDescriptors, nbPoints);
60  DP::Labels descLabels;
61  descLabels.push_back(DP::Label("dummyDesc", 3));
62 
63  PM::Int64Matrix randTimes = PM::Int64Matrix::Random(dimTime, nbPoints);
64  DP::Labels timeLabels;
65  timeLabels.push_back(DP::Label("dummyTime", 2));
66 
67  // Construct the point cloud from the generated matrices
68  DP pointCloud = DP(randFeat, featLabels, randDesc, descLabels, randTimes, timeLabels);
69 
70  return pointCloud;
71  }
72 };
73 
74 TEST_F(DataFilterTest, IdentityDataPointsFilter)
75 {
76  // build test cloud
77  DP ref2DCopy(ref2D);
78 
79  // apply and checked
80  addFilter("IdentityDataPointsFilter");
81  icp.readingDataPointsFilters.apply(ref2DCopy);
82  EXPECT_TRUE(ref2D == ref2DCopy);
83 }
84 
85 TEST_F(DataFilterTest, RemoveNaNDataPointsFilter)
86 {
87  // build test cloud
88  DP ref2DCopy(ref2D);
89  int goodCount(0);
90  const NumericType nan(std::numeric_limits<NumericType>::quiet_NaN());
91  for (int i(0); i < ref2DCopy.features.cols(); ++i)
92  {
93  if (rand() % 3 == 0)
94  {
95  ref2DCopy.features(rand() % ref2DCopy.features.rows(), i) = nan;
96  }
97  else
98  ++goodCount;
99  }
100 
101  // apply and checked
102  addFilter("RemoveNaNDataPointsFilter");
103  icp.readingDataPointsFilters.apply(ref2DCopy);
104  EXPECT_TRUE(ref2DCopy.features.cols() == goodCount);
105 }
106 
107 TEST_F(DataFilterTest, MaxDistDataPointsFilter)
108 {
109  // Max dist has been selected to not affect the points
110  params = PM::Parameters();
111  params["dim"] = "0";
112  params["maxDist"] = toParam(6.0);
113 
114  // Filter on x axis
115  params["dim"] = "0";
116  icp.readingDataPointsFilters.clear();
117  addFilter("MaxDistDataPointsFilter", params);
118  validate2dTransformation();
119  validate3dTransformation();
120 
121  // Filter on y axis
122  params["dim"] = "1";
123  icp.readingDataPointsFilters.clear();
124  addFilter("MaxDistDataPointsFilter", params);
125  validate2dTransformation();
126  validate3dTransformation();
127 
128  // Filter on z axis (not existing)
129  params["dim"] = "2";
130  icp.readingDataPointsFilters.clear();
131  addFilter("MaxDistDataPointsFilter", params);
132  EXPECT_ANY_THROW(validate2dTransformation());
133  validate3dTransformation();
134 
135  // Filter on a radius
136  params["dim"] = "-1";
137  icp.readingDataPointsFilters.clear();
138  addFilter("MaxDistDataPointsFilter", params);
139  validate2dTransformation();
140  validate3dTransformation();
141 
142  // Parameter outside valid range
143  params["dim"] = "3";
144  //TODO: specify the exception, move that to GenericTest
145  EXPECT_ANY_THROW(addFilter("MaxDistDataPointsFilter", params));
146 
147 }
148 
149 TEST_F(DataFilterTest, MinDistDataPointsFilter)
150 {
151  // Min dist has been selected to not affect the points too much
152  params = PM::Parameters();
153  params["dim"] = "0";
154  params["minDist"] = toParam(0.05);
155 
156  // Filter on x axis
157  params["dim"] = "0";
158  icp.readingDataPointsFilters.clear();
159  addFilter("MinDistDataPointsFilter", params);
160  validate2dTransformation();
161  validate3dTransformation();
162 
163  // Filter on y axis
164  params["dim"] = "1";
165  icp.readingDataPointsFilters.clear();
166  addFilter("MinDistDataPointsFilter", params);
167  validate2dTransformation();
168  validate3dTransformation();
169 
170  //TODO: move that to specific 2D test
171  // Filter on z axis (not existing)
172  params["dim"] = "2";
173  icp.readingDataPointsFilters.clear();
174  addFilter("MinDistDataPointsFilter", params);
175  EXPECT_ANY_THROW(validate2dTransformation());
176  validate3dTransformation();
177 
178  // Filter on a radius
179  params["dim"] = "-1";
180  icp.readingDataPointsFilters.clear();
181  addFilter("MinDistDataPointsFilter", params);
182  validate2dTransformation();
183  validate3dTransformation();
184 
185 }
186 
187 TEST_F(DataFilterTest, MaxQuantileOnAxisDataPointsFilter)
188 {
189  // Ratio has been selected to not affect the points too much
190  string ratio = "0.95";
191  params = PM::Parameters();
192  params["dim"] = "0";
193  params["ratio"] = ratio;
194 
195  // Filter on x axis
196  params["dim"] = "0";
197  icp.readingDataPointsFilters.clear();
198  addFilter("MaxQuantileOnAxisDataPointsFilter", params);
199  validate2dTransformation();
200  validate3dTransformation();
201 
202  // Filter on y axis
203  params["dim"] = "1";
204  icp.readingDataPointsFilters.clear();
205  addFilter("MaxQuantileOnAxisDataPointsFilter", params);
206  validate2dTransformation();
207  validate3dTransformation();
208 
209  // Filter on z axis (not existing)
210  params["dim"] = "2";
211  icp.readingDataPointsFilters.clear();
212  addFilter("MaxQuantileOnAxisDataPointsFilter", params);
213  EXPECT_ANY_THROW(validate2dTransformation());
214  validate3dTransformation();
215 }
216 
217 
218 
219 TEST_F(DataFilterTest, SurfaceNormalDataPointsFilter)
220 {
221  // This filter create descriptor, so parameters should'nt impact results
222  params = PM::Parameters();
223  params["knn"] = "5";
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" ;
230  // FIXME: the parameter keepMatchedIds seems to do nothing...
231 
232  addFilter("SurfaceNormalDataPointsFilter", params);
233  validate2dTransformation();
234  validate3dTransformation();
235 
236  // TODO: standardize how filter are tested:
237  // 1- impact on number of points
238  // 2- impact on descriptors
239  // 3- impact on ICP (that's what we test now)
240 }
241 
242 TEST_F(DataFilterTest, MaxDensityDataPointsFilter)
243 {
244  // Ratio has been selected to not affect the points too much
245  vector<double> ratio = {100, 1000, 5000};
246 
247  for(unsigned i=0; i < ratio.size(); i++)
248  {
249  icp.readingDataPointsFilters.clear();
250  params = PM::Parameters();
251  params["knn"] = "5";
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" ;
258 
259  addFilter("SurfaceNormalDataPointsFilter", params);
260 
261  params = PM::Parameters();
262  params["maxDensity"] = toParam(ratio[i]);
263 
264  addFilter("MaxDensityDataPointsFilter", params);
265 
266  // FIXME BUG: the density in 2D is not well computed
267  //validate2dTransformation();
268 
269  //double nbInitPts = data2D.features.cols();
270  //double nbRemainingPts = icp.getPrefilteredReadingPtsCount();
271  //EXPECT_TRUE(nbRemainingPts < nbInitPts);
272 
273  validate3dTransformation();
274 
275  double nbInitPts = data3D.features.cols();
276  double nbRemainingPts = icp.getPrefilteredReadingPtsCount();
277  EXPECT_TRUE(nbRemainingPts < nbInitPts);
278  }
279 }
280 
281 TEST_F(DataFilterTest, SamplingSurfaceNormalDataPointsFilter)
282 {
283  // This filter create descriptor AND subsample
284  params = PM::Parameters();
285  params["knn"] = "5";
286  params["averageExistingDescriptors"] = "1";
287  params["keepNormals"] = "1";
288  params["keepDensities"] = "1";
289  params["keepEigenValues"] = "1";
290  params["keepEigenVectors"] = "1";
291 
292  addFilter("SamplingSurfaceNormalDataPointsFilter", params);
293  validate2dTransformation();
294  validate3dTransformation();
295 
296 }
297 
298 //TODO: this filter is broken, fix it!
299 /*
300 TEST_F(DataFilterTest, ElipsoidsDataPointsFilter)
301 {
302  // This filter creates descriptor AND subsamples
303  params = PM::Parameters();
304  params["knn"] = "5";
305  params["averageExistingDescriptors"] = "1";
306  params["keepNormals"] = "1";
307  params["keepDensities"] = "1";
308  params["keepEigenValues"] = "1";
309  params["keepEigenVectors"] = "1";
310  params["maxBoxDim"] = "inf";
311  params["maxTimeWindow"] = "10";
312  params["minPlanarity"] = "0";
313  params["keepMeans"] = "1";
314  params["keepCovariances"] = "1";
315  params["keepWeights"] = "1";
316  params["keepShapes"] = "1";
317  params["keepIndices"] = "1";
318 
319  addFilter("ElipsoidsDataPointsFilter", params);
320  validate2dTransformation();
321  validate3dTransformation();
322 }
323 */
324 
325 TEST_F(DataFilterTest, GestaltDataPointsFilter)
326 {
327  // This filter creates descriptor AND subsamples
328  params = PM::Parameters();
329  params["knn"] = "5";
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";
340 
341  addFilter("GestaltDataPointsFilter", params);
342  validate3dTransformation();
343 }
344 
345 TEST_F(DataFilterTest, OrientNormalsDataPointsFilter)
346 {
347  // Used to create normal for reading point cloud
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)}
355  }
356  );
357  validate2dTransformation();
358  validate3dTransformation();
359 }
360 
361 
362 TEST_F(DataFilterTest, RandomSamplingDataPointsFilter)
363 {
364  for(const double prob : {0.8, 0.85, 0.9, 0.95})
365  {
366  for(const unsigned int samplingMethod : {0, 1})
367  {
368  // Try to avoid too low value for the reduction to avoid under sampling
369  params = PM::Parameters();
370  params["prob"] = toParam(prob);
371  params["randomSamplingMethod"] = toParam(samplingMethod);
372 
373  icp.readingDataPointsFilters.clear();
374  addFilter("RandomSamplingDataPointsFilter", params);
375  validate2dTransformation();
376  validate3dTransformation();
377  }
378  }
379 }
380 
381 TEST_F(DataFilterTest, FixStepSamplingDataPointsFilter)
382 {
383  vector<unsigned> steps = {1, 2, 3};
384  for(unsigned i=0; i<steps.size(); i++)
385  {
386  // Try to avoid too low value for the reduction to avoid under sampling
387  params = PM::Parameters();
388  params["startStep"] = toParam(steps[i]);
389 
390  icp.readingDataPointsFilters.clear();
391  addFilter("FixStepSamplingDataPointsFilter", params);
392  validate2dTransformation();
393  validate3dTransformation();
394  }
395 }
396 
397 TEST_F(DataFilterTest, MaxPointCountDataPointsFilter)
398 {
399  DP cloud = ref3D;
400 
401  const size_t maxCount = 1000;
402 
403  params = PM::Parameters();
404  params["seed"] = "42";
405  params["maxCount"] = toParam(maxCount);
406 
407  std::shared_ptr<PM::DataPointsFilter> maxPtsFilter =
408  PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
409 
410  DP filteredCloud = maxPtsFilter->filter(cloud);
411 
412  //Check number of points
413  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
414  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
415  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
416 
417  EXPECT_EQ(filteredCloud.getNbPoints(), maxCount);
418 
419  //Same seed should result same filtered cloud
420  DP filteredCloud2 = maxPtsFilter->filter(cloud);
421 
422  EXPECT_TRUE(filteredCloud == filteredCloud2);
423 
424  //Different seeds should not result same filtered cloud but same number
425  params.clear();
426  params["seed"] = "1";
427  params["maxCount"] = toParam(maxCount);
428 
429  std::shared_ptr<PM::DataPointsFilter> maxPtsFilter2 =
430  PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
431 
432  DP filteredCloud3 = maxPtsFilter2->filter(cloud);
433 
434  EXPECT_FALSE(filteredCloud3 == filteredCloud2);
435 
436  EXPECT_EQ(filteredCloud3.getNbPoints(), maxCount);
437 
438  EXPECT_EQ(filteredCloud3.getNbPoints(), filteredCloud2.getNbPoints());
439  EXPECT_EQ(filteredCloud3.getDescriptorDim(), filteredCloud2.getDescriptorDim());
440  EXPECT_EQ(filteredCloud3.getTimeDim(), filteredCloud2.getTimeDim());
441 
442  //Validate transformation
443  icp.readingDataPointsFilters.clear();
444  addFilter("MaxPointCountDataPointsFilter", params);
445  validate2dTransformation();
446  validate3dTransformation();
447 }
448 
449 TEST_F(DataFilterTest, OctreeGridDataPointsFilter)
450 {
451  const unsigned int nbPts = 60000;
452  const DP cloud = generateRandomDataPoints(nbPts);
453  params = PM::Parameters();
454 
455  std::shared_ptr<PM::DataPointsFilter> octreeFilter;
456 
457  for(const int meth : {0,1,2,3})
458  for(const size_t maxData : {1,5})
459  for(const NumericType maxSize : {0.,0.05})
460  {
461  params.clear();
462  params["maxPointByNode"] = toParam(maxData);
463  params["maxSizeByNode"] = toParam(maxSize);
464  params["samplingMethod"] = toParam(meth);
465  params["buildParallel"] = "1";
466 
467  octreeFilter = PM::get().DataPointsFilterRegistrar.create("OctreeGridDataPointsFilter", params);
468 
469  const DP filteredCloud = octreeFilter->filter(cloud);
470 
471  if(maxData==1 and maxSize==0.)
472  {
473  // 1/pts by octants + validate parallel build
474  // the number of point should not change
475  // the sampling methods should not change anything
476  //Check number of points
477  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
478  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
479  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
480 
481  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
482  }
483  else
484  {
485  //Check number of points
486  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
487  }
488  //Validate transformation
489  icp.readingDataPointsFilters.clear();
490  addFilter("OctreeGridDataPointsFilter", params);
491  validate2dTransformation();
492  validate3dTransformation();
493  }
494 }
495 
496 TEST_F(DataFilterTest, NormalSpaceDataPointsFilter)
497 {
498  const size_t nbPts = 60000;
499  DP cloud = generateRandomDataPoints(nbPts);
500  params = PM::Parameters();
501 
502  //const size_t nbPts2D = ref2D.getNbPoints();
503  const size_t nbPts3D = ref3D.getNbPoints();
504 
505  std::shared_ptr<PM::DataPointsFilter> nssFilter;
506 
507  //Compute normals
508  auto paramsNorm = PM::Parameters();
509  paramsNorm["knn"] = "5";
510  paramsNorm["epsilon"] = "0.1";
511  paramsNorm["keepNormals"] = "1";
512  std::shared_ptr<PM::DataPointsFilter> normalFilter = PM::get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", paramsNorm);
513 
514  normalFilter->inPlaceFilter(cloud);
515 
516  //Evaluate filter
517  std::vector<size_t> samples = {/* 2*nbPts2D/3, nbPts2D,*/ 1500, 5000, nbPts, nbPts3D};
518  for(const NumericType epsilon : {M_PI/6., M_PI/32., M_PI/64.})
519  for(const size_t nbSample : samples)
520  {
521  icp.readingDataPointsFilters.clear();
522 
523  params.clear();
524  params["epsilon"] = toParam(epsilon);
525  params["nbSample"] = toParam(nbSample);
526 
527  nssFilter = PM::get().DataPointsFilterRegistrar.create("NormalSpaceDataPointsFilter", params);
528 
529  addFilter("SurfaceNormalDataPointsFilter", paramsNorm);
530  addFilter("NormalSpaceDataPointsFilter", params);
531 
532  const DP filteredCloud = nssFilter->filter(cloud);
533 
534  /*
535  if(nbSample <= nbPts2D)
536  {
537  validate2dTransformation();
538  EXPECT_LE(filteredCloud.getNbPoints(), nbPts2D);
539  continue;
540  }
541  else if (nbSample == nbPts3D)
542  {
543  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts3D);
544  }
545  else */
546  if (nbSample == nbPts)
547  {
548  //Check number of points
549  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
550  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
551  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
552 
553  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
554  }
555 
556  validate3dTransformation();
557  EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
558  }
559 }
560 
561 TEST_F(DataFilterTest, CovarianceSamplingDataPointsFilter)
562 {
563  const size_t nbPts = 60000;
564  DP cloud = generateRandomDataPoints(nbPts);
565  params = PM::Parameters();
566 
567  const size_t nbPts3D = ref3D.getNbPoints();
568 
569  std::shared_ptr<PM::DataPointsFilter> covsFilter;
570 
571  //Compute normals
572  auto paramsNorm = PM::Parameters();
573  paramsNorm["knn"] = "5";
574  paramsNorm["epsilon"] = "0.1";
575  paramsNorm["keepNormals"] = "1";
576  std::shared_ptr<PM::DataPointsFilter> normalFilter = PM::get().DataPointsFilterRegistrar.create("SurfaceNormalDataPointsFilter", paramsNorm);
577 
578  normalFilter->inPlaceFilter(cloud);
579 
580  //Evaluate filter
581  std::vector<size_t> samples = {500, 1500, 5000, nbPts, nbPts3D};
582  for(const size_t nbSample : samples)
583  {
584  icp.readingDataPointsFilters.clear();
585 
586  params.clear();
587  params["nbSample"] = toParam(nbSample);
588 
589  covsFilter = PM::get().DataPointsFilterRegistrar.create("CovarianceSamplingDataPointsFilter", params);
590 
591  addFilter("SurfaceNormalDataPointsFilter", paramsNorm);
592  addFilter("CovarianceSamplingDataPointsFilter", params);
593 
594  const DP filteredCloud = covsFilter->filter(cloud);
595 
596  if (nbSample == nbPts3D)
597  {
598  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts3D);
599  }
600  else if (nbSample == nbPts)
601  {
602  //Check number of points
603  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
604  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
605  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
606 
607  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
608  }
609 
610  validate3dTransformation();
611  EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
612  }
613 }
614 
615 TEST_F(DataFilterTest, VoxelGridDataPointsFilter)
616 {
617  // Test with point cloud
618  DP cloud = generateRandomDataPoints();
619 
620  params = PM::Parameters();
621  params["vSizeX"] = "0.5";
622  params["vSizeY"] = "0.5";
623  params["vSizeZ"] = "0.5";
624  params["useCentroid"] = toParam(true);
625  params["averageExistingDescriptors"] = toParam(true);
626 
627  std::shared_ptr<PM::DataPointsFilter> voxelFilter =
628  PM::get().DataPointsFilterRegistrar.create("VoxelGridDataPointsFilter", params);
629 
630  DP filteredCloud = voxelFilter->filter(cloud);
631 
632  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
633  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
634  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
635 
636  // Test with ICP
637  vector<bool> useCentroid = {false, true};
638  vector<bool> averageExistingDescriptors = {false, true};
639  for (unsigned i = 0 ; i < useCentroid.size() ; i++)
640  {
641  for (unsigned j = 0; j < averageExistingDescriptors.size(); j++)
642  {
643  params = PM::Parameters();
644  params["vSizeX"] = "0.02";
645  params["vSizeY"] = "0.02";
646  params["vSizeZ"] = "0.02";
647  params["useCentroid"] = toParam(true);
648  params["averageExistingDescriptors"] = toParam(true);
649 
650  icp.readingDataPointsFilters.clear();
651  addFilter("VoxelGridDataPointsFilter", params);
652  validate2dTransformation();
653  }
654  }
655 
656  for (unsigned i = 0 ; i < useCentroid.size() ; i++)
657  {
658  for (unsigned j = 0; j < averageExistingDescriptors.size(); j++)
659  {
660  params = PM::Parameters();
661  params["vSizeX"] = "1";
662  params["vSizeY"] = "1";
663  params["vSizeZ"] = "1";
664  params["useCentroid"] = toParam(true);
665  params["averageExistingDescriptors"] = toParam(true);
666 
667  icp.readingDataPointsFilters.clear();
668  addFilter("VoxelGridDataPointsFilter", params);
669  validate3dTransformation();
670  }
671  }
672 }
673 
674 TEST_F(DataFilterTest, CutAtDescriptorThresholdDataPointsFilter)
675 {
676  // Copied from density ratio above
677  vector<double> thresholds = {100, 1000, 5000};
678 
679  DP ref3Ddensities = ref3D;
680  // Adding descriptor "densities"
681  icp.readingDataPointsFilters.clear();
682  params = PM::Parameters();
683  params["knn"] = "5";
684  params["epsilon"] = "0.1";
685  params["keepNormals"] = "0";
686  params["keepDensities"] = "1";
687  params["keepEigenValues"] = "0";
688  params["keepEigenVectors"] = "0";
689  params["keepMatchedIds"] = "0";
690 
691 
692  addFilter("SurfaceNormalDataPointsFilter", params);
693  icp.readingDataPointsFilters.apply(ref3Ddensities);
694 
695  for(unsigned i=0; i < thresholds.size(); i++)
696  {
697  int belowCount=0;
698  int aboveCount=0;
699 
700  // counting points above and below
701  PM::DataPoints::View densities = ref3Ddensities.getDescriptorViewByName("densities");
702  for (unsigned j=0; j < densities.cols(); ++j)
703  {
704  if (densities(0, j) <= thresholds[i])
705  {
706  ++belowCount;
707  }
708  if (densities(0, j) >= thresholds[i])
709  {
710  ++aboveCount;
711  }
712  }
713 
714  for(bool useLargerThan(true); useLargerThan; useLargerThan=false)
715  {
716  DP ref3DCopy = ref3Ddensities;
717 
718  icp.readingDataPointsFilters.clear();
719  params = PM::Parameters();
720  params["descName"] = toParam("densities");
721  params["useLargerThan"] = toParam(useLargerThan);
722  params["threshold"] = toParam(thresholds[i]);
723 
724 
725  addFilter("CutAtDescriptorThresholdDataPointsFilter", params);
726  icp.readingDataPointsFilters.apply(ref3DCopy);
727  if (useLargerThan)
728  {
729  EXPECT_TRUE(ref3DCopy.features.cols() == belowCount);
730  }
731  else
732  {
733  EXPECT_TRUE(ref3DCopy.features.cols() == aboveCount);
734  }
735  }
736  }
737 }
738 
739 TEST_F(DataFilterTest, DistanceLimitDataPointsFilter)
740 {
741  params = PM::Parameters();
742  params["dim"] = "0";
743  params["dist"] = toParam(6.0);
744  params["removeInside"] = "0";
745 
746  // Filter on x axis
747  params["dim"] = "0";
748  icp.readingDataPointsFilters.clear();
749  addFilter("DistanceLimitDataPointsFilter", params);
750  validate2dTransformation();
751  validate3dTransformation();
752 
753  // Filter on y axis
754  params["dim"] = "1";
755  icp.readingDataPointsFilters.clear();
756  addFilter("DistanceLimitDataPointsFilter", params);
757  validate2dTransformation();
758  validate3dTransformation();
759 
760  // Filter on z axis (not existing)
761  params["dim"] = "2";
762  icp.readingDataPointsFilters.clear();
763  addFilter("DistanceLimitDataPointsFilter", params);
764  EXPECT_ANY_THROW(validate2dTransformation());
765  validate3dTransformation();
766 
767  // Filter on a radius
768  params["dim"] = "-1";
769  icp.readingDataPointsFilters.clear();
770  addFilter("DistanceLimitDataPointsFilter", params);
771  validate2dTransformation();
772  validate3dTransformation();
773 
774  // Parameter outside valid range
775  params["dim"] = "3";
776  //TODO: specify the exception, move that to GenericTest
777  EXPECT_ANY_THROW(addFilter("DistanceLimitDataPointsFilter", params));
778 
779 
780  params = PM::Parameters();
781  params["dim"] = "0";
782  params["dist"] = toParam(0.05);
783  params["removeInside"] = "1";
784 
785  // Filter on x axis
786  params["dim"] = "0";
787  icp.readingDataPointsFilters.clear();
788  addFilter("DistanceLimitDataPointsFilter", params);
789  validate2dTransformation();
790  validate3dTransformation();
791 
792  // Filter on y axis
793  params["dim"] = "1";
794  icp.readingDataPointsFilters.clear();
795  addFilter("DistanceLimitDataPointsFilter", params);
796  validate2dTransformation();
797  validate3dTransformation();
798 
799  //TODO: move that to specific 2D test
800  // Filter on z axis (not existing)
801  params["dim"] = "2";
802  icp.readingDataPointsFilters.clear();
803  addFilter("DistanceLimitDataPointsFilter", params);
804  EXPECT_ANY_THROW(validate2dTransformation());
805  validate3dTransformation();
806 
807  // Filter on a radius
808  params["dim"] = "-1";
809  icp.readingDataPointsFilters.clear();
810  addFilter("DistanceLimitDataPointsFilter", params);
811  validate2dTransformation();
812  validate3dTransformation();
813 }
814 
815 TEST_F(DataFilterTest, SameFilterInstanceTwice)
816 {
817  params = PM::Parameters();
818 
819  std::shared_ptr<PM::DataPointsFilter> df = PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
820 
821  icp.referenceDataPointsFilters.push_back(df);
822  icp.readingDataPointsFilters.push_back(df);
823 }
824 
825 TEST_F(DataFilterTest, RemoveSensorBiasDataPointsFilter)
826 {
827  const size_t nbPts = 6;
828  const double expectedErrors_LMS1xx[6] = {0., -0.0015156, -0.059276,
829  0., -0.002311, -0.163689};
830  const double expectedErrors_HDL32E[6] = {0., -0.002945, -0.075866,
831  0., -0.002998,-0.082777 };
832 
833  PM::Matrix points = (PM::Matrix(3,6) << 1, 1, 1, 5, 5, 5,
834  0, 0, 0, 0, 0, 0,
835  0, 0, 0, 0, 0, 0).finished();
836  DP::Labels pointsLabels;
837  pointsLabels.push_back(DP::Label("x", 1));
838  pointsLabels.push_back(DP::Label("y", 1));
839  pointsLabels.push_back(DP::Label("z", 1));
840  pointsLabels.push_back(DP::Label("pad", 1));
841 
842  PM::Matrix desc = (PM::Matrix(4,6) << 0., 0.7854, 1.4835, 0., 0.7854, 1.4835, //0,45,85 degrees
843  -1, -1, -1, -5, -5, -5,
844  0, 0, 0, 0, 0, 0, // observation : point to sensor
845  0, 0, 0, 0, 0, 0).finished();
846 
847  PM::Matrix desc2 = (PM::Matrix(4,6) << 0., 0.7854, std::nanf(""), 0., 0.7854, M_PI_2, //0,45,90 degrees
848  -1, -1, -1, -5, -5, -5,
849  0, 0, 0, 0, 0, 0, // observation : point to sensor
850  0, 0, 0, 0, 0, 0).finished();
851  DP::Labels descLabels;
852  descLabels.push_back(DP::Label("incidenceAngles", 1));
853  descLabels.push_back(DP::Label("observationDirections", 3));
854 
855  PM::Int64Matrix randTimes = PM::Int64Matrix::Random(2, nbPts);
856  DP::Labels timeLabels;
857  timeLabels.push_back(DP::Label("dummyTime", 2));
858 
859  // Construct the point cloud from the generated matrices
860  DP pointCloud = DP(points, pointsLabels, desc, descLabels, randTimes, timeLabels);
861 
862 
863  PM::Parameters parameters;
864  parameters["sensorType"] = toParam(0); //LMS_1xx
865  std::shared_ptr<PM::DataPointsFilter> removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
866  DP resultCloud = removeSensorBiasFilter->filter(pointCloud);
867  EXPECT_EQ(pointCloud.getNbPoints(), resultCloud.getNbPoints());
868 
869  for(std::size_t i = 0; i< nbPts; ++i)
870  {
871  const double error = pointCloud.features.col(i).norm() - resultCloud.features.col(i).norm();
872  EXPECT_NEAR(expectedErrors_LMS1xx[i], error, 1e-3); // below mm
873  }
874 
875  parameters["sensorType"] = toParam(1); //HDL32E
876  removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
877  resultCloud = removeSensorBiasFilter->filter(pointCloud);
878 
879  for(std::size_t i = 0; i< nbPts; ++i)
880  {
881  const double error = pointCloud.features.col(i).norm() - resultCloud.features.col(i).norm();
882  EXPECT_NEAR(expectedErrors_HDL32E[i], error, 1e-4); // below mm
883  }
884 
885 
886  //test points rejection
887  pointCloud = DP(points, pointsLabels, desc2, descLabels, randTimes, timeLabels);
888 
889  parameters["sensorType"] = toParam(0); //LMS_1xx
890  parameters["angleThreshold"] = toParam(30.);
891  removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
892  resultCloud = removeSensorBiasFilter->filter(pointCloud);
893 
894  //four points should have been rejected
895  EXPECT_EQ(pointCloud.getNbPoints()-4, resultCloud.getNbPoints());
896 }
897 
898 TEST_F(DataFilterTest, SaliencyDataPointsFilter)
899 {
900  // This filter creates descriptors
901  params = PM::Parameters();
902  params["k"] = "50";
903  params["sigma"] = "1.0";
904  params["keepNormals"] = "1";
905  params["keepLabels"] = "1";
906  params["keepTensors"] = "1";
907 
908  addFilter("SaliencyDataPointsFilter", params);
909  validate3dTransformation();
910 }
911 
912 TEST_F(DataFilterTest, SpectralDecompositionDataPointsFilter)
913 {
914  using DPFiltersPtr = std::shared_ptr<PM::DataPointsFilter>;
915 
916  // Test with point cloud
917  DP cloud = generateRandomDataPoints(300000);
918 
919  // This filter creates descriptors
920  params = PM::Parameters();
921  params["k"] = "50";
922  params["sigma"] = "0.1";
923  params["radius"] = "0.4";
924  params["itMax"] = "15";
925  params["keepNormals"] = "1";
926  params["keepLabels"] = "1";
927  params["keepLambdas"] = "1";
928  params["keepTensors"] = "1";
929 
930  DPFiltersPtr spdf = PM::get().DataPointsFilterRegistrar.create(
931  "SpectralDecompositionDataPointsFilter", params
932  );
933 
934  DP filteredCloud = spdf->filter(cloud);
935 
936  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
937  EXPECT_EQ(cloud.getDescriptorDim()+(3+3+1+3+1+4+7+3), filteredCloud.getDescriptorDim());
938  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
939 
940  params = PM::Parameters();
941  params["k"] = "50";
942  params["sigma"] = "1.";
943  params["radius"] = "2.";
944  params["itMax"] = "15";
945  params["keepNormals"] = "1";
946  params["keepLabels"] = "1";
947  params["keepLambdas"] = "1";
948  params["keepTensors"] = "1";
949 
950  addFilter("SpectralDecompositionDataPointsFilter", params);
951  validate3dTransformation();
952 }
953 
954 TEST_F(DataFilterTest, AddDescriptorDataPointsFilter)
955 {
956  using DPFiltersPtr = std::shared_ptr<PM::DataPointsFilter>;
957 
958  // Test with point cloud
959  DP cloud = generateRandomDataPoints(100);
960 
961  std::string descriptorName = "test_descriptor";
962  std::size_t descriptorDimension = 3;
963  std::vector<NumericType> descriptorValues{2, 3, 4};
964 
965  // This filter adds a new descriptor
966  params = PM::Parameters();
967  params["descriptorName"] = descriptorName;
968  params["descriptorDimension"] = toParam(descriptorDimension);
969  params["descriptorValues"] = toParam(descriptorValues);
970 
971  DPFiltersPtr addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create(
972  "AddDescriptorDataPointsFilter", params
973  );
974 
975  DP filteredCloud = addDescriptorFilter->filter(cloud);
976 
977  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
978  EXPECT_EQ(cloud.getDescriptorDim()+descriptorDimension, filteredCloud.getDescriptorDim());
979  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
980 
981  Eigen::Matrix<NumericType, 1, Eigen::Dynamic> row = Eigen::Matrix<NumericType, 1, Eigen::Dynamic>::Ones(cloud.getNbPoints());
982  EXPECT_EQ(filteredCloud.descriptorLabels.back().text, descriptorName);
983  EXPECT_EQ(filteredCloud.descriptorLabels.back().span, descriptorDimension);
984  for(unsigned i = 0; i < descriptorDimension; ++i)
985  {
986  EXPECT_EQ(filteredCloud.descriptors.row(filteredCloud.descriptors.rows()-3+i), row*descriptorValues[i]);
987  }
988 
989 
990  descriptorValues = std::vector<NumericType>{-2, -3, -4};
991  params["descriptorValues"] = toParam(descriptorValues);
992 
993  addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create(
994  "AddDescriptorDataPointsFilter", params
995  );
996  DP filteredCloudOvewriteParams = addDescriptorFilter->filter(filteredCloud);
997  EXPECT_EQ(filteredCloudOvewriteParams.descriptorLabels.back().text, descriptorName);
998  EXPECT_EQ(filteredCloudOvewriteParams.descriptorLabels.back().span, descriptorDimension);
999  for(unsigned i = 0; i < descriptorDimension; ++i)
1000  {
1001  EXPECT_EQ(filteredCloudOvewriteParams.descriptors.row(filteredCloud.descriptors.rows()-3+i), row*descriptorValues[i]);
1002  }
1003 
1004 
1005  descriptorValues = std::vector<NumericType>{-2, -3, -4, -5};
1006  params["descriptorDimension"] = toParam(4);
1007  params["descriptorValues"] = toParam(descriptorValues);
1008  addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create(
1009  "AddDescriptorDataPointsFilter", params
1010  );
1011  EXPECT_THROW(addDescriptorFilter->filter(filteredCloud), std::runtime_error);
1012 
1013  params = PM::Parameters();
1014  params["descriptorName"] = "my_descriptor";
1015  params["descriptorDimension"] = toParam(descriptorDimension);
1016  params["descriptorValues"] = "[2, 3, 4]";
1017 
1018  addFilter("AddDescriptorDataPointsFilter", params);
1019  validate3dTransformation();
1020 }
FixStepSamplingDataPointsFilter
Systematic sampling, with variation over time.
Definition: FixStepSampling.h:41
EXPECT_GE
#define EXPECT_GE(val1, val2)
Definition: gtest.h:19757
PointMatcher< float >::Parameters
Parametrizable::Parameters Parameters
alias
Definition: PointMatcher.h:186
PointMatcherSupport::toParam
std::string toParam(const S &value)
Return the string value using lexical_cast.
Definition: Parametrizable.h:123
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
OrientNormalsDataPointsFilter
Reorientation of normals.
Definition: OrientNormals.h:41
DistanceLimitDataPointsFilter
Subsampling. Filter points based on distance measured on a specific axis.
Definition: DistanceLimit.h:41
MaxQuantileOnAxisDataPointsFilter
Subsampling. Filter points beyond a maximum quantile measured on a specific axis.
Definition: MaxQuantileOnAxis.h:41
MinDistDataPointsFilter
Subsampling. Filter points before a minimum distance measured on a specific axis.
Definition: MinDist.h:41
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
icp_customized.name
string name
Definition: icp_customized.py:45
ref3D
DP ref3D
Definition: utest.cpp:47
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
Definition: gtest.h:19868
RemoveSensorBiasDataPointsFilter
Definition: RemoveSensorBias.h:41
MaxDistDataPointsFilter
Subsampling. Filter points beyond a maximum distance measured on a specific axis.
Definition: MaxDist.h:41
PointMatcher::DataPoints::getTimeDim
unsigned getTimeDim() const
Return the total number of times.
Definition: pointmatcher/DataPoints.cpp:194
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:19327
testing::internal::string
::std::string string
Definition: gtest.h:1979
IcpHelper
Definition: utest.h:32
EXPECT_GT
#define EXPECT_GT(val1, val2)
Definition: gtest.h:19759
GestaltDataPointsFilter
Gestalt descriptors filter as described in Bosse & Zlot ICRA 2013.
Definition: Gestalt.h:41
SurfaceNormalDataPointsFilter
Surface normals estimation. Find the normal for every point using eigen-decomposition of neighbour po...
Definition: SurfaceNormal.h:43
SaliencyDataPointsFilter
Definition: Saliency.h:40
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:19747
DataFilterTest::addFilter
void addFilter(string name)
Definition: DataFilters.cpp:38
PointMatcher< float >::Int64Matrix
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
A dense signed 64-bits matrix.
Definition: PointMatcher.h:173
OctreeGridDataPointsFilter
Data Filter based on Octree representation.
Definition: OctreeGrid.h:54
EXPECT_FALSE
#define EXPECT_FALSE(condition)
Definition: gtest.h:19330
align_sequence.params
params
Definition: align_sequence.py:13
DataFilterTest
Definition: DataFilters.cpp:13
AddDescriptorDataPointsFilter
Add new descriptor to an existing point cloud.
Definition: AddDescriptor.h:10
VoxelGridDataPointsFilter
Definition: VoxelGrid.h:40
DataFilterTest::addFilter
void addFilter(string name, PM::Parameters params)
Definition: DataFilters.cpp:30
SamplingSurfaceNormalDataPointsFilter
Sampling surface normals. First decimate the space until there is at most knn points,...
Definition: SamplingSurfaceNormal.h:41
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
RandomSamplingDataPointsFilter
Random sampling.
Definition: RandomSampling.h:43
PointMatcher< float >::Matrix
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
TEST_F
TEST_F(DataFilterTest, IdentityDataPointsFilter)
Definition: DataFilters.cpp:74
ref2D
DP ref2D
Definition: utest.cpp:45
SpectralDecompositionDataPointsFilter
Definition: SpectralDecomposition.h:49
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: pointmatcher/DataPoints.cpp:158
IdentityDataPointsFilter
IdentityDataPointsFilter, does nothing.
Definition: DataPointsFilters/Identity.h:41
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
RemoveNaNDataPointsFilter
Remove points having NaN as coordinate.
Definition: RemoveNaN.h:41
data3D
DP data3D
Definition: utest.cpp:48
NumericType
double NumericType
Definition: utest.h:15
CovarianceSamplingDataPointsFilter
Definition: CovarianceSampling.h:40
DataFilterTest::SetUp
virtual void SetUp()
Definition: DataFilters.cpp:17
std
EXPECT_ANY_THROW
#define EXPECT_ANY_THROW(statement)
Definition: gtest.h:19315
MaxDensityDataPointsFilter
Subsampling. Reduce the points number by randomly removing points with a dentsity higher than a tresh...
Definition: MaxDensity.h:41
icp
Definition: icp.py:1
CutAtDescriptorThresholdDataPointsFilter
Subsampling. Cut points with value of a given descriptor above or below a given threshold.
Definition: CutAtDescriptorThreshold.h:43
PointMatcher::DataPoints::getDescriptorDim
unsigned getDescriptorDim() const
Return the total number of descriptors.
Definition: pointmatcher/DataPoints.cpp:187
EXPECT_THROW
#define EXPECT_THROW(statement, expected_exception)
Definition: gtest.h:19311
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:146
MaxPointCountDataPointsFilter
Maximum number of points.
Definition: MaxPointCount.h:41
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
DP
PM::DataPoints DP
Definition: convert.cpp:45
DataFilterTest::TearDown
virtual void TearDown()
Definition: DataFilters.cpp:28
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:555
DataFilterTest::generateRandomDataPoints
DP generateRandomDataPoints(int nbPoints=100)
Definition: DataFilters.cpp:46
NormalSpaceDataPointsFilter
Definition: NormalSpace.h:40
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:221

libpointmatcher
Author(s):
autogenerated on Mon Sep 16 2024 02:24:07