MRPT  2.0.5
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-2020, 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 
14 #include <mrpt/3rdparty/tclap/CmdLine.h>
16 #include <mrpt/gui.h>
20 #include <mrpt/io/CFileStream.h>
23 #include <mrpt/maps/CSimpleMap.h>
26 #include <mrpt/math/wrap2pi.h>
27 #include <mrpt/obs/CSensoryFrame.h>
30 #include <mrpt/poses/CPoint2D.h>
33 #include <mrpt/random.h>
35 #include <mrpt/system/datetime.h>
36 #include <mrpt/system/filesystem.h>
37 #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 
300 
301  if (!CONFIG_FIL.empty())
302  {
303  // Load options for the grid-matching algorithm:
304  CConfigFile cfg(CONFIG_FIL);
305  gridAlign.options.loadFromConfigFile(cfg, "grid-match");
306 
307  // load alternative config. for the metric maps:
308  if (cfg.sectionExists("metric_maps"))
309  {
310  map1_inits.loadFromConfigFile(cfg, "metric_maps");
311  map2_inits = map1_inits;
312  }
313  }
314 
315  // Dump params:
316  if (IS_VERBOSE)
317  {
318  gridAlign.options.dumpToConsole();
319  map1_inits.dumpToConsole();
320  }
321 
322  // Create the map with a points & grid-map within:
323  the_map1.setListOfMaps(map1_inits);
324  the_map2.setListOfMaps(map2_inits);
325 
328 
329  ASSERT_(grid1);
330  ASSERT_(grid2);
331 
332  // ---------------------------------------------
333  // Options: RANSAC
334  // ---------------------------------------------
335  gridAlign.options.ransac_SOG_sigma_m = grid1->getResolution() * 2;
336 
337  CSimpleMap map1, map2, map2noisy;
338 
339  // Load maps:
340  {
342  archiveFrom(f) >> map1;
343  }
344 
345  // If it's detect_test only load one map:
347  {
349  archiveFrom(f) >> map2;
350  }
351 
352  // Generate the_map1 now:
354 
355  size_t N1 = max(
356  40,
357  (int)round(grid1->getArea() * gridAlign.options.featsPerSquareMeter));
358 
359  COccupancyGridMapFeatureExtractor gridfeatextract;
360 
361  CLandmarksMap lm1;
362 
363  gridfeatextract.extractFeatures(
364  *grid1, lm1, N1, gridAlign.options.feature_descriptor,
366 
367  if (!NOSAVE && (is_match || is_detect_test)) // If it's only a
368  // SAVE_CORR_AND_NONCORR_DISTS
369  // test, save time
370  {
371  grid1->saveAsBitmapFile(format("%s/map1.png", RESULTS_DIR.c_str()));
372  grid1->saveAsBitmapFileWithLandmarks(
373  format("%s/map1_LM.png", RESULTS_DIR.c_str()), &lm1, true);
374  CImage img;
375  grid1->getAsImageFiltered(img);
376  img.saveToFile(format("%s/map1_filt.png", RESULTS_DIR.c_str()));
377  }
378 
379  {
380  CVectorFloat stats_covDet, stats_stdPhi;
381  CVectorFloat stats_errorXY, stats_errorPhi;
382  CVectorFloat stats_bruteErrorXY, stats_bruteErrorPhi;
383  CVectorFloat stats_GT_likelihood;
384  std::vector<uint32_t> overallGTcorrsFound;
385 
386  for (unsigned int iter = 0; iter < N_ITERS; iter++)
387  {
388  // Insert noise into the laser scans of map2:
389  // ------------------------------------------------
390  map2noisy = is_detect_test ? map1 : map2;
391 
392  for (unsigned int q = 0; q < map2noisy.size(); q++)
393  {
394  CPose3DPDF::Ptr PDF;
396 
397  map2noisy.get(q, PDF, SF);
398 
399  // If it's detect_test, translate the map2 by a fixed, known
400  // quantity:
401  if (is_detect_test)
402  {
404  gt = -gt;
405  PDF->changeCoordinatesReference(CPose3D(gt));
406  }
407 
408  if (NOISE_IN_LASER)
409  {
411  SF->getObservationByClass<CObservation2DRangeScan>();
412  if (obs)
413  {
414  for (unsigned int k = 0; k < obs->getScanSize(); k++)
415  {
416  float& v = obs->getScanRange(k);
418  0, STD_NOISE_LASER);
419  if (v < 0) v = 0;
420  }
421  }
422  } // end of NOISE_IN_LASER
423 
424  if (NOISE_IN_POSE)
425  {
426  CPosePDFGaussian::Ptr newPDF =
427  std::make_shared<CPosePDFGaussian>();
428  newPDF->copyFrom(*PDF);
429 
430  // Change the pose:
431  newPDF->mean.x_incr(
432  getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY));
433  newPDF->mean.y_incr(
434  getRandomGenerator().drawGaussian1D(0, STD_NOISE_XY));
435  newPDF->mean.phi_incr(getRandomGenerator().drawGaussian1D(
436  0, DEG2RAD(STD_NOISE_PHI)));
437  newPDF->mean.normalizePhi();
438 
439  // Change into the map:
440  map2noisy.set(q, newPDF, CSensoryFrame::Ptr());
441 
442  } // end of NOISE_IN_POSE
443  }
444 
445  the_map2.loadFromProbabilisticPosesAndObservations(map2noisy);
446 
447  size_t N2 = max(
448  40,
449  (int)round(
450  grid2->getArea() * gridAlign.options.featsPerSquareMeter));
451 
452  CLandmarksMap lm2;
453  gridfeatextract.extractFeatures(
454  *grid2, lm2, N2, gridAlign.options.feature_descriptor,
456 
457  if (!NOSAVE && (is_match || is_detect_test)) // If it's only a
458  // SAVE_CORR_AND_NONCORR_DISTS
459  // test, save time
460  {
461  // Save maps:
462  grid2->saveAsBitmapFile(format(
463  "%s/map2_noise_%f.png", RESULTS_DIR.c_str(), STD_NOISE_XY));
464  grid2->saveAsBitmapFileWithLandmarks(
465  format(
466  "%s/map2_LM_noise_%f.png", RESULTS_DIR.c_str(),
467  STD_NOISE_XY),
468  &lm2, true);
469  CImage img;
470  grid2->getAsImageFiltered(img);
471  img.saveToFile(format(
472  "%s/map2_filt_noise_%f.png", RESULTS_DIR.c_str(),
473  STD_NOISE_XY));
474  }
475 
476  // Only if the case of "save-corr-dists" we can do NOT align the
477  // maps, since we're not interested in that
478  if (is_match || is_detect_test)
479  {
480  // --------------------------
481  // DO ALIGN
482  // --------------------------
483  CPosePDF::Ptr parts = gridAlign.Align(
484  &the_map1, &the_map2, CPose2D(0, 0, 0), info);
485 
486  // Get the mean, or the best Gassian mean in the case of a SOG:
488  {
489  CPosePDFSOG::Ptr pdf_SOG =
490  std::dynamic_pointer_cast<CPosePDFSOG>(parts);
491  pdf_SOG->getMostLikelyCovarianceAndMean(
493  }
494  else
495  {
496  parts->getCovarianceAndMean(estimateCOV, estimateMean);
497  }
498 
499  float stdPhi = sqrt(estimateCOV(2, 2));
500 
501  const auto estimateCOV22 = estimateCOV.extractMatrix<2, 2>();
502  float stdXY = sqrt(estimateCOV22.det());
503 
504  float Axy = estimateMean.distance2DTo(GT_Ax, GT_Ay);
505  float Aphi =
507  float AxyBrute =
509  float AphiBrute = fabs(math::wrapToPi(
511 
513  "Done in %.03fms\n", 1000.0f * info.executionTime);
514 
515  MRPT_LOG_INFO_STREAM("Mean pose:\n\t " << estimateMean);
516  MRPT_LOG_INFO_STREAM("Estimate covariance::\n" << estimateCOV);
517 
518  // Save particles:
519  if (IS_CLASS(*parts, CPosePDFParticles))
520  {
521  CPosePDFParticles::Ptr partsPdf =
522  std::dynamic_pointer_cast<CPosePDFParticles>(parts);
523 
524  partsPdf->saveToTextFile(format(
525  "%s/particles_noise_%.03f.txt", RESULTS_DIR.c_str(),
526  STD_NOISE_XY));
527 
529  "Goodness: %.03f%%\n", 100 * info.goodness);
531  partsPdf->particlesCount() << " particles");
532  MRPT_LOG_INFO_STREAM("Covariance:\n\t " << estimateCOV);
533  }
534  else if (IS_CLASS(*parts, CPosePDFSOG))
535  {
536  CPosePDFSOG::Ptr pdf_SOG =
537  std::dynamic_pointer_cast<CPosePDFSOG>(parts);
539  "SoG has %u modes.", (unsigned int)pdf_SOG->size());
540 
541  pdf_SOG->normalizeWeights();
542  // pdf_SOG->saveToTextFile("_debug_SoG.txt");
543 
544  stats_GT_likelihood.push_back((float)pdf_SOG->evaluatePDF(
545  CPose2D(GT_Ax, GT_Ay, GT_Aphi_rad), true));
546 
547  if (f_out_log.fileOpenCorrectly())
548  {
549  f_out_log.printf("%% SOG_log_w x y phi \n");
550  for (size_t m = 0; m < pdf_SOG->size(); m++)
551  f_out_log.printf(
552  " %e %f %f %f\n", pdf_SOG->get(m).log_w,
553  pdf_SOG->get(m).mean.x(),
554  pdf_SOG->get(m).mean.y(),
555  pdf_SOG->get(m).mean.phi());
556  }
557 
558  if (SAVE_SOG_3DSCENE)
559  {
560  COpenGLScene scene3D;
561  opengl::CSetOfObjects::Ptr thePDF3D =
562  std::make_shared<opengl::CSetOfObjects>();
563  pdf_SOG->getAs3DObject(thePDF3D);
565  std::make_shared<opengl::CGridPlaneXY>(
566  -10, 10, -10, 10, 0, 1);
567  scene3D.insert(gridXY);
568  scene3D.insert(thePDF3D);
569  scene3D.saveToFile("_out_SoG.3Dscene");
570  }
571 
572  if (!SAVE_ICP_GOODNESS_FIL.empty())
573  {
576  SAVE_ICP_GOODNESS_FIL, true); // append & as column
577  }
578 
579  // Save all the maps overlap hypotheses:
581  {
583  size_t nNode;
584  CImage imgGrid1, imgCanvas;
585  grid1->resizeGrid(
586  min(grid1->getXMin(), -60.0f),
587  max(grid1->getXMax(), 60.0f),
588  min(-40.0f, grid1->getYMin()),
589  max(30.0f, grid1->getYMax()));
590  grid1->getAsImage(imgGrid1, true);
591  int imgGrid1LY = imgGrid1.getHeight();
592  const CPose2D nullPose(0, 0, 0);
593 
594  const TPoint2D q1(grid1->getXMin(), grid1->getYMin());
595  const TPoint2D q2(grid1->getXMin(), grid1->getYMax());
596  const TPoint2D q3(grid1->getXMax(), grid1->getYMax());
597  const TPoint2D q4(grid1->getXMax(), grid1->getYMin());
598 
599  const TPoint2D p1(grid2->getXMin(), grid2->getYMin());
600  const TPoint2D p2(grid2->getXMin(), grid2->getYMax());
601  const TPoint2D p3(grid2->getXMax(), grid2->getYMax());
602  const TPoint2D p4(grid2->getXMax(), grid2->getYMin());
603  for (nNode = 0, it = pdf_SOG->begin();
604  it != pdf_SOG->end(); it++, nNode++)
605  {
606  CPose2D x = it->mean;
607  const TPoint2D pp1(x + p1);
608  const TPoint2D pp2(x + p2);
609  const TPoint2D pp3(x + p3);
610  const TPoint2D pp4(x + p4);
611 
612  // Draw the background = the_map1:
613  imgCanvas = imgGrid1;
614 
615  // Draw the overlaped the_map2:
616  imgCanvas.line(
617  grid1->x2idx(pp1.x),
618  imgGrid1LY - 1 - grid1->y2idx(pp1.y),
619  grid1->x2idx(pp2.x),
620  imgGrid1LY - 1 - grid1->y2idx(pp2.y),
621  TColor::black());
622  imgCanvas.line(
623  grid1->x2idx(pp2.x),
624  imgGrid1LY - 1 - grid1->y2idx(pp2.y),
625  grid1->x2idx(pp3.x),
626  imgGrid1LY - 1 - grid1->y2idx(pp3.y),
627  TColor::black());
628  imgCanvas.line(
629  grid1->x2idx(pp3.x),
630  imgGrid1LY - 1 - grid1->y2idx(pp3.y),
631  grid1->x2idx(pp4.x),
632  imgGrid1LY - 1 - grid1->y2idx(pp4.y),
633  TColor::black());
634  imgCanvas.line(
635  grid1->x2idx(pp4.x),
636  imgGrid1LY - 1 - grid1->y2idx(pp4.y),
637  grid1->x2idx(pp1.x),
638  imgGrid1LY - 1 - grid1->y2idx(pp1.y),
639  TColor::black());
640 
641  imgCanvas.saveToFile(format(
642  "%s/_OVERLAP_MAPS_SOG_MODE_%04u.png",
643  RESULTS_DIR.c_str(), (unsigned int)nNode));
644 
645  // Save as 3D scene:
646  COpenGLScene scene;
647  CSetOfObjects::Ptr obj1 =
648  std::make_shared<CSetOfObjects>();
649  the_map1.getAs3DObject(obj1);
650  CSetOfObjects::Ptr obj2 =
651  std::make_shared<CSetOfObjects>();
652  the_map2.getAs3DObject(obj2);
653 
654  obj2->setPose(x);
655 
656  scene.insert(obj1);
657  scene.insert(obj2);
658 
659  // Add also the borders of the maps:
660  CSetOfLines::Ptr lines =
661  std::make_shared<CSetOfLines>();
662  lines->setLineWidth(3);
663  lines->setColor(0, 0, 1);
664 
665  lines->appendLine(q1.x, q1.y, 0, q2.x, q2.y, 0);
666  lines->appendLine(q2.x, q2.y, 0, q3.x, q3.y, 0);
667  lines->appendLine(q3.x, q3.y, 0, q4.x, q4.y, 0);
668  lines->appendLine(q4.x, q4.y, 0, q1.x, q1.y, 0);
669 
670  lines->appendLine(pp1.x, pp1.y, 0, pp2.x, pp2.y, 0);
671  lines->appendLine(pp2.x, pp2.y, 0, pp3.x, pp3.y, 0);
672  lines->appendLine(pp3.x, pp3.y, 0, pp4.x, pp4.y, 0);
673  lines->appendLine(pp4.x, pp4.y, 0, pp1.x, pp1.y, 0);
674 
675  scene.insert(lines);
676 
677  scene.saveToFile(format(
678  "%s/_OVERLAP_MAPS_SOG_MODE_%04u.3Dscene",
679  RESULTS_DIR.c_str(), (unsigned int)nNode));
680  }
681 
682  } // end SAVE_SOG_ALL
683 
684 #ifdef SAVE_SOG_GRID
685  // Save grid evaluation of the SOG:
686  CMatrixF gridLimits(1, 4);
687  gridLimits(0, 0) = estimateMean.x - 0.10f;
688  gridLimits(0, 1) = estimateMean.x + 0.10f,
689  gridLimits(0, 2) = estimateMean.y - 0.10f;
690  gridLimits(0, 3) = estimateMean.y + 0.10f;
691  gridLimits.saveToTextFile(format(
692  "%s/SOG_grid_limits_noise_%f.txt", RESULTS_DIR.c_str(),
693  STD_NOISE_XY));
694 
695  CMatrixD evalGrid;
696  pdf_SOG->evaluatePDFInArea(
697  gridLimits(0, 0), gridLimits(0, 1), gridLimits(0, 2),
698  gridLimits(0, 3), 0.002f, 0, evalGrid,
699  true // Sum over all phis
700  );
701 
702  evalGrid.saveToTextFile(format(
703  "%s/SOG_grid_noise_%f.txt", RESULTS_DIR.c_str(),
704  STD_NOISE_XY));
705 #endif
706  } // end if is SOG
707 
708  // STATS:
709  stats_covDet.push_back(stdXY);
710  stats_stdPhi.push_back(stdPhi);
711  stats_errorXY.push_back(Axy);
712  stats_errorPhi.push_back(Aphi);
713  stats_bruteErrorXY.push_back(AxyBrute);
714  stats_bruteErrorPhi.push_back(AphiBrute);
715 
716  } // end if we really do align
717 
718  // Save the descriptor distances for corresponding and
719  // non-corresponding features
720  // (only known if we are in the "is_detect_test" mode!)
722  {
723  MRPT_LOG_INFO_STREAM("Generating coor & no.corr distances...");
724 
725  const bool SAVE_ALSO_COORS_DEBUG_MAPS = false;
726 
727  CLandmarksMap::Ptr lmap1 = info.landmarks_map1;
728  CLandmarksMap::Ptr lmap2 = info.landmarks_map2;
729 
730  // only for the case of non "--match":
731  if (!lmap1 && !lmap2)
732  {
733  lmap1 = CLandmarksMap::Create();
734  lmap2 = CLandmarksMap::Create();
735 
736  gridfeatextract.extractFeatures(
737  *grid1, *lmap1, N1,
738  gridAlign.options.feature_descriptor,
740  gridfeatextract.extractFeatures(
741  *grid2, *lmap2, N2,
742  gridAlign.options.feature_descriptor,
744  }
745 
746  ASSERT_(lmap1 && lmap2);
747 
748  // GT transformation:
749  const CPose2D GT_Ap(GT_Ax, GT_Ay, GT_Aphi_rad);
750  TMatchingPairList gt_corrs;
751 
752  CFileOutputStream fout_CORR("GT_EXP_CORR.txt", true);
753  CFileOutputStream fout_NCORR("GT_EXP_NCORR.txt", true);
754 
755  // Compute the distances:
756  for (size_t i1 = 0; i1 < lmap1->landmarks.size(); i1++)
757  {
758  // Distances in descriptor space
759  CVectorDouble D(lmap2->landmarks.size());
760  // Distances in (x,y)
761  CVectorDouble dErrs(lmap2->landmarks.size());
762  size_t i2;
763 
764  const CLandmark* l1 = lmap1->landmarks.get(i1);
765 
766  for (i2 = 0; i2 < lmap2->landmarks.size(); i2++)
767  {
768  CLandmark* l2 = lmap2->landmarks.get(i2);
769 
770  CPoint2D P1 = CPoint2D(l1->pose_mean);
771  CPoint2D P2 = GT_Ap + CPoint2D(l2->pose_mean);
772 
773  const double dErr = P1.distanceTo(P2);
774  dErrs[i2] = dErr;
775 
776  ASSERT_(!l1->features.empty());
777  ASSERT_(!l2->features.empty());
778 
779  D[i2] = l1->features[0].descriptorDistanceTo(
780  l2->features[0]);
781  }
782 
783  size_t best_match = 0;
784  dErrs.minCoeff(best_match);
785  double MIN_DESCR_DIST = mrpt::math::minimum(D);
786  if (dErrs[best_match] < 0.20)
787  {
788  CLandmark* l2 = lmap2->landmarks.get(best_match);
789  gt_corrs.push_back(TMatchingPair(
790  i1, best_match, l1->pose_mean.x, l1->pose_mean.y,
791  l1->pose_mean.z, l2->pose_mean.x, l2->pose_mean.y,
792  l2->pose_mean.z));
793  }
794  else
795  best_match = (unsigned int)(-1);
796 
797  // double m,s; // Mean & Std:
798  // mrpt::math::meanAndStd(D,m,s);
799  // D = Abs( (D-m)/s );
800  // The output files:
801  for (i2 = 0; i2 < lmap2->landmarks.size(); i2++)
802  {
803  if (i2 == best_match)
804  fout_CORR.printf(
805  "%f %f\n", D[i2], D[i2] - MIN_DESCR_DIST);
806  else
807  fout_NCORR.printf(
808  "%f %f\n", D[i2], D[i2] - MIN_DESCR_DIST);
809  }
810 
811  // mrpt::system::pause();
812  } // end for i1
813 
814  MRPT_LOG_INFO_STREAM("GT corrs found: " << gt_corrs.size());
815  overallGTcorrsFound.push_back(gt_corrs.size());
816  if (SAVE_ALSO_COORS_DEBUG_MAPS)
817  {
818  COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
819  "GT_corrs.png", grid1.get(), grid2.get(), gt_corrs);
821  }
822  }
823 
824  } // end iter
825 
826  f_log.printf(
827  "%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %e %f %f\n",
829  math::mean(stats_covDet), math::stddev(stats_covDet),
830  math::mean(stats_stdPhi), math::stddev(stats_stdPhi),
831  math::mean(stats_errorXY), math::stddev(stats_errorXY),
832  math::mean(stats_errorPhi), math::stddev(stats_errorPhi),
833  math::mean(stats_bruteErrorXY), math::stddev(stats_bruteErrorXY),
834  math::mean(stats_bruteErrorPhi), math::stddev(stats_bruteErrorPhi),
835  math::mean(stats_GT_likelihood), math::mean(overallGTcorrsFound),
836  math::stddev(overallGTcorrsFound));
837 
838  } // For each noise
839 
840  MRPT_END
841 }
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.
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
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
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:1123
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
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
T x
X,Y coordinates.
Definition: TPoint2D.h:25
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::img::TColorf color
Color of points.
Definition: CPointsMap.h:329
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.
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:855
STL namespace.
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:165
mrpt::system::COutputLogger COutputLogger
T::Ptr mapByClass(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 ...
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.
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:592
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...
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
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)
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
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:441
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.
TCLAP::CmdLine cmd("system_control_rate_timer_example")
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.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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
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
#define MRPT_END
Definition: exceptions.h:245
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
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:13
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:56
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:329
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:198
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.
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...
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 numeric std::vector as a plain-text file compatible wi...
#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:148
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:235
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 2.0.5 Git: fc2d69e97 Fri Jul 10 18:20:57 2020 +0200 at vie jul 10 18:30:13 CEST 2020