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 float nan(std::numeric_limits<float>::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  vector<double> prob = {0.80, 0.85, 0.90, 0.95};
365  for(unsigned i = 0; i < prob.size(); i++)
366  {
367  // Try to avoid to low value for the reduction to avoid under sampling
368  params = PM::Parameters();
369  params["prob"] = toParam(prob[i]);
370 
371  icp.readingDataPointsFilters.clear();
372  addFilter("RandomSamplingDataPointsFilter", params);
373  validate2dTransformation();
374  validate3dTransformation();
375  }
376 }
377 
378 TEST_F(DataFilterTest, FixStepSamplingDataPointsFilter)
379 {
380  vector<unsigned> steps = {1, 2, 3};
381  for(unsigned i=0; i<steps.size(); i++)
382  {
383  // Try to avoid too low value for the reduction to avoid under sampling
384  params = PM::Parameters();
385  params["startStep"] = toParam(steps[i]);
386 
387  icp.readingDataPointsFilters.clear();
388  addFilter("FixStepSamplingDataPointsFilter", params);
389  validate2dTransformation();
390  validate3dTransformation();
391  }
392 }
393 
394 TEST_F(DataFilterTest, MaxPointCountDataPointsFilter)
395 {
396  DP cloud = ref3D;
397 
398  const size_t maxCount = 1000;
399 
400  params = PM::Parameters();
401  params["seed"] = "42";
402  params["maxCount"] = toParam(maxCount);
403 
404  std::shared_ptr<PM::DataPointsFilter> maxPtsFilter =
405  PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
406 
407  DP filteredCloud = maxPtsFilter->filter(cloud);
408 
409  //Check number of points
410  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
411  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
412  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
413 
414  EXPECT_EQ(filteredCloud.getNbPoints(), maxCount);
415 
416  //Same seed should result same filtered cloud
417  DP filteredCloud2 = maxPtsFilter->filter(cloud);
418 
419  EXPECT_TRUE(filteredCloud == filteredCloud2);
420 
421  //Different seeds should not result same filtered cloud but same number
422  params.clear();
423  params["seed"] = "1";
424  params["maxCount"] = toParam(maxCount);
425 
426  std::shared_ptr<PM::DataPointsFilter> maxPtsFilter2 =
427  PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
428 
429  DP filteredCloud3 = maxPtsFilter2->filter(cloud);
430 
431  EXPECT_FALSE(filteredCloud3 == filteredCloud2);
432 
433  EXPECT_EQ(filteredCloud3.getNbPoints(), maxCount);
434 
435  EXPECT_EQ(filteredCloud3.getNbPoints(), filteredCloud2.getNbPoints());
436  EXPECT_EQ(filteredCloud3.getDescriptorDim(), filteredCloud2.getDescriptorDim());
437  EXPECT_EQ(filteredCloud3.getTimeDim(), filteredCloud2.getTimeDim());
438 
439  //Validate transformation
440  icp.readingDataPointsFilters.clear();
441  addFilter("MaxPointCountDataPointsFilter", params);
442  validate2dTransformation();
443  validate3dTransformation();
444 }
445 
446 TEST_F(DataFilterTest, OctreeGridDataPointsFilter)
447 {
448  const unsigned int nbPts = 60000;
449  const DP cloud = generateRandomDataPoints(nbPts);
450  params = PM::Parameters();
451 
452  std::shared_ptr<PM::DataPointsFilter> octreeFilter;
453 
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})
457  {
458  params.clear();
459  params["maxPointByNode"] = toParam(maxData);
460  params["maxSizeByNode"] = toParam(maxSize);
461  params["samplingMethod"] = toParam(meth);
462  params["buildParallel"] = "1";
463 
464  octreeFilter = PM::get().DataPointsFilterRegistrar.create("OctreeGridDataPointsFilter", params);
465 
466  const DP filteredCloud = octreeFilter->filter(cloud);
467 
468  if(maxData==1 and maxSize==0.)
469  {
470  // 1/pts by octants + validate parallel build
471  // the number of point should not change
472  // the sampling methods should not change anything
473  //Check number of points
474  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
475  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
476  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
477 
478  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
479  }
480  else
481  {
482  //Check number of points
483  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
484  }
485  //Validate transformation
486  icp.readingDataPointsFilters.clear();
487  addFilter("OctreeGridDataPointsFilter", params);
488  validate2dTransformation();
489  validate3dTransformation();
490  }
491 }
492 
493 TEST_F(DataFilterTest, NormalSpaceDataPointsFilter)
494 {
495  const size_t nbPts = 60000;
496  DP cloud = generateRandomDataPoints(nbPts);
497  params = PM::Parameters();
498 
499  //const size_t nbPts2D = ref2D.getNbPoints();
500  const size_t nbPts3D = ref3D.getNbPoints();
501 
502  std::shared_ptr<PM::DataPointsFilter> nssFilter;
503 
504  //Compute normals
505  auto paramsNorm = PM::Parameters();
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);
510 
511  normalFilter->inPlaceFilter(cloud);
512 
513  //Evaluate filter
514  std::vector<size_t> samples = {/* 2*nbPts2D/3, nbPts2D,*/ 1500, 5000, nbPts, nbPts3D};
515  for(const float epsilon : {M_PI/6., M_PI/32., M_PI/64.})
516  for(const size_t nbSample : samples)
517  {
518  icp.readingDataPointsFilters.clear();
519 
520  params.clear();
521  params["epsilon"] = toParam(epsilon);
522  params["nbSample"] = toParam(nbSample);
523 
524  nssFilter = PM::get().DataPointsFilterRegistrar.create("NormalSpaceDataPointsFilter", params);
525 
526  addFilter("SurfaceNormalDataPointsFilter", paramsNorm);
527  addFilter("NormalSpaceDataPointsFilter", params);
528 
529  const DP filteredCloud = nssFilter->filter(cloud);
530 
531  /*
532  if(nbSample <= nbPts2D)
533  {
534  validate2dTransformation();
535  EXPECT_LE(filteredCloud.getNbPoints(), nbPts2D);
536  continue;
537  }
538  else if (nbSample == nbPts3D)
539  {
540  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts3D);
541  }
542  else */
543  if (nbSample == nbPts)
544  {
545  //Check number of points
546  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
547  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
548  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
549 
550  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
551  }
552 
553  validate3dTransformation();
554  EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
555  }
556 }
557 
558 TEST_F(DataFilterTest, CovarianceSamplingDataPointsFilter)
559 {
560  const size_t nbPts = 60000;
561  DP cloud = generateRandomDataPoints(nbPts);
562  params = PM::Parameters();
563 
564  const size_t nbPts3D = ref3D.getNbPoints();
565 
566  std::shared_ptr<PM::DataPointsFilter> covsFilter;
567 
568  //Compute normals
569  auto paramsNorm = PM::Parameters();
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);
574 
575  normalFilter->inPlaceFilter(cloud);
576 
577  //Evaluate filter
578  std::vector<size_t> samples = {500, 1500, 5000, nbPts, nbPts3D};
579  for(const size_t nbSample : samples)
580  {
581  icp.readingDataPointsFilters.clear();
582 
583  params.clear();
584  params["nbSample"] = toParam(nbSample);
585 
586  covsFilter = PM::get().DataPointsFilterRegistrar.create("CovarianceSamplingDataPointsFilter", params);
587 
588  addFilter("SurfaceNormalDataPointsFilter", paramsNorm);
589  addFilter("CovarianceSamplingDataPointsFilter", params);
590 
591  const DP filteredCloud = covsFilter->filter(cloud);
592 
593  if (nbSample == nbPts3D)
594  {
595  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts3D);
596  }
597  else if (nbSample == nbPts)
598  {
599  //Check number of points
600  EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints());
601  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
602  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
603 
604  EXPECT_EQ(filteredCloud.getNbPoints(), nbPts);
605  }
606 
607  validate3dTransformation();
608  EXPECT_GE(cloud.getNbPoints(), filteredCloud.getNbPoints());
609  }
610 }
611 
612 TEST_F(DataFilterTest, VoxelGridDataPointsFilter)
613 {
614  // Test with point cloud
615  DP cloud = generateRandomDataPoints();
616 
617  params = PM::Parameters();
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);
623 
624  std::shared_ptr<PM::DataPointsFilter> voxelFilter =
625  PM::get().DataPointsFilterRegistrar.create("VoxelGridDataPointsFilter", params);
626 
627  DP filteredCloud = voxelFilter->filter(cloud);
628 
629  EXPECT_GT(cloud.getNbPoints(), filteredCloud.getNbPoints());
630  EXPECT_EQ(cloud.getDescriptorDim(), filteredCloud.getDescriptorDim());
631  EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim());
632 
633  // Test with ICP
634  vector<bool> useCentroid = {false, true};
635  vector<bool> averageExistingDescriptors = {false, true};
636  for (unsigned i = 0 ; i < useCentroid.size() ; i++)
637  {
638  for (unsigned j = 0; j < averageExistingDescriptors.size(); j++)
639  {
640  params = PM::Parameters();
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);
646 
647  icp.readingDataPointsFilters.clear();
648  addFilter("VoxelGridDataPointsFilter", params);
649  validate2dTransformation();
650  }
651  }
652 
653  for (unsigned i = 0 ; i < useCentroid.size() ; i++)
654  {
655  for (unsigned j = 0; j < averageExistingDescriptors.size(); j++)
656  {
657  params = PM::Parameters();
658  params["vSizeX"] = "1";
659  params["vSizeY"] = "1";
660  params["vSizeZ"] = "1";
661  params["useCentroid"] = toParam(true);
662  params["averageExistingDescriptors"] = toParam(true);
663 
664  icp.readingDataPointsFilters.clear();
665  addFilter("VoxelGridDataPointsFilter", params);
666  validate3dTransformation();
667  }
668  }
669 }
670 
671 TEST_F(DataFilterTest, CutAtDescriptorThresholdDataPointsFilter)
672 {
673  // Copied from density ratio above
674  vector<double> thresholds = {100, 1000, 5000};
675 
676  DP ref3Ddensities = ref3D;
677  // Adding descriptor "densities"
678  icp.readingDataPointsFilters.clear();
679  params = PM::Parameters();
680  params["knn"] = "5";
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";
687 
688 
689  addFilter("SurfaceNormalDataPointsFilter", params);
690  icp.readingDataPointsFilters.apply(ref3Ddensities);
691 
692  for(unsigned i=0; i < thresholds.size(); i++)
693  {
694  int belowCount=0;
695  int aboveCount=0;
696 
697  // counting points above and below
698  PM::DataPoints::View densities = ref3Ddensities.getDescriptorViewByName("densities");
699  for (unsigned j=0; j < densities.cols(); ++j)
700  {
701  if (densities(0, j) <= thresholds[i])
702  {
703  ++belowCount;
704  }
705  if (densities(0, j) >= thresholds[i])
706  {
707  ++aboveCount;
708  }
709  }
710 
711  for(bool useLargerThan(true); useLargerThan; useLargerThan=false)
712  {
713  DP ref3DCopy = ref3Ddensities;
714 
715  icp.readingDataPointsFilters.clear();
716  params = PM::Parameters();
717  params["descName"] = toParam("densities");
718  params["useLargerThan"] = toParam(useLargerThan);
719  params["threshold"] = toParam(thresholds[i]);
720 
721 
722  addFilter("CutAtDescriptorThresholdDataPointsFilter", params);
723  icp.readingDataPointsFilters.apply(ref3DCopy);
724  if (useLargerThan)
725  {
726  EXPECT_TRUE(ref3DCopy.features.cols() == belowCount);
727  }
728  else
729  {
730  EXPECT_TRUE(ref3DCopy.features.cols() == aboveCount);
731  }
732  }
733  }
734 }
735 
736 TEST_F(DataFilterTest, DistanceLimitDataPointsFilter)
737 {
738  params = PM::Parameters();
739  params["dim"] = "0";
740  params["dist"] = toParam(6.0);
741  params["removeInside"] = "0";
742 
743  // Filter on x axis
744  params["dim"] = "0";
745  icp.readingDataPointsFilters.clear();
746  addFilter("DistanceLimitDataPointsFilter", params);
747  validate2dTransformation();
748  validate3dTransformation();
749 
750  // Filter on y axis
751  params["dim"] = "1";
752  icp.readingDataPointsFilters.clear();
753  addFilter("DistanceLimitDataPointsFilter", params);
754  validate2dTransformation();
755  validate3dTransformation();
756 
757  // Filter on z axis (not existing)
758  params["dim"] = "2";
759  icp.readingDataPointsFilters.clear();
760  addFilter("DistanceLimitDataPointsFilter", params);
761  EXPECT_ANY_THROW(validate2dTransformation());
762  validate3dTransformation();
763 
764  // Filter on a radius
765  params["dim"] = "-1";
766  icp.readingDataPointsFilters.clear();
767  addFilter("DistanceLimitDataPointsFilter", params);
768  validate2dTransformation();
769  validate3dTransformation();
770 
771  // Parameter outside valid range
772  params["dim"] = "3";
773  //TODO: specify the exception, move that to GenericTest
774  EXPECT_ANY_THROW(addFilter("DistanceLimitDataPointsFilter", params));
775 
776 
777  params = PM::Parameters();
778  params["dim"] = "0";
779  params["dist"] = toParam(0.05);
780  params["removeInside"] = "1";
781 
782  // Filter on x axis
783  params["dim"] = "0";
784  icp.readingDataPointsFilters.clear();
785  addFilter("DistanceLimitDataPointsFilter", params);
786  validate2dTransformation();
787  validate3dTransformation();
788 
789  // Filter on y axis
790  params["dim"] = "1";
791  icp.readingDataPointsFilters.clear();
792  addFilter("DistanceLimitDataPointsFilter", params);
793  validate2dTransformation();
794  validate3dTransformation();
795 
796  //TODO: move that to specific 2D test
797  // Filter on z axis (not existing)
798  params["dim"] = "2";
799  icp.readingDataPointsFilters.clear();
800  addFilter("DistanceLimitDataPointsFilter", params);
801  EXPECT_ANY_THROW(validate2dTransformation());
802  validate3dTransformation();
803 
804  // Filter on a radius
805  params["dim"] = "-1";
806  icp.readingDataPointsFilters.clear();
807  addFilter("DistanceLimitDataPointsFilter", params);
808  validate2dTransformation();
809  validate3dTransformation();
810 }
811 
812 TEST_F(DataFilterTest, SameFilterInstanceTwice)
813 {
814  params = PM::Parameters();
815 
816  std::shared_ptr<PM::DataPointsFilter> df = PM::get().DataPointsFilterRegistrar.create("MaxPointCountDataPointsFilter", params);
817 
818  icp.referenceDataPointsFilters.push_back(df);
819  icp.readingDataPointsFilters.push_back(df);
820 }
821 
822 TEST_F(DataFilterTest, RemoveSensorBiasDataPointsFilter)
823 {
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 };
829 
830  PM::Matrix points = (PM::Matrix(3,6) << 1, 1, 1, 5, 5, 5,
831  0, 0, 0, 0, 0, 0,
832  0, 0, 0, 0, 0, 0).finished();
833  DP::Labels pointsLabels;
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));
838 
839  PM::Matrix desc = (PM::Matrix(4,6) << 0., 0.7854, 1.4835, 0., 0.7854, 1.4835, //0,45,85 degrees
840  -1, -1, -1, -5, -5, -5,
841  0, 0, 0, 0, 0, 0, // observation : point to sensor
842  0, 0, 0, 0, 0, 0).finished();
843 
844  PM::Matrix desc2 = (PM::Matrix(4,6) << 0., 0.7854, std::nanf(""), 0., 0.7854, M_PI_2, //0,45,90 degrees
845  -1, -1, -1, -5, -5, -5,
846  0, 0, 0, 0, 0, 0, // observation : point to sensor
847  0, 0, 0, 0, 0, 0).finished();
848  DP::Labels descLabels;
849  descLabels.push_back(DP::Label("incidenceAngles", 1));
850  descLabels.push_back(DP::Label("observationDirections", 3));
851 
852  PM::Int64Matrix randTimes = PM::Int64Matrix::Random(2, nbPts);
853  DP::Labels timeLabels;
854  timeLabels.push_back(DP::Label("dummyTime", 2));
855 
856  // Construct the point cloud from the generated matrices
857  DP pointCloud = DP(points, pointsLabels, desc, descLabels, randTimes, timeLabels);
858 
859 
860  PM::Parameters parameters;
861  parameters["sensorType"] = toParam(0); //LMS_1xx
862  std::shared_ptr<PM::DataPointsFilter> removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
863  DP resultCloud = removeSensorBiasFilter->filter(pointCloud);
864  EXPECT_EQ(pointCloud.getNbPoints(), resultCloud.getNbPoints());
865 
866  for(std::size_t i = 0; i< nbPts; ++i)
867  {
868  const double error = pointCloud.features.col(i).norm() - resultCloud.features.col(i).norm();
869  EXPECT_NEAR(expectedErrors_LMS1xx[i], error, 1e-3); // below mm
870  }
871 
872  parameters["sensorType"] = toParam(1); //HDL32E
873  removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
874  resultCloud = removeSensorBiasFilter->filter(pointCloud);
875 
876  for(std::size_t i = 0; i< nbPts; ++i)
877  {
878  const double error = pointCloud.features.col(i).norm() - resultCloud.features.col(i).norm();
879  EXPECT_NEAR(expectedErrors_HDL32E[i], error, 1e-4); // below mm
880  }
881 
882 
883  //test points rejection
884  pointCloud = DP(points, pointsLabels, desc2, descLabels, randTimes, timeLabels);
885 
886  parameters["sensorType"] = toParam(0); //LMS_1xx
887  parameters["angleThreshold"] = toParam(30.);
888  removeSensorBiasFilter = PM::get().DataPointsFilterRegistrar.create("RemoveSensorBiasDataPointsFilter", parameters);
889  resultCloud = removeSensorBiasFilter->filter(pointCloud);
890 
891  //four points should have been rejected
892  EXPECT_EQ(pointCloud.getNbPoints()-4, resultCloud.getNbPoints());
893 }
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 a string value using lexical_cast.
Definition: Parametrizable.h:92
epsilon
double epsilon
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
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:193
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:19327
IcpHelper
Definition: utest.h:31
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
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
DataFilterTest
Definition: DataFilters.cpp:13
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
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
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
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
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:186
PointMatcher< float >::get
static const PointMatcher & get()
Return instances.
Definition: Registry.cpp:141
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:554
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

mp2p_icp
Author(s):
autogenerated on Thu Dec 26 2024 03:48:10