MRPT  1.9.9
CGridMapAlignerApp.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "apps-precomp.h" // Precompiled headers
11 
13 
15 #include <mrpt/gui.h>
19 #include <mrpt/io/CFileStream.h>
21 #include <mrpt/maps/CSimpleMap.h>
24 #include <mrpt/math/wrap2pi.h>
25 #include <mrpt/obs/CSensoryFrame.h>
28 #include <mrpt/otherlibs/tclap/CmdLine.h>
29 #include <mrpt/poses/CPoint2D.h>
32 #include <mrpt/random.h>
34 #include <mrpt/system/datetime.h>
35 #include <mrpt/system/filesystem.h>
36 #include <mrpt/system/os.h>
38 
39 using namespace mrpt::apps;
40 
42  : mrpt::system::COutputLogger("CGridMapAlignerApp")
43 {
44 }
45 
46 void CGridMapAlignerApp::initialize(int argc, const char** argv)
47 {
49 
50  // Declare the supported options.
51  TCLAP::CmdLine cmd(
52  "grid-matching", ' ', mrpt::system::MRPT_getVersion().c_str());
53 
54  TCLAP::SwitchArg arg_match(
55  "m", "match", "Operation: match two maps", cmd, false);
56  TCLAP::SwitchArg arg_detect(
57  "d", "detect-test", "Operation: Quality of match with one map", cmd,
58  false);
59 
60  TCLAP::ValueArg<std::string> arg_filgrid1(
61  "1", "map1", "Map #1 to align (*.simplemap)", true, "",
62  "map1.simplemap", cmd);
63  TCLAP::ValueArg<std::string> arg_filgrid2(
64  "2", "map2", "Map #2 to align (*.simplemap)", false, "",
65  "map2.simplemap", cmd);
66 
67  TCLAP::ValueArg<std::string> arg_out(
68  "o", "out", "Output file for the results", false,
69  "gridmatching_out.txt", "result_outfile", cmd);
70  TCLAP::ValueArg<std::string> arg_config(
71  "c", "config", "Optional config. file with more params", false, "",
72  "config.ini", cmd);
73 
74  TCLAP::ValueArg<std::string> arg_aligner_method(
75  "", "aligner", "The method to use for map aligning", false,
76  "amModifiedRANSAC", "[amCorrelation|amRobustMatch|amModifiedRANSAC]",
77  cmd);
78  TCLAP::ValueArg<std::string> arg_out_dir(
79  "", "out-dir", "The output directory", false, "GRID-MATCHING_RESULTS",
80  "GRID-MATCHING_RESULTS", cmd);
81 
82  TCLAP::SwitchArg arg_savesog3d(
83  "3", "save-sog-3d", "Save a 3D view of all the SOG modes", cmd, false);
84  TCLAP::SwitchArg arg_savesogall(
85  "a", "save-sog-all", "Save all the map overlaps", cmd, false);
86  TCLAP::SwitchArg arg_savecorrdists(
87  "t", "save-corr-dists", "Save corr & non-corr distances", cmd, false);
88  TCLAP::ValueArg<std::string> arg_icpgoodness(
89  "i", "save-icp-goodness", "Append all ICP goodness values here", false,
90  "", "icp_goodness.txt", cmd);
91 
92  TCLAP::ValueArg<double> arg_noise_std_xy(
93  "x", "noise-std-xy", "In detect-test mode,std. noise in XY", false, 0,
94  "sigma", cmd);
95  TCLAP::ValueArg<double> arg_noise_std_phi(
96  "p", "noise-std-phi", "In detect-test mode,std. noise in PHI (deg)",
97  false, 0, "sigma", cmd);
98  TCLAP::ValueArg<double> arg_noise_std_laser(
99  "l", "noise-std-laser", "In detect-test mode,std. noise range (m)",
100  false, 0, "sigma", cmd);
101  TCLAP::ValueArg<unsigned int> arg_niters(
102  "N", "iters", "In detect-test mode,number of trials", false, 1,
103  "rep.count", cmd);
104 
105  TCLAP::ValueArg<double> arg_Ax(
106  "X", "Ax", "In detect-test mode, displacement in X (m)", false, 4, "X",
107  cmd);
108  TCLAP::ValueArg<double> arg_Ay(
109  "Y", "Ay", "In detect-test mode, displacement in Y (m)", false, 2, "Y",
110  cmd);
111  TCLAP::ValueArg<double> arg_Aphi(
112  "P", "Aphi", "In detect-test mode, displacement in PHI (deg)", false,
113  30, "PHI", cmd);
114 
115  TCLAP::SwitchArg arg_noise_pose(
116  "O", "noise-pose", "detect-test mode: enable noise in pose", cmd,
117  false);
118  TCLAP::SwitchArg arg_noise_laser(
119  "L", "noise-laser", "detect-test mode: enable noise in laser", cmd,
120  false);
121 
122  TCLAP::SwitchArg arg_verbose("v", "verbose", "Verbose output", cmd, false);
123  TCLAP::SwitchArg arg_nologo(
124  "g", "nologo", "skip the logo at startup", cmd, false);
125  TCLAP::SwitchArg arg_nosave(
126  "n", "nosave", "skip saving map images", cmd, false);
127  TCLAP::SwitchArg arg_skip_icp(
128  "s", "noicp", "skip ICP optimization stage", cmd, false);
129  TCLAP::SwitchArg arg_most_likely(
130  "", "most-likely-only",
131  "Keep the most-likely Gaussian mode from the SOG", cmd, false);
132 
133  // Parse arguments:
134  if (!cmd.parse(argc, argv))
135  THROW_EXCEPTION("CLI arguments parsing tells we should exit.");
136 
137  fil_grid1 = arg_filgrid1.getValue();
138  fil_grid2 = arg_filgrid2.getValue();
139  OUTPUT_FIL = arg_out.getValue();
140  CONFIG_FIL = arg_config.getValue();
141  SAVE_ICP_GOODNESS_FIL = arg_icpgoodness.getValue();
142 
145  name2value(arg_aligner_method.getValue());
146 
147  STD_NOISE_XY = arg_noise_std_xy.getValue();
148  STD_NOISE_PHI = DEG2RAD(arg_noise_std_phi.getValue());
149  STD_NOISE_LASER = arg_noise_std_laser.getValue();
150  N_ITERS = arg_niters.getValue();
151 
152  GT_Ax = arg_Ax.getValue();
153  GT_Ay = arg_Ay.getValue();
154  GT_Aphi_rad = DEG2RAD(arg_Aphi.getValue());
155 
156  SAVE_SOG_3DSCENE = arg_savesog3d.getValue();
157  SAVE_SOG_ALL_MAPS_OVERLAP_HYPOTHESES = arg_savesogall.getValue();
158  IS_VERBOSE = arg_verbose.getValue();
159  NOSAVE = arg_nosave.getValue();
160  SAVE_CORR_AND_NONCORR_DISTS = arg_savecorrdists.getValue();
161 
162  if (!arg_nologo.getValue())
163  {
164  printf(" grid-matching - Part of the MRPT\n");
165  printf(
166  " MRPT C++ Library: %s - Sources timestamp: %s\n",
169  }
170 
171  SKIP_ICP_STAGE = arg_skip_icp.getValue();
172  MOST_LIKELY_SOG_MODE_ONLY = arg_most_likely.getValue();
173  NOISE_IN_POSE = arg_noise_pose.getValue();
174  NOISE_IN_LASER = arg_noise_laser.getValue();
175 
176  RESULTS_DIR = arg_out_dir.getValue();
177 
178  is_match = arg_match.getValue();
179  is_detect_test = arg_detect.getValue();
180 
181  if (((!is_match && !is_detect_test) || (is_match && is_detect_test)) &&
183  {
184  std::cerr << std::endl
185  << "Error: One operation mode 'match' or 'detect-test' or "
186  "'save-corr-dists' must be selected."
187  << std::endl;
188  TCLAP::StdOutput so;
189  so.usage(cmd);
190  THROW_EXCEPTION("Wrong CLI parameters.");
191  }
192 
193  if (is_match)
194  {
195  // maps:
196  if (!arg_filgrid1.isSet() || !arg_filgrid2.isSet())
197  {
198  std::cerr << std::endl
199  << "Error: Two maps must be passed: --map1=xxx and "
200  "--map2=xxx"
201  << std::endl;
202  TCLAP::StdOutput so;
203  so.usage(cmd);
204  THROW_EXCEPTION("Wrong CLI parameters.");
205  }
206  }
207 
208  MRPT_END
209 }
210 
212 {
213  MRPT_START
214 
215  using namespace mrpt::slam;
216  using namespace mrpt::maps;
217  using namespace mrpt::obs;
218  using namespace mrpt::opengl;
219  using namespace mrpt::tfest;
220  using namespace mrpt::math;
221  using namespace mrpt::serialization;
222  using namespace mrpt::config;
223  using namespace mrpt::gui;
224  using namespace mrpt::img;
225  using namespace mrpt::io;
226  using namespace mrpt::random;
227  using namespace mrpt::poses;
228  using namespace std;
229 
230  // do_grid_align: all that follows
231  CImage img1, img2;
232 
234 
235  // Grid or simplemaps?
237  {
238  string ext1 = mrpt::system::extractFileExtension(fil_grid1, true);
239  string ext2 = mrpt::system::extractFileExtension(fil_grid2, true);
240  if (ext1 != ext2 || ext1 != "simplemap")
241  {
242  MRPT_LOG_INFO_STREAM("Map1: " << fil_grid1);
243  MRPT_LOG_INFO_STREAM("Map2: " << fil_grid2);
244  THROW_EXCEPTION("Both file extensions must be 'simplemap'");
245  }
246  }
247  if (is_detect_test)
248  {
249  string ext1 = mrpt::system::extractFileExtension(fil_grid1, true);
250  if (ext1 != "simplemap")
251  {
252  MRPT_LOG_INFO_STREAM("Map: " << fil_grid1);
253  THROW_EXCEPTION("The file extension must be 'simplemap'");
254  }
255  }
256 
257  CMultiMetricMap the_map1, the_map2;
258 
259  TSetOfMetricMapInitializers map1_inits, map2_inits;
260  {
262  def.resolution = 0.05f;
265  map1_inits.push_back(def);
266  map2_inits.push_back(def);
267  }
268 
269  if (!SKIP_ICP_STAGE)
270  {
273  def.renderOpts.color = mrpt::img::TColorf(0, 0, 1);
274  map1_inits.push_back(def);
275  def.renderOpts.color = mrpt::img::TColorf(1, 0, 0);
276  map2_inits.push_back(def);
277  }
278 
279  mrpt::system::deleteFiles(format("%s/*.*", RESULTS_DIR.c_str()));
281 
282  CFileOutputStream f_log(RESULTS_DIR + string("/log.txt"));
283  f_log.printf(
284  "%% std_noise_xy std_noise_phi std_noise_laser covDet:mean&std "
285  "stdPhi:mean&std error2D:mean&std errorPhi:mean&phi "
286  "bruteErr2D:mean&std brutErrPhi:mean&std GT_likelihood "
287  "#GTcorrFound:mean&std \n");
288  f_log.printf("%% -----------------------------------------\n");
289 
290  CFileStream f_out_log;
291  if (!OUTPUT_FIL.empty())
292  f_out_log.open(OUTPUT_FIL, fomWrite); //, fomAppend );
293 
294  CGridMapAligner gridAlign;
295  gridAlign.setMinLoggingLevel(this->getMinLoggingLevel());
296 
298  float tim;
299 
301 
302  if (!CONFIG_FIL.empty())
303  {
304  // Load options for the grid-matching algorithm:
305  CConfigFile cfg(CONFIG_FIL);
306  gridAlign.options.loadFromConfigFile(cfg, "grid-match");
307 
308  // load alternative config. for the metric maps:
309  if (cfg.sectionExists("metric_maps"))
310  {
311  map1_inits.loadFromConfigFile(cfg, "metric_maps");
312  map2_inits = map1_inits;
313  }
314  }
315 
316  // Dump params:
317  if (IS_VERBOSE)
318  {
319  gridAlign.options.dumpToConsole();
320  map1_inits.dumpToConsole();
321  }
322 
323  // Create the map with a points & grid-map within:
324  the_map1.setListOfMaps(map1_inits);
325  the_map2.setListOfMaps(map2_inits);
326 
329 
330  ASSERT_(grid1);
331  ASSERT_(grid2);
332 
333  // ---------------------------------------------
334  // Options: RANSAC
335  // ---------------------------------------------
336  gridAlign.options.ransac_SOG_sigma_m = grid1->getResolution() * 2;
337 
338  CSimpleMap map1, map2, map2noisy;
339 
340  // Load maps:
341  {
343  archiveFrom(f) >> map1;
344  }
345 
346  // If it's detect_test only load one map:
348  {
350  archiveFrom(f) >> map2;
351  }
352 
353  // Generate the_map1 now:
355 
356  size_t N1 = max(
357  40,
358  (int)round(grid1->getArea() * gridAlign.options.featsPerSquareMeter));
359 
360  COccupancyGridMapFeatureExtractor gridfeatextract;
361 
362  CLandmarksMap lm1;
363 
364  gridfeatextract.extractFeatures(
365  *grid1, lm1, N1, gridAlign.options.feature_descriptor,
367 
368  if (!NOSAVE && (is_match || is_detect_test)) // If it's only a
369  // SAVE_CORR_AND_NONCORR_DISTS
370  // test, save time
371  {
372  grid1->saveAsBitmapFile(format("%s/map1.png", RESULTS_DIR.c_str()));
373  grid1->saveAsBitmapFileWithLandmarks(
374  format("%s/map1_LM.png", RESULTS_DIR.c_str()), &lm1, true);
375  CImage img;
376  grid1->getAsImageFiltered(img);
377  img.saveToFile(format("%s/map1_filt.png", RESULTS_DIR.c_str()));
378  }
379 
380  {
381  CVectorFloat stats_covDet, stats_stdPhi;
382  CVectorFloat stats_errorXY, stats_errorPhi;
383  CVectorFloat stats_bruteErrorXY, stats_bruteErrorPhi;
384  CVectorFloat stats_GT_likelihood;
385  std::vector<uint32_t> overallGTcorrsFound;
386 
387  for (unsigned int iter = 0; iter < N_ITERS; iter++)
388  {
389  // Insert noise into the laser scans of map2:
390  // ------------------------------------------------
391  map2noisy = is_detect_test ? map1 : map2;
392 
393  for (unsigned int q = 0; q < map2noisy.size(); q++)
394  {
395  CPose3DPDF::Ptr PDF;
397 
398  map2noisy.get(q, PDF, SF);
399 
400  // If it's detect_test, translate the map2 by a fixed, known
401  // quantity:
402  if (is_detect_test)
403  {
405  gt = -gt;
406  PDF->changeCoordinatesReference(CPose3D(gt));
407  }
408 
409  if (NOISE_IN_LASER)
410  {
412  SF->getObservationByClass<CObservation2DRangeScan>();
413  if (obs)
414  {
415  for (unsigned int k = 0; k < obs->getScanSize(); k++)
416  {
417  float& v = obs->getScanRange(k);
419  0, STD_NOISE_LASER);
420  if (v < 0) v = 0;
421  }
422  }
423  } // end of NOISE_IN_LASER
424 
425  if (NOISE_IN_POSE)
426  {
427  CPosePDFGaussian::Ptr newPDF =
428  std::make_shared<CPosePDFGaussian>();
429  newPDF->copyFrom(*PDF);
430 
431  // Change the pose:
432  newPDF->mean.x_incr(
433  getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY));
434  newPDF->mean.y_incr(
435  getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY));
436  newPDF->mean.phi_incr(getRandomGenerator().drawGaussian1D(
437  0, DEG2RAD(STD_NOISE_PHI)));
438  newPDF->mean.normalizePhi();
439 
440  // Change into the map:
441  map2noisy.set(q, newPDF, CSensoryFrame::Ptr());
442 
443  } // end of NOISE_IN_POSE
444  }
445 
446  the_map2.loadFromProbabilisticPosesAndObservations(map2noisy);
447 
448  // grid2->resetFeaturesCache();
449 
450  size_t N2 = max(
451  40,
452  (int)round(
453  grid2->getArea() * gridAlign.options.featsPerSquareMeter));
454 
455  CLandmarksMap lm2;
456  gridfeatextract.extractFeatures(
457  *grid2, lm2, N2, gridAlign.options.feature_descriptor,
459 
460  if (!NOSAVE && (is_match || is_detect_test)) // If it's only a
461  // SAVE_CORR_AND_NONCORR_DISTS
462  // test, save time
463  {
464  // Save maps:
465  grid2->saveAsBitmapFile(format(
466  "%s/map2_noise_%f.png", RESULTS_DIR.c_str(), STD_NOISE_XY));
467  grid2->saveAsBitmapFileWithLandmarks(
468  format(
469  "%s/map2_LM_noise_%f.png", RESULTS_DIR.c_str(),
470  STD_NOISE_XY),
471  &lm2, true);
472  CImage img;
473  grid2->getAsImageFiltered(img);
474  img.saveToFile(format(
475  "%s/map2_filt_noise_%f.png", RESULTS_DIR.c_str(),
476  STD_NOISE_XY));
477  }
478 
479  // Only if the case of "save-corr-dists" we can do NOT align the
480  // maps, since we're not interested in that
481  if (is_match || is_detect_test)
482  {
483  // --------------------------
484  // DO ALIGN
485  // --------------------------
486  CPosePDF::Ptr parts = gridAlign.Align(
487  &the_map1, &the_map2, CPose2D(0, 0, 0), &tim, &info);
488 
489  // Get the mean, or the best Gassian mean in the case of a SOG:
491  {
492  CPosePDFSOG::Ptr pdf_SOG =
493  std::dynamic_pointer_cast<CPosePDFSOG>(parts);
494  pdf_SOG->getMostLikelyCovarianceAndMean(
496  }
497  else
498  {
499  parts->getCovarianceAndMean(estimateCOV, estimateMean);
500  }
501 
502  float stdPhi = sqrt(estimateCOV(2, 2));
503 
504  const auto estimateCOV22 = estimateCOV.extractMatrix<2, 2>();
505  float stdXY = sqrt(estimateCOV22.det());
506 
507  float Axy = estimateMean.distance2DTo(GT_Ax, GT_Ay);
508  float Aphi =
510  float AxyBrute =
512  float AphiBrute = fabs(math::wrapToPi(
514 
515  MRPT_LOG_INFO_FMT("Done in %.03fms\n", 1000.0f * tim);
516 
517  MRPT_LOG_INFO_STREAM("Mean pose:\n\t " << estimateMean);
518  MRPT_LOG_INFO_STREAM("Estimate covariance::\n" << estimateCOV);
519 
520  // Save particles:
521  if (IS_CLASS(*parts, CPosePDFParticles))
522  {
523  CPosePDFParticles::Ptr partsPdf =
524  std::dynamic_pointer_cast<CPosePDFParticles>(parts);
525 
526  partsPdf->saveToTextFile(format(
527  "%s/particles_noise_%.03f.txt", RESULTS_DIR.c_str(),
528  STD_NOISE_XY));
529 
531  "Goodness: %.03f%%\n", 100 * info.goodness);
533  partsPdf->particlesCount() << " particles");
534  MRPT_LOG_INFO_STREAM("Covariance:\n\t " << estimateCOV);
535  }
536  else if (IS_CLASS(*parts, CPosePDFSOG))
537  {
538  CPosePDFSOG::Ptr pdf_SOG =
539  std::dynamic_pointer_cast<CPosePDFSOG>(parts);
541  "SoG has %u modes.", (unsigned int)pdf_SOG->size());
542 
543  pdf_SOG->normalizeWeights();
544  // pdf_SOG->saveToTextFile("_debug_SoG.txt");
545 
546  stats_GT_likelihood.push_back((float)pdf_SOG->evaluatePDF(
547  CPose2D(GT_Ax, GT_Ay, GT_Aphi_rad), true));
548 
549  if (f_out_log.fileOpenCorrectly())
550  {
551  f_out_log.printf("%% SOG_log_w x y phi \n");
552  for (size_t m = 0; m < pdf_SOG->size(); m++)
553  f_out_log.printf(
554  " %e %f %f %f\n", pdf_SOG->get(m).log_w,
555  pdf_SOG->get(m).mean.x(),
556  pdf_SOG->get(m).mean.y(),
557  pdf_SOG->get(m).mean.phi());
558  }
559 
560  if (SAVE_SOG_3DSCENE)
561  {
562  COpenGLScene scene3D;
563  opengl::CSetOfObjects::Ptr thePDF3D =
564  std::make_shared<opengl::CSetOfObjects>();
565  pdf_SOG->getAs3DObject(thePDF3D);
567  std::make_shared<opengl::CGridPlaneXY>(
568  -10, 10, -10, 10, 0, 1);
569  scene3D.insert(gridXY);
570  scene3D.insert(thePDF3D);
571  scene3D.saveToFile("_out_SoG.3Dscene");
572  }
573 
574  if (!SAVE_ICP_GOODNESS_FIL.empty())
575  {
578  SAVE_ICP_GOODNESS_FIL, true); // append & as column
579  }
580 
581  // Save all the maps overlap hypotheses:
583  {
585  size_t nNode;
586  CImage imgGrid1, imgCanvas;
587  grid1->resizeGrid(
588  min(grid1->getXMin(), -60.0f),
589  max(grid1->getXMax(), 60.0f),
590  min(-40.0f, grid1->getYMin()),
591  max(30.0f, grid1->getYMax()));
592  grid1->getAsImage(imgGrid1, true);
593  int imgGrid1LY = imgGrid1.getHeight();
594  const CPose2D nullPose(0, 0, 0);
595 
596  const TPoint2D q1(grid1->getXMin(), grid1->getYMin());
597  const TPoint2D q2(grid1->getXMin(), grid1->getYMax());
598  const TPoint2D q3(grid1->getXMax(), grid1->getYMax());
599  const TPoint2D q4(grid1->getXMax(), grid1->getYMin());
600 
601  const TPoint2D p1(grid2->getXMin(), grid2->getYMin());
602  const TPoint2D p2(grid2->getXMin(), grid2->getYMax());
603  const TPoint2D p3(grid2->getXMax(), grid2->getYMax());
604  const TPoint2D p4(grid2->getXMax(), grid2->getYMin());
605  for (nNode = 0, it = pdf_SOG->begin();
606  it != pdf_SOG->end(); it++, nNode++)
607  {
608  CPose2D x = it->mean;
609  const TPoint2D pp1(x + p1);
610  const TPoint2D pp2(x + p2);
611  const TPoint2D pp3(x + p3);
612  const TPoint2D pp4(x + p4);
613 
614  // Draw the background = the_map1:
615  imgCanvas = imgGrid1;
616 
617  // Draw the overlaped the_map2:
618  imgCanvas.line(
619  grid1->x2idx(pp1.x),
620  imgGrid1LY - 1 - grid1->y2idx(pp1.y),
621  grid1->x2idx(pp2.x),
622  imgGrid1LY - 1 - grid1->y2idx(pp2.y),
623  TColor::black());
624  imgCanvas.line(
625  grid1->x2idx(pp2.x),
626  imgGrid1LY - 1 - grid1->y2idx(pp2.y),
627  grid1->x2idx(pp3.x),
628  imgGrid1LY - 1 - grid1->y2idx(pp3.y),
629  TColor::black());
630  imgCanvas.line(
631  grid1->x2idx(pp3.x),
632  imgGrid1LY - 1 - grid1->y2idx(pp3.y),
633  grid1->x2idx(pp4.x),
634  imgGrid1LY - 1 - grid1->y2idx(pp4.y),
635  TColor::black());
636  imgCanvas.line(
637  grid1->x2idx(pp4.x),
638  imgGrid1LY - 1 - grid1->y2idx(pp4.y),
639  grid1->x2idx(pp1.x),
640  imgGrid1LY - 1 - grid1->y2idx(pp1.y),
641  TColor::black());
642 
643  imgCanvas.saveToFile(format(
644  "%s/_OVERLAP_MAPS_SOG_MODE_%04u.png",
645  RESULTS_DIR.c_str(), (unsigned int)nNode));
646 
647  // Save as 3D scene:
648  COpenGLScene scene;
649  CSetOfObjects::Ptr obj1 =
650  std::make_shared<CSetOfObjects>();
651  the_map1.getAs3DObject(obj1);
652  CSetOfObjects::Ptr obj2 =
653  std::make_shared<CSetOfObjects>();
654  the_map2.getAs3DObject(obj2);
655 
656  obj2->setPose(x);
657 
658  scene.insert(obj1);
659  scene.insert(obj2);
660 
661  // Add also the borders of the maps:
662  CSetOfLines::Ptr lines =
663  std::make_shared<CSetOfLines>();
664  lines->setLineWidth(3);
665  lines->setColor(0, 0, 1);
666 
667  lines->appendLine(q1.x, q1.y, 0, q2.x, q2.y, 0);
668  lines->appendLine(q2.x, q2.y, 0, q3.x, q3.y, 0);
669  lines->appendLine(q3.x, q3.y, 0, q4.x, q4.y, 0);
670  lines->appendLine(q4.x, q4.y, 0, q1.x, q1.y, 0);
671 
672  lines->appendLine(pp1.x, pp1.y, 0, pp2.x, pp2.y, 0);
673  lines->appendLine(pp2.x, pp2.y, 0, pp3.x, pp3.y, 0);
674  lines->appendLine(pp3.x, pp3.y, 0, pp4.x, pp4.y, 0);
675  lines->appendLine(pp4.x, pp4.y, 0, pp1.x, pp1.y, 0);
676 
677  scene.insert(lines);
678 
679  scene.saveToFile(format(
680  "%s/_OVERLAP_MAPS_SOG_MODE_%04u.3Dscene",
681  RESULTS_DIR.c_str(), (unsigned int)nNode));
682  }
683 
684  } // end SAVE_SOG_ALL
685 
686 #ifdef SAVE_SOG_GRID
687  // Save grid evaluation of the SOG:
688  CMatrixF gridLimits(1, 4);
689  gridLimits(0, 0) = estimateMean.x - 0.10f;
690  gridLimits(0, 1) = estimateMean.x + 0.10f,
691  gridLimits(0, 2) = estimateMean.y - 0.10f;
692  gridLimits(0, 3) = estimateMean.y + 0.10f;
693  gridLimits.saveToTextFile(format(
694  "%s/SOG_grid_limits_noise_%f.txt", RESULTS_DIR.c_str(),
695  STD_NOISE_XY));
696 
697  CMatrixD evalGrid;
698  pdf_SOG->evaluatePDFInArea(
699  gridLimits(0, 0), gridLimits(0, 1), gridLimits(0, 2),
700  gridLimits(0, 3), 0.002f, 0, evalGrid,
701  true // Sum over all phis
702  );
703 
704  evalGrid.saveToTextFile(format(
705  "%s/SOG_grid_noise_%f.txt", RESULTS_DIR.c_str(),
706  STD_NOISE_XY));
707 #endif
708  } // end if is SOG
709 
710  // STATS:
711  stats_covDet.push_back(stdXY);
712  stats_stdPhi.push_back(stdPhi);
713  stats_errorXY.push_back(Axy);
714  stats_errorPhi.push_back(Aphi);
715  stats_bruteErrorXY.push_back(AxyBrute);
716  stats_bruteErrorPhi.push_back(AphiBrute);
717 
718  } // end if we really do align
719 
720  // Save the descriptor distances for corresponding and
721  // non-corresponding features
722  // (only known if we are in the "is_detect_test" mode!)
724  {
725  MRPT_LOG_INFO_STREAM("Generating coor & no.corr distances...");
726 
727  const bool SAVE_ALSO_COORS_DEBUG_MAPS = false;
728 
729  CLandmarksMap::Ptr lmap1 = info.landmarks_map1;
730  CLandmarksMap::Ptr lmap2 = info.landmarks_map2;
731 
732  // only for the case of non "--match":
733  if (!lmap1 && !lmap2)
734  {
735  lmap1 = CLandmarksMap::Create();
736  lmap2 = CLandmarksMap::Create();
737 
738  gridfeatextract.extractFeatures(
739  *grid1, *lmap1, N1,
740  gridAlign.options.feature_descriptor,
742  gridfeatextract.extractFeatures(
743  *grid2, *lmap2, N2,
744  gridAlign.options.feature_descriptor,
746  }
747 
748  ASSERT_(lmap1 && lmap2);
749 
750  // GT transformation:
751  const CPose2D GT_Ap(GT_Ax, GT_Ay, GT_Aphi_rad);
752  TMatchingPairList gt_corrs;
753 
754  CFileOutputStream fout_CORR("GT_EXP_CORR.txt", true);
755  CFileOutputStream fout_NCORR("GT_EXP_NCORR.txt", true);
756 
757  // Compute the distances:
758  for (size_t i1 = 0; i1 < lmap1->landmarks.size(); i1++)
759  {
760  // Distances in descriptor space
761  CVectorDouble D(lmap2->landmarks.size());
762  // Distances in (x,y)
763  CVectorDouble dErrs(lmap2->landmarks.size());
764  size_t i2;
765 
766  const CLandmark* l1 = lmap1->landmarks.get(i1);
767 
768  for (i2 = 0; i2 < lmap2->landmarks.size(); i2++)
769  {
770  CLandmark* l2 = lmap2->landmarks.get(i2);
771 
772  CPoint2D P1 = CPoint2D(l1->pose_mean);
773  CPoint2D P2 = GT_Ap + CPoint2D(l2->pose_mean);
774 
775  const double dErr = P1.distanceTo(P2);
776  dErrs[i2] = dErr;
777 
778  ASSERT_(!l1->features.empty());
779  ASSERT_(!l2->features.empty());
780 
781  D[i2] = l1->features[0].descriptorDistanceTo(
782  l2->features[0]);
783  }
784 
785  size_t best_match = 0;
786  dErrs.minCoeff(best_match);
787  double MIN_DESCR_DIST = mrpt::math::minimum(D);
788  if (dErrs[best_match] < 0.20)
789  {
790  CLandmark* l2 = lmap2->landmarks.get(best_match);
791  gt_corrs.push_back(TMatchingPair(
792  i1, best_match, l1->pose_mean.x, l1->pose_mean.y,
793  l1->pose_mean.z, l2->pose_mean.x, l2->pose_mean.y,
794  l2->pose_mean.z));
795  }
796  else
797  best_match = (unsigned int)(-1);
798 
799  // double m,s; // Mean & Std:
800  // mrpt::math::meanAndStd(D,m,s);
801  // D = Abs( (D-m)/s );
802  // The output files:
803  for (i2 = 0; i2 < lmap2->landmarks.size(); i2++)
804  {
805  if (i2 == best_match)
806  fout_CORR.printf(
807  "%f %f\n", D[i2], D[i2] - MIN_DESCR_DIST);
808  else
809  fout_NCORR.printf(
810  "%f %f\n", D[i2], D[i2] - MIN_DESCR_DIST);
811  }
812 
813  // mrpt::system::pause();
814  } // end for i1
815 
816  MRPT_LOG_INFO_STREAM("GT corrs found: " << gt_corrs.size());
817  overallGTcorrsFound.push_back(gt_corrs.size());
818  if (SAVE_ALSO_COORS_DEBUG_MAPS)
819  {
820  COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
821  "GT_corrs.png", grid1.get(), grid2.get(), gt_corrs);
823  }
824  }
825 
826  } // end iter
827 
828  f_log.printf(
829  "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %e %f %f\n",
831  math::mean(stats_covDet), math::stddev(stats_covDet),
832  math::mean(stats_stdPhi), math::stddev(stats_stdPhi),
833  math::mean(stats_errorXY), math::stddev(stats_errorXY),
834  math::mean(stats_errorPhi), math::stddev(stats_errorPhi),
835  math::mean(stats_bruteErrorXY), math::stddev(stats_bruteErrorXY),
836  math::mean(stats_bruteErrorPhi), math::stddev(stats_bruteErrorPhi),
837  math::mean(stats_GT_likelihood), math::mean(overallGTcorrsFound),
838  math::stddev(overallGTcorrsFound));
839 
840  } // For each noise
841 
842  MRPT_END
843 }
This CStream derived class allow using a file as a read/write binary stream, creating it if the file ...
Definition: CFileStream.h:37
A namespace of pseudo-random numbers generators of diferent distributions.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:30
double x
X,Y coordinates.
Definition: TPoint2D.h:23
void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid) override
Draws a line.
Definition: CImage.cpp:1146
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
mrpt::poses::CPose2D estimateMean
static ENUMTYPE name2value(const std::string &name)
Gives the numerical name for a given enum text name.
Definition: TEnumType.h:100
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
void loadFromProbabilisticPosesAndObservations(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map...
Definition: CMetricMap.cpp:36
Template for column vectors of dynamic size, compatible with Eigen.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:34
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:328
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
double stddev(const VECTORLIKE &v, bool unbiased=true)
Computes the standard deviation of a vector.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
This file implements several operations that operate element-wise on individual or pairs of container...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
mrpt::maps::CPointsMap::TRenderOptions renderOpts
Rendering as 3D object options.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:878
STL namespace.
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
mrpt::system::COutputLogger COutputLogger
The ICP algorithm return information.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
void push_back(const T &val)
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > extractMatrix(const int start_row=0, const int start_col=0) const
Definition: MatrixBase.h:247
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:591
VerbosityLevel getMinLoggingLevel() const
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
CListGaussianModes::iterator iterator
Definition: CPosePDFSOG.h:67
This base provides a set of functions for maths stuff.
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::vision::TDescriptorType feature_descriptor
The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images.
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
constexpr double DEG2RAD(const double x)
Degrees to radians.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
GLint GLvoid * img
Definition: glext.h:3769
mrpt::vision::CFeatureExtraction::TOptions feature_detector_options
All the parameters for the feature detector.
mrpt::math::CMatrixDouble33 estimateCOV
A list of TMatchingPair.
Definition: TMatchingPair.h:70
void deleteFiles(const std::string &s)
Delete one or more files, especified by the (optional) path and the file name (including &#39;...
Definition: filesystem.cpp:190
This namespace contains representation of robot actions and observations.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
bool sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
void set(size_t index, const mrpt::poses::CPose3DPDF::Ptr &in_posePDF, const mrpt::obs::CSensoryFrame::Ptr &in_SF)
Changes the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:73
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
float maxDistanceInsertion
The largest distance at which cells will be updated (Default 15 meters)
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
This CStream derived class allow using a file as a write-only, binary stream.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
float ransac_SOG_sigma_m
The square root of the uncertainty normalization variance var_m (see papers...)
A class used to store a 2D point.
Definition: CPoint2D.h:32
bool open(const std::string &fileName, TFileOpenModes mode=fomRead|fomWrite)
Opens the file, returning true on success.
Definition: CFileStream.cpp:56
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:432
void extractFeatures(const mrpt::maps::COccupancyGridMap2D &grid, mrpt::maps::CLandmarksMap &outMap, const size_t number_of_features, const mrpt::vision::TDescriptorType descriptors, const mrpt::vision::CFeatureExtraction::TOptions &feat_options)
Computes a set of distinctive landmarks from an occupancy grid, and store them (previous content is n...
void run()
Runs with the current parameter set.
void push_back(const MAP_DEFINITION &o)
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
CONTAINER::Scalar minimum(const CONTAINER &v)
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") ...
A class for storing an occupancy grid map.
std::vector< double > icp_goodness_all_sog_modes
The ICP goodness of all potential SOG modes at the stage "sog2", thus before the removing of "bad" IC...
mrpt::slam::CGridMapAligner::TAlignerMethod aligner_method
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
const char * argv[]
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
double distance2DTo(double ax, double ay) const
Returns the 2D distance from this pose/point to a 2D point (ignores Z, if it exists).
Definition: CPoseOrPoint.h:237
T::Ptr mapByClass(const size_t &ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
#define MRPT_END
Definition: exceptions.h:245
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
std::vector< mrpt::vision::CFeature > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
double mean(const CONTAINER &v)
Computes the mean value of a vector.
float featsPerSquareMeter
Features extraction from grid map: How many features to extract.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:58
TAlignerMethod methodSelection
The aligner method:
mrpt::slam::CGridMapAligner::TConfigParams options
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
const int argc
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:337
A class for detecting features from occupancy grid maps.
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0...
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
GLenum GLint x
Definition: glext.h:3542
bool vectorToTextFile(const std::vector< float > &vec, const std::string &fileName, bool append=false, bool byRows=false)
A useful function for debugging, which saves a std::vector into a text file (compat.
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
This class stores any customizable set of metric maps.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Lightweight 2D point.
Definition: TPoint2D.h:31
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport)...
mrpt::poses::CPose2D noRobustEstimation
The "brute" estimation from using all the available correspondences, provided just for comparison pur...
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
Functions for estimating the optimal transformation between two frames of references given measuremen...
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:234
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019