MRPT  2.0.3
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>
22 #include <mrpt/maps/CSimpleMap.h>
25 #include <mrpt/math/wrap2pi.h>
26 #include <mrpt/obs/CSensoryFrame.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 
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;
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 }
CSetOfLines.h
mrpt::config
Definition: config/CConfigFile.h:14
os.h
filesystem.h
ops_containers.h
apps-precomp.h
mrpt::apps::CGridMapAlignerApp::GT_Aphi_rad
double GT_Aphi_rad
Definition: CGridMapAlignerApp.h:67
mrpt::config::CLoadableOptions::dumpToConsole
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
Definition: CLoadableOptions.cpp:43
mrpt::slam::CGridMapAligner::TReturnInfo::noRobustEstimation
mrpt::poses::CPose2D noRobustEstimation
The "brute" estimation from using all the available correspondences, provided just for comparison pur...
Definition: CGridMapAligner.h:146
CPoint2D.h
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
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.
Definition: CMetricMapsAlignmentAlgorithm.cpp:22
MRPT_LOG_INFO_FMT
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:463
mrpt::maps::COccupancyGridMap2D::TInsertionOptions::maxDistanceInsertion
float maxDistanceInsertion
The largest distance at which cells will be updated (Default 15 meters)
Definition: COccupancyGridMap2D.h:496
mrpt::apps::CGridMapAlignerApp::fil_grid1
std::string fil_grid1
Definition: CGridMapAlignerApp.h:61
mrpt::io::CFileStream
This CStream derived class allow using a file as a read/write binary stream, creating it if the file ...
Definition: CFileStream.h:37
mrpt::math::minimum
CONTAINER::Scalar minimum(const CONTAINER &v)
Definition: ops_containers.h:143
mrpt::io
Definition: img/CImage.h:24
mrpt::maps::CMultiMetricMap::mapByClass
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Definition: CMultiMetricMap.h:156
mrpt::maps::COccupancyGridMap2D::TMapDefinition::resolution
float resolution
Definition: COccupancyGridMap2D.h:1152
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::opengl::COpenGLScene::insert
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).
Definition: COpenGLScene.cpp:177
vector_loadsave.h
mrpt::opengl::CGridPlaneXY::Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
Definition: CGridPlaneXY.h:31
mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations
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
mrpt::slam::COccupancyGridMapFeatureExtractor
A class for detecting features from occupancy grid maps.
Definition: COccupancyGridMapFeatureExtractor.h:32
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
CMultiMetricMap.h
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::apps::CGridMapAlignerApp::GT_Ay
double GT_Ay
Definition: CGridMapAlignerApp.h:67
mrpt::maps::CSimplePointsMap::TMapDefinition::renderOpts
mrpt::maps::CPointsMap::TRenderOptions renderOpts
Rendering as 3D object options.
Definition: CSimplePointsMap.h:141
mrpt::apps::CGridMapAlignerApp::estimateCOV
mrpt::math::CMatrixDouble33 estimateCOV
Definition: CGridMapAlignerApp.h:81
mrpt::apps::CGridMapAlignerApp::SAVE_SOG_ALL_MAPS_OVERLAP_HYPOTHESES
bool SAVE_SOG_ALL_MAPS_OVERLAP_HYPOTHESES
Definition: CGridMapAlignerApp.h:48
mrpt::math::MatrixBase::extractMatrix
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > extractMatrix(const int start_row=0, const int start_col=0) const
Definition: MatrixBase.h:247
mrpt::math::TPoint2D_< double >
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::slam::CGridMapAligner::TConfigParams::feature_detector_options
mrpt::vision::CFeatureExtraction::TOptions feature_detector_options
All the parameters for the feature detector.
Definition: CGridMapAligner.h:77
mrpt::apps::CGridMapAlignerApp::CONFIG_FIL
std::string CONFIG_FIL
Definition: CGridMapAlignerApp.h:63
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints
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
CFileOutputStream.h
mrpt::apps::CGridMapAlignerApp::STD_NOISE_PHI
double STD_NOISE_PHI
Definition: CGridMapAlignerApp.h:65
mrpt::slam::TMetricMapAlignmentResult::executionTime
double executionTime
Definition: CMetricMapsAlignmentAlgorithm.h:31
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::obs::CObservation2DRangeScan::getScanRange
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
Definition: CObservation2DRangeScan.cpp:496
IS_CLASS
#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
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:244
CConfigFile.h
mrpt::maps::CSimplePointsMap::TMapDefinition
Definition: CSimplePointsMap.h:135
CFileGZOutputStream.h
mrpt::apps::CGridMapAlignerApp::OUTPUT_FIL
std::string OUTPUT_FIL
Definition: CGridMapAlignerApp.h:62
mrpt::slam::CGridMapAligner::options
mrpt::slam::CGridMapAligner::TConfigParams options
wrap2pi.h
mrpt::apps::CGridMapAlignerApp::initialize
void initialize(int argc, const char **argv)
Initializes the application from CLI parameters.
Definition: CGridMapAlignerApp.cpp:46
mrpt::apps::CGridMapAlignerApp::NOISE_IN_LASER
bool NOISE_IN_LASER
Definition: CGridMapAlignerApp.h:69
mrpt::slam::CGridMapAligner::TConfigParams::ransac_SOG_sigma_m
float ransac_SOG_sigma_m
The square root of the uncertainty normalization variance var_m (see papers...)
Definition: CGridMapAligner.h:87
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::utils::COutputLogger
mrpt::system::COutputLogger COutputLogger
Definition: utils/COutputLogger.h:5
mrpt::maps::COccupancyGridMap2D::TMapDefinition::insertionOpts
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
Definition: COccupancyGridMap2D.h:1154
mrpt::io::CFileStream::fileOpenCorrectly
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
Definition: CFileStream.cpp:188
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
mrpt::apps::CGridMapAlignerApp::SAVE_CORR_AND_NONCORR_DISTS
bool SAVE_CORR_AND_NONCORR_DISTS
Definition: CGridMapAlignerApp.h:49
mrpt::slam::CGridMapAligner::TConfigParams::loadFromConfigFile
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.
Definition: CGridMapAligner.cpp:1128
mrpt::slam::CGridMapAligner::TReturnInfo::icp_goodness_all_sog_modes
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...
Definition: CGridMapAligner.h:181
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
random.h
mrpt::maps::COccupancyGridMap2D::TInsertionOptions::maxOccupancyUpdateCertainty
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating cell with a bayesian approach (default 0....
Definition: COccupancyGridMap2D.h:499
mrpt::obs::CObservation2DRangeScan::Ptr
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
Definition: CObservation2DRangeScan.h:56
mrpt::apps::CGridMapAlignerApp::is_detect_test
bool is_detect_test
Definition: CGridMapAlignerApp.h:58
mrpt::slam::COccupancyGridMapFeatureExtractor::extractFeatures
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...
Definition: COccupancyGridMapFeatureExtractor.cpp:80
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:92
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
mrpt::math::CMatrixF
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
mrpt::apps::CGridMapAlignerApp::SKIP_ICP_STAGE
bool SKIP_ICP_STAGE
Definition: CGridMapAlignerApp.h:52
mrpt::maps::CSimplePointsMap::TMapDefinition::insertionOpts
mrpt::maps::CPointsMap::TInsertionOptions insertionOpts
Observations insertion options.
Definition: CSimplePointsMap.h:137
mrpt::apps::CGridMapAlignerApp::STD_NOISE_LASER
double STD_NOISE_LASER
Definition: CGridMapAlignerApp.h:66
mrpt::maps::COccupancyGridMap2D::TMapDefinition
Definition: COccupancyGridMap2D.h:1149
mrpt::apps::CGridMapAlignerApp::RESULTS_DIR
std::string RESULTS_DIR
Definition: CGridMapAlignerApp.h:60
mrpt::apps::CGridMapAlignerApp::MOST_LIKELY_SOG_MODE_ONLY
bool MOST_LIKELY_SOG_MODE_ONLY
Definition: CGridMapAlignerApp.h:53
mrpt::img
Definition: CCanvas.h:16
mrpt::io::fomWrite
@ fomWrite
Definition: CFileStream.h:26
mrpt::apps::CGridMapAlignerApp::N_ITERS
unsigned int N_ITERS
Definition: CGridMapAlignerApp.h:72
mrpt::poses::CPoseOrPoint::distance2DTo
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
mrpt::slam::CGridMapAligner::TConfigParams::featsPerSquareMeter
float featsPerSquareMeter
Features extraction from grid map: How many features to extract.
Definition: CGridMapAligner.h:102
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:26
mrpt::maps::CLandmarksMap::Ptr
std::shared_ptr< mrpt::maps ::CLandmarksMap > Ptr
Definition: CLandmarksMap.h:76
mrpt::maps::CLandmark
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::maps::CLandmark::features
std::vector< mrpt::vision::CFeature > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::opengl::CSetOfLines::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfLines > Ptr
Definition: CSetOfLines.h:35
mrpt::apps::CGridMapAlignerApp::fil_grid2
std::string fil_grid2
Definition: CGridMapAlignerApp.h:61
mrpt::img::TColorf
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile
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.
Definition: TMetricMapInitializer.cpp:69
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::img::CImage::line
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:1117
mrpt::maps::CSimpleMap::size
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
mrpt::maps::CSimpleMap::set
void set(size_t index, const mrpt::poses::CPose3DPDF::Ptr &in_posePDF, const mrpt::obs::CSensoryFrame::Ptr &in_SF)
Changes the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:73
mrpt::system::COutputLogger::setMinLoggingLevel
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
Definition: COutputLogger.cpp:144
mrpt::maps::CSimpleMap::get
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:56
mrpt::apps
Definition: BaseAppDataSource.h:15
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
mrpt::slam::CGridMapAligner::TReturnInfo::goodness
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CGridMapAligner.h:140
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::system::deleteFiles
void deleteFiles(const std::string &s)
Delete one or more files, especified by the (optional) path and the file name (including '?...
Definition: filesystem.cpp:190
mrpt::maps::CLandmark::pose_mean
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
mrpt::system::MRPT_getCompilationDate
std::string MRPT_getCompilationDate()
Returns the MRPT source code timestamp, according to the Reproducible-Builds specifications: https://...
Definition: os.cpp:154
mrpt::math::MatrixVectorBase::saveToTextFile
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.
Definition: MatrixVectorBase_impl.h:159
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::poses::CPosePDFParticles
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,...
Definition: CPosePDFParticles.h:30
mrpt::serialization
Definition: aligned_serialization.h:13
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::system::extractFileExtension
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
mrpt::slam::CGridMapAligner
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map,...
Definition: CGridMapAligner.h:40
mrpt::poses::CPosePDFSOG::iterator
CListGaussianModes::iterator iterator
Definition: CPosePDFSOG.h:67
mrpt::io::CFileOutputStream
This CStream derived class allow using a file as a write-only, binary stream.
Definition: io/CFileOutputStream.h:22
mrpt::math::CVectorDynamic
Template for column vectors of dynamic size, compatible with Eigen.
Definition: CVectorDynamic.h:31
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::io::CFileStream::open
bool open(const std::string &fileName, TFileOpenModes mode=fomRead|fomWrite)
Opens the file, returning true on success.
Definition: CFileStream.cpp:56
mrpt::slam::CGridMapAligner::TConfigParams::methodSelection
TAlignerMethod methodSelection
The aligner method:
Definition: CGridMapAligner.h:69
mrpt::img::CImage::saveToFile
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:330
mrpt::apps::CGridMapAlignerApp::run
void run()
Runs with the current parameter set.
Definition: CGridMapAlignerApp.cpp:211
mrpt::apps::CGridMapAlignerApp::NOISE_IN_POSE
bool NOISE_IN_POSE
Definition: CGridMapAlignerApp.h:70
CSensoryFrame.h
mrpt::maps::CMultiMetricMap::setListOfMaps
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 ...
Definition: CMultiMetricMap.cpp:113
mrpt::math::stddev
double stddev(const VECTORLIKE &v, bool unbiased=true)
Computes the standard deviation of a vector.
Definition: ops_containers.h:359
mrpt::poses::CPosePDFGaussian::Ptr
std::shared_ptr< mrpt::poses ::CPosePDFGaussian > Ptr
Definition: CPosePDFGaussian.h:30
mrpt::opengl::COpenGLScene::saveToFile
bool saveToFile(const std::string &fil) const
Saves the scene to a 3Dscene file, loadable by the application SceneViewer3D.
Definition: COpenGLScene.cpp:293
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
mrpt::slam::CGridMapAligner::TReturnInfo
The ICP algorithm return information.
Definition: CGridMapAligner.h:132
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
mrpt::math::MatrixVectorBase::minCoeff
Scalar minCoeff() const
Minimum value in the matrix/vector.
Definition: MatrixVectorBase_impl.h:357
mrpt::apps::CGridMapAlignerApp::GT_Ax
double GT_Ax
Definition: CGridMapAlignerApp.h:67
mrpt::img::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CPosePDFGaussian.h
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
mrpt::apps::CGridMapAlignerApp::estimateMean
mrpt::poses::CPose2D estimateMean
Definition: CGridMapAlignerApp.h:80
mrpt::apps::CGridMapAlignerApp::is_match
bool is_match
Definition: CGridMapAlignerApp.h:58
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::TSetOfMetricMapInitializers::push_back
void push_back(const MAP_DEFINITION &o)
Definition: TMetricMapInitializer.h:97
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:31
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
CFileGZInputStream.h
mrpt::apps::CGridMapAlignerApp::SAVE_ICP_GOODNESS_FIL
std::string SAVE_ICP_GOODNESS_FIL
Definition: CGridMapAlignerApp.h:55
mrpt::config::CConfigFileBase::sectionExists
bool sectionExists(const std::string &section_name) const
Checks if a given section exists (name is case insensitive)
Definition: CConfigFileBase.cpp:205
mrpt::random::CRandomGenerator::drawGaussian1D
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:194
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::CVectorDynamic::push_back
void push_back(const T &val)
Definition: CVectorDynamic.h:197
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::poses::CPosePDFParticles::Ptr
std::shared_ptr< mrpt::poses ::CPosePDFParticles > Ptr
Definition: CPosePDFParticles.h:40
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
gui.h
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
MRPT_LOG_INFO_STREAM
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:473
mrpt::slam::CGridMapAligner::TReturnInfo::landmarks_map1
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences")
Definition: CGridMapAligner.h:161
mrpt::apps::CGridMapAlignerApp::STD_NOISE_XY
double STD_NOISE_XY
Definition: CGridMapAlignerApp.h:65
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
mrpt::maps::COccupancyGridMap2D::Ptr
std::shared_ptr< mrpt::maps ::COccupancyGridMap2D > Ptr
Definition: COccupancyGridMap2D.h:57
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::system::COutputLogger::getMinLoggingLevel
VerbosityLevel getMinLoggingLevel() const
Definition: system/COutputLogger.h:201
mrpt::io::CStream::printf
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
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::CMultiMetricMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CMultiMetricMap.cpp:261
CGridMapAlignerApp.h
mrpt::poses::CPosePDFSOG::Ptr
std::shared_ptr< mrpt::poses ::CPosePDFSOG > Ptr
Definition: CPosePDFSOG.h:36
CFileStream.h
mrpt::maps::CPointsMap::TRenderOptions::color
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:329
mrpt::apps::CGridMapAlignerApp::SAVE_SOG_3DSCENE
bool SAVE_SOG_3DSCENE
Definition: CGridMapAlignerApp.h:47
CArchive.h
mrpt::poses::CPoint2D
A class used to store a 2D point.
Definition: CPoint2D.h:32
CSimpleMap.h
CSimplePointsMap.h
mrpt::system::createDirectory
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
mrpt::system::vectorToTextFile
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.
Definition: system/src/vector_loadsave.cpp:19
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:41
CGridPlaneXY.h
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::typemeta::TEnumType::name2value
static ENUMTYPE name2value(const std::string &name)
Gives the numerical name for a given enum text name.
Definition: TEnumType.h:100
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
CPosePDFParticles.h
mrpt::slam::CGridMapAligner::TReturnInfo::landmarks_map2
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
Definition: CGridMapAligner.h:161
mrpt::slam::CGridMapAligner::TConfigParams::feature_descriptor
mrpt::vision::TDescriptorType feature_descriptor
The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF,...
Definition: CGridMapAligner.h:73
mrpt::apps::CGridMapAlignerApp::CGridMapAlignerApp
CGridMapAlignerApp()
Definition: CGridMapAlignerApp.cpp:41
mrpt::apps::CGridMapAlignerApp::aligner_method
mrpt::slam::CGridMapAligner::TAlignerMethod aligner_method
Definition: CGridMapAlignerApp.h:74
mrpt::system::MRPT_getVersion
std::string MRPT_getVersion()
Returns a string describing the MRPT version.
Definition: os.cpp:187
mrpt::poses::CPose2D::changeCoordinatesReference
void changeCoordinatesReference(const CPose2D &p)
makes: this = p (+) this
Definition: CPose2D.h:304
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system::pause
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:430
datetime.h
mrpt::poses::CPosePDFSOG
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFSOG.h:34
mrpt::apps::CGridMapAlignerApp::NOSAVE
bool NOSAVE
Definition: CGridMapAlignerApp.h:51
mrpt::apps::CGridMapAlignerApp::IS_VERBOSE
bool IS_VERBOSE
Definition: CGridMapAlignerApp.h:50



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020