Main MRPT website > C++ reference for MRPT 1.9.9
CGridMapAligner.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/slam/CGridMapAligner.h>
13 #include <mrpt/random.h>
14 #include <mrpt/poses/CPoint2DPDFGaussian.h>
15 #include <mrpt/poses/CPosePDFGaussian.h>
16 #include <mrpt/poses/CPose3DPDFGaussian.h>
17 #include <mrpt/math/ops_containers.h>
18 #include <mrpt/math/distributions.h>
19 #include <mrpt/math/geometry.h>
20 #include <mrpt/system/filesystem.h>
21 #include <mrpt/utils/CEnhancedMetaFile.h>
22 
23 #include <mrpt/maps/COccupancyGridMap2D.h>
24 #include <mrpt/maps/CMultiMetricMap.h>
25 #include <mrpt/slam/CICP.h>
26 #include <mrpt/maps/CLandmarksMap.h>
27 #include <mrpt/tfest/se2.h>
28 
29 using namespace mrpt::math;
30 using namespace mrpt::slam;
31 using namespace mrpt::maps;
32 using namespace mrpt::utils;
33 using namespace mrpt::poses;
34 using namespace mrpt::random;
35 using namespace mrpt::vision;
36 using namespace std;
37 
38 /*---------------------------------------------------------------
39 The method for aligning a pair of 2D points map.
40 * The meaning of some parameters are implementation dependent,
41 * so look for derived classes for instructions.
42 * The target is to find a PDF for the pose displacement between
43 * maps, <b>thus the pose of m2 relative to m1</b>. This pose
44 * is returned as a PDF rather than a single value.
45 *
46 * \param m1 [IN] The first map
47 * \param m2 [IN] The second map. The pose of this map respect to m1
48 is to be estimated.
49 * \param grossEst [IN] IGNORED
50 * \param runningTime [OUT] A pointer to a container for obtaining the
51 algorithm running time in seconds, or nullptr if you don't need it.
52 * \param info [OUT] See derived classes for details, or nullptr if it
53 isn't needed.
54 *
55 * \sa CPointsMapAlignmentAlgorithm
56  ---------------------------------------------------------------*/
57 CPosePDF::Ptr CGridMapAligner::AlignPDF(
58  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
59  const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
60  void* info)
61 {
62  MRPT_START
63 
64  switch (options.methodSelection)
65  {
66  case CGridMapAligner::amCorrelation:
67  return AlignPDF_correlation(
68  mm1, mm2, initialEstimationPDF, runningTime, info);
69 
70  case CGridMapAligner::amModifiedRANSAC:
71  case CGridMapAligner::amRobustMatch:
72  // The same function has an internal switch for the specific method:
73  return AlignPDF_robustMatch(
74  mm1, mm2, initialEstimationPDF, runningTime, info);
75 
76  default:
77  THROW_EXCEPTION("Wrong value found in 'options.methodSelection'!!");
78  }
79 
80  MRPT_END
81 }
82 
83 bool myVectorOrder(const pair<size_t, float>& o1, const pair<size_t, float>& o2)
84 {
85  return o1.second < o2.second;
86 }
87 
88 /*---------------------------------------------------------------
89  AlignPDF_robustMatch
90 ---------------------------------------------------------------*/
91 CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch(
92  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
93  const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
94  void* info)
95 {
96  MRPT_START
97 
98  ASSERT_(
99  options.methodSelection == CGridMapAligner::amRobustMatch ||
100  options.methodSelection == CGridMapAligner::amModifiedRANSAC)
101 
102  TReturnInfo outInfo;
103  mrpt::utils::TMatchingPairList& correspondences =
104  outInfo.correspondences; // Use directly this placeholder to save 1
105  // variable & 1 copy.
106  mrpt::utils::TMatchingPairList largestConsensusCorrs;
107 
108  CTicTac* tictac = nullptr;
109 
110  CPose2D grossEst = initialEstimationPDF.mean;
111 
112  CLandmarksMap::Ptr lm1(new CLandmarksMap());
113  CLandmarksMap::Ptr lm2(new CLandmarksMap());
114 
115  std::vector<size_t> idxs1, idxs2;
116  std::vector<size_t>::iterator it1, it2;
117 
118  // Asserts:
119  // -----------------
120  const CMultiMetricMap* multimap1 = nullptr;
121  const CMultiMetricMap* multimap2 = nullptr;
122  const COccupancyGridMap2D* m1 = nullptr;
123  const COccupancyGridMap2D* m2 = nullptr;
124 
125  if (IS_CLASS(mm1, CMultiMetricMap) && IS_CLASS(mm2, CMultiMetricMap))
126  {
127  multimap1 = static_cast<const CMultiMetricMap*>(mm1);
128  multimap2 = static_cast<const CMultiMetricMap*>(mm2);
129 
130  ASSERT_(multimap1->m_gridMaps.size() && multimap1->m_gridMaps[0]);
131  ASSERT_(multimap2->m_gridMaps.size() && multimap2->m_gridMaps[0]);
132 
133  m1 = multimap1->m_gridMaps[0].get();
134  m2 = multimap2->m_gridMaps[0].get();
135  }
136  else if (
137  IS_CLASS(mm1, COccupancyGridMap2D) &&
138  IS_CLASS(mm2, COccupancyGridMap2D))
139  {
140  m1 = static_cast<const COccupancyGridMap2D*>(mm1);
141  m2 = static_cast<const COccupancyGridMap2D*>(mm2);
142  }
143  else
144  THROW_EXCEPTION(
145  "Metric maps must be of classes COccupancyGridMap2D or "
146  "CMultiMetricMap")
147 
148  ASSERTMSG_(
149  m1->getResolution() == m2->getResolution(),
150  mrpt::format(
151  "Different resolutions for m1, m2:\n"
152  "\tres(m1) = %f\n\tres(m2) = %f\n",
153  m1->getResolution(), m2->getResolution()));
154 
155  // The algorithm output auxiliar info:
156  // -------------------------------------------------
157  outInfo.goodness = 1.0f;
158 
159  outInfo.landmarks_map1 = lm1;
160  outInfo.landmarks_map2 = lm2;
161 
162  // The PDF to estimate:
163  // ------------------------------------------------------
164  CPosePDFSOG::Ptr pdf_SOG = mrpt::make_aligned_shared<CPosePDFSOG>();
165 
166  // Extract features from grid-maps:
167  // ------------------------------------------------------
168  const size_t N1 = std::max(
169  40, mrpt::utils::round(m1->getArea() * options.featsPerSquareMeter));
170  const size_t N2 = std::max(
171  40, mrpt::utils::round(m2->getArea() * options.featsPerSquareMeter));
172 
173  m_grid_feat_extr.extractFeatures(
174  *m1, *lm1, N1, options.feature_descriptor,
175  options.feature_detector_options);
176  m_grid_feat_extr.extractFeatures(
177  *m2, *lm2, N2, options.feature_descriptor,
178  options.feature_detector_options);
179 
180  if (runningTime)
181  {
182  tictac = new CTicTac();
183  tictac->Tic();
184  }
185 
186  const size_t nLM1 = lm1->size();
187  const size_t nLM2 = lm2->size();
188 
189  // At least two landmarks at each map!
190  // ------------------------------------------------------
191  if (nLM1 < 2 || nLM2 < 2)
192  {
193  outInfo.goodness = 0;
194  }
195  else
196  {
197  //#define METHOD_FFT
198  //#define DEBUG_SHOW_CORRELATIONS
199 
200  // Compute correlation between landmarks:
201  // ---------------------------------------------
202  CMatrixFloat CORR(lm1->size(), lm2->size()), auxCorr;
203  CImage im1(1, 1, 1), im2(1, 1, 1); // Grayscale
204  CVectorFloat corr;
205  unsigned int corrsCount = 0;
206  std::vector<bool> hasCorr(nLM1, false);
207 
208  const double thres_max = options.threshold_max;
209  const double thres_delta = options.threshold_delta;
210 
211  // CDisplayWindowPlots::Ptr auxWin;
212  if (options.debug_show_corrs)
213  {
214  // auxWin = CDisplayWindowPlots::Ptr( new
215  // CDisplayWindowPlots("Individual corr.") );
216  std::cerr << "Warning: options.debug_show_corrs has no effect "
217  "since MRPT 0.9.1\n";
218  }
219 
220  for (size_t idx1 = 0; idx1 < nLM1; idx1++)
221  {
222  // CVectorFloat corrs_indiv;
223  vector<pair<size_t, float>> corrs_indiv; // (index, distance);
224  // Index is used to
225  // recover the original
226  // index after sorting.
227  vector<float> corrs_indiv_only;
228  corrs_indiv.reserve(nLM2);
229  corrs_indiv_only.reserve(nLM2);
230 
231  for (size_t idx2 = 0; idx2 < nLM2; idx2++)
232  {
233  float minDist;
234  minDist =
235  lm1->landmarks.get(idx1)->features[0]->descriptorDistanceTo(
236  *lm2->landmarks.get(idx2)->features[0]);
237 
238  corrs_indiv.push_back(std::make_pair(idx2, minDist));
239  corrs_indiv_only.push_back(minDist);
240  } // end for idx2
241 
242  // double corr_mean,corr_std;
243  // mrpt::math::meanAndStd(corrs_indiv_only,corr_mean,corr_std);
244  const double corr_best = mrpt::math::minimum(corrs_indiv_only);
245  // cout << "M: " << corr_mean << " std: " << corr_std << " best: "
246  // << corr_best << endl;
247 
248  // Sort the list and keep the N best features:
249  std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder);
250 
251  // const size_t nBestToKeep = std::min( (size_t)30,
252  // corrs_indiv.size() );
253  const size_t nBestToKeep = corrs_indiv.size();
254 
255  for (size_t w = 0; w < nBestToKeep; w++)
256  {
257  if (corrs_indiv[w].second <= thres_max &&
258  corrs_indiv[w].second <= (corr_best + thres_delta))
259  {
260  idxs1.push_back(idx1);
261  idxs2.push_back(corrs_indiv[w].first);
262  outInfo.correspondences_dists_maha.push_back(
263  TReturnInfo::TPairPlusDistance(
264  idx1, corrs_indiv[w].first, corrs_indiv[w].second));
265  }
266  }
267 
268 #if 0
269  if (options.debug_show_corrs)
270  {
271  auxWin->setWindowTitle(format("Corr: %i - rest", int(idx1)));
272 
273  auxWin->plot(corrs_indiv_only,".3","the_corr");
274 
275  CVectorFloat xs(2), ys(2);
276  xs[0]=0; xs[1]=corrs_indiv_only.size()+1;
277 
278  ys[0]=ys[1] = corr_best+thres_delta;
279  auxWin->plot(xs,ys,"g","the_thres");
280 
281  if (idx1==0) auxWin->axis_fit();
282  auxWin->waitForKey();
283  }
284 #endif
285 
286  } // end for idx1
287 
288  // Save an image with each feature and its matches:
289  if (options.save_feat_coors)
290  {
291  mrpt::system::deleteFilesInDirectory("grid_feats");
292  mrpt::system::createDirectory("grid_feats");
293 
294  std::map<size_t, std::set<size_t>> corrs_by_idx;
295  for (size_t l = 0; l < idxs1.size(); l++)
296  corrs_by_idx[idxs1[l]].insert(idxs2[l]);
297 
298  for (std::map<size_t, std::set<size_t>>::iterator it =
299  corrs_by_idx.begin();
300  it != corrs_by_idx.end(); ++it)
301  {
302  CMatrixFloat descriptor1;
303  lm1->landmarks.get(it->first)
304  ->features[0]
305  ->getFirstDescriptorAsMatrix(descriptor1);
306 
307  im1 = CImage(descriptor1, true);
308 
309  const size_t FEAT_W = im1.getWidth();
310  const size_t FEAT_H = im1.getHeight();
311  const size_t nF = it->second.size();
312 
313  CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF);
314  img_compose.filledRectangle(
315  0, 0, img_compose.getWidth() - 1,
316  img_compose.getHeight() - 1, TColor::black());
317 
318  img_compose.drawImage(5, 5, im1);
319 
320  size_t j;
321  std::set<size_t>::iterator it_j;
322  string fil =
323  format("grid_feats/_FEAT_MATCH_%03i", (int)it->first);
324 
325  for (j = 0, it_j = it->second.begin(); j < nF; ++j, ++it_j)
326  {
327  fil += format("_%u", static_cast<unsigned int>(*it_j));
328 
329  CMatrixFloat descriptor2;
330  lm2->landmarks.get(*it_j)
331  ->features[0]
332  ->getFirstDescriptorAsMatrix(descriptor2);
333  im2 = CImage(descriptor2, true);
334  img_compose.drawImage(
335  10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
336  }
337  fil += ".png";
338  img_compose.saveToFile(fil);
339  } // end for map
340  }
341 
342  // ------------------------------------------------------------------
343  // Create the list of correspondences from the lists: idxs1 & idxs2
344  // ------------------------------------------------------------------
345  correspondences.clear();
346  for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end();
347  ++it1, ++it2)
348  {
349  mrpt::utils::TMatchingPair mp;
350  mp.this_idx = *it1;
351  mp.this_x = lm1->landmarks.get(*it1)->pose_mean.x;
352  mp.this_y = lm1->landmarks.get(*it1)->pose_mean.y;
353  mp.this_z = lm1->landmarks.get(*it1)->pose_mean.z;
354 
355  mp.other_idx = *it2;
356  mp.other_x = lm2->landmarks.get(*it2)->pose_mean.x;
357  mp.other_y = lm2->landmarks.get(*it2)->pose_mean.y;
358  mp.other_z = lm2->landmarks.get(*it2)->pose_mean.z;
359  correspondences.push_back(mp);
360 
361  if (!hasCorr[*it1])
362  {
363  hasCorr[*it1] = true;
364  corrsCount++;
365  }
366  } // end for corres.
367 
368  outInfo.goodness = (2.0f * corrsCount) / (nLM1 + nLM2);
369 
370  // Compute the estimation using ALL the correspondences (NO ROBUST):
371  // ----------------------------------------------------------------------
372  mrpt::math::TPose2D noRobustEst;
373  if (!mrpt::tfest::se2_l2(correspondences, noRobustEst))
374  {
375  // There's no way to match the maps! e.g. no correspondences
376  outInfo.goodness = 0;
377  pdf_SOG->clear();
378  outInfo.sog1 = pdf_SOG;
379  outInfo.sog2 = pdf_SOG;
380  outInfo.sog3 = pdf_SOG;
381  }
382  else
383  {
384  outInfo.noRobustEstimation = mrpt::poses::CPose2D(noRobustEst);
385  MRPT_LOG_INFO(
386  mrpt::format(
387  "[CGridMapAligner] Overall estimation(%u corrs, total: "
388  "%u): (%.03f,%.03f,%.03fdeg)\n",
389  corrsCount, (unsigned)correspondences.size(),
390  outInfo.noRobustEstimation.x(),
391  outInfo.noRobustEstimation.y(),
392  RAD2DEG(outInfo.noRobustEstimation.phi())));
393 
394  // The list of SOG modes & their corresponding sub-sets of
395  // matchings:
396  typedef mrpt::aligned_containers<mrpt::utils::TMatchingPairList,
397  CPosePDFSOG::TGaussianMode>::map_t
398  TMapMatchingsToPoseMode;
399  TMapMatchingsToPoseMode sog_modes;
400 
401  // ---------------------------------------------------------------
402  // Now, we have to choose between the methods:
403  // - CGridMapAligner::amRobustMatch ("normal" RANSAC)
404  // - CGridMapAligner::amModifiedRANSAC
405  // ---------------------------------------------------------------
406  if (options.methodSelection == CGridMapAligner::amRobustMatch)
407  {
408  // ====================================================
409  // METHOD: "Normal" RANSAC
410  // ====================================================
411 
412  // RANSAC over the found correspondences:
413  // -------------------------------------------------
414  const unsigned int min_inliers =
415  options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2;
416 
417  mrpt::tfest::TSE2RobustParams tfest_params;
418  tfest_params.ransac_minSetSize = min_inliers;
419  tfest_params.ransac_maxSetSize = nLM1 + nLM2;
420  tfest_params.ransac_mahalanobisDistanceThreshold =
421  options.ransac_mahalanobisDistanceThreshold;
422  tfest_params.ransac_nSimulations = 0; // 0=auto
423  tfest_params.ransac_fuseByCorrsMatch = true;
424  tfest_params.ransac_fuseMaxDiffXY = 0.01;
425  tfest_params.ransac_fuseMaxDiffPhi = DEG2RAD(0.1);
426  tfest_params.ransac_algorithmForLandmarks = true;
427  tfest_params.probability_find_good_model =
428  options.ransac_prob_good_inliers;
429  tfest_params.verbose = false;
430 
431  mrpt::tfest::TSE2RobustResult tfest_result;
432  mrpt::tfest::se2_l2_robust(
433  correspondences, options.ransac_SOG_sigma_m, tfest_params,
434  tfest_result);
435 
436  ASSERT_(pdf_SOG);
437  *pdf_SOG = tfest_result.transformation;
438  largestConsensusCorrs = tfest_result.largestSubSet;
439 
440  // Simplify the SOG by merging close modes:
441  // -------------------------------------------------
442  size_t nB = pdf_SOG->size();
443  outInfo.sog1 = pdf_SOG;
444 
445  // Keep only the most-likely Gaussian mode:
446  CPosePDFSOG::TGaussianMode best_mode;
447  best_mode.log_w = -std::numeric_limits<double>::max();
448 
449  for (size_t n = 0; n < pdf_SOG->size(); n++)
450  {
451  const CPosePDFSOG::TGaussianMode& m = (*pdf_SOG)[n];
452 
453  if (m.log_w > best_mode.log_w) best_mode = m;
454  }
455 
456  pdf_SOG->clear();
457  pdf_SOG->push_back(best_mode);
458  outInfo.sog2 = pdf_SOG;
459 
460  MRPT_LOG_INFO_STREAM(
461  "[CGridMapAligner] amRobustMatch: "
462  << nB << " SOG modes reduced to " << pdf_SOG->size()
463  << " (most-likely) (min.inliers=" << min_inliers << ")\n");
464 
465  } // end of amRobustMatch
466  else
467  {
468  // ====================================================
469  // METHOD: "Modified" RANSAC
470  // ====================================================
471  mrpt::utils::TMatchingPairList all_corrs = correspondences;
472 
473  const size_t nCorrs = all_corrs.size();
474  ASSERT_(nCorrs >= 2)
475 
476  pdf_SOG->clear(); // Start with 0 Gaussian modes
477 
478  // Build a points-map for exploiting its KD-tree:
479  // ----------------------------------------------------
480  CSimplePointsMap lm1_pnts, lm2_pnts;
481  lm1_pnts.reserve(nLM1);
482  for (size_t i = 0; i < nLM1; i++)
483  lm1_pnts.insertPoint(lm1->landmarks.get(i)->pose_mean);
484  lm2_pnts.reserve(nLM2);
485  for (size_t i = 0; i < nLM2; i++)
486  lm2_pnts.insertPoint(lm2->landmarks.get(i)->pose_mean);
487 
488  // RANSAC loop
489  // ---------------------
490  const size_t minInliersTOaccept =
491  round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2));
492  // Set an initial # of iterations:
493  const unsigned int ransac_min_nSimulations =
494  2 * (nLM1 + nLM2); // 1000;
495  unsigned int ransac_nSimulations =
496  10; // It doesn't matter actually, since will be changed in
497  // the first loop
498  const double probability_find_good_model = 0.9999;
499 
500  const double chi2_thres_dim1 =
501  mrpt::math::chi2inv(options.ransac_chi2_quantile, 1);
502  const double chi2_thres_dim2 =
503  mrpt::math::chi2inv(options.ransac_chi2_quantile, 2);
504 
505  // Generic 2x2 covariance matrix for all features in their local
506  // coords:
507  CMatrixDouble22 COV_pnt;
508  COV_pnt.get_unsafe(0, 0) = COV_pnt.get_unsafe(1, 1) =
509  square(options.ransac_SOG_sigma_m);
510 
511  // The absolute number of trials.
512  // in practice it's only important for a reduced number of
513  // correspondences, to avoid a dead-lock:
514  // It's the binomial coefficient:
515  // / n \ n! n * (n-1)
516  // | | = ----------- = -----------
517  // \ 2 / 2! (n-2)! 2
518  //
519  const unsigned int max_trials =
520  (nCorrs * (nCorrs - 1) / 2) *
521  5; // "*5" is just for safety...
522 
523  unsigned int iter = 0; // Valid iterations (those passing the
524  // first mahalanobis test)
525  unsigned int trials = 0; // counter of all iterations,
526  // including "iter" + failing ones.
527  while (iter < ransac_nSimulations &&
528  trials <
529  max_trials) // ransac_nSimulations can be dynamic
530  {
531  trials++;
532 
533  mrpt::utils::TMatchingPairList tentativeSubSet;
534 
535  // Pick 2 random correspondences:
536  uint32_t idx1, idx2;
537  idx1 = getRandomGenerator().drawUniform32bit() % nCorrs;
538  do
539  {
540  idx2 = getRandomGenerator().drawUniform32bit() % nCorrs;
541  } while (idx1 == idx2); // Avoid a degenerated case!
542 
543  // Uniqueness of features:
544  if (all_corrs[idx1].this_idx == all_corrs[idx2].this_idx ||
545  all_corrs[idx1].this_idx == all_corrs[idx2].other_idx)
546  continue;
547  if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx ||
548  all_corrs[idx1].other_idx == all_corrs[idx2].other_idx)
549  continue;
550 
551  // Check the feasibility of this pair "idx1"-"idx2":
552  // The distance between the pair of points in MAP1 must be
553  // very close
554  // to that of their correspondences in MAP2:
555  const double corrs_dist1 =
556  mrpt::math::distanceBetweenPoints(
557  all_corrs[idx1].this_x, all_corrs[idx1].this_y,
558  all_corrs[idx1].this_z, all_corrs[idx2].this_x,
559  all_corrs[idx2].this_y, all_corrs[idx2].this_z);
560 
561  const double corrs_dist2 =
562  mrpt::math::distanceBetweenPoints(
563  all_corrs[idx1].other_x, all_corrs[idx1].other_y,
564  all_corrs[idx1].other_z, all_corrs[idx2].other_x,
565  all_corrs[idx2].other_y, all_corrs[idx2].other_z);
566 
567  // Is is a consistent possibility?
568  // We use a chi2 test (see paper for the derivation)
569  const double corrs_dist_chi2 =
570  square(square(corrs_dist1) - square(corrs_dist2)) /
571  (8.0 * square(options.ransac_SOG_sigma_m) *
572  (square(corrs_dist1) + square(corrs_dist2)));
573 
574  if (corrs_dist_chi2 > chi2_thres_dim1) continue; // Nope
575 
576  iter++; // Do not count iterations if they fail the test
577  // above.
578 
579  // before proceeding with this hypothesis, is it an old one?
580  bool is_new_hyp = true;
581  for (TMapMatchingsToPoseMode::iterator itOldHyps =
582  sog_modes.begin();
583  itOldHyps != sog_modes.end(); ++itOldHyps)
584  {
585  if (itOldHyps->first.contains(all_corrs[idx1]) &&
586  itOldHyps->first.contains(all_corrs[idx2]))
587  {
588  // Increment weight:
589  itOldHyps->second.log_w = std::log(
590  std::exp(itOldHyps->second.log_w) + 1.0);
591  is_new_hyp = false;
592  break;
593  }
594  }
595  if (!is_new_hyp) continue;
596 
597  // Ok, it's a new hypothesis:
598  tentativeSubSet.push_back(all_corrs[idx1]);
599  tentativeSubSet.push_back(all_corrs[idx2]);
600 
601  // Maintain a list of already used landmarks IDs in both
602  // maps to avoid repetitions:
603  vector_bool used_landmarks1(nLM1, false);
604  vector_bool used_landmarks2(nLM2, false);
605 
606  used_landmarks1[all_corrs[idx1].this_idx] = true;
607  used_landmarks1[all_corrs[idx2].this_idx] = true;
608  used_landmarks2[all_corrs[idx1].other_idx] = true;
609  used_landmarks2[all_corrs[idx2].other_idx] = true;
610 
611  // Build the transformation for these temptative
612  // correspondences:
613  bool keep_incorporating = true;
614  CPosePDFGaussian temptPose;
615  do // Incremently incorporate inliers:
616  {
617  if (!mrpt::tfest::se2_l2(tentativeSubSet, temptPose))
618  continue; // Invalid matching...
619 
620  // The computed cov is "normalized", i.e. must be
621  // multiplied by std^2_xy
622  temptPose.cov *= square(options.ransac_SOG_sigma_m);
623 
624  // cout << "q: " << temptPose << endl;
625 
626  // Find the landmark in MAP2 with the best (maximum)
627  // product-integral:
628  // (i^* , j^*) = arg max_(i,j) \int p_i()p_j()
629  //----------------------------------------------------------------------
630  const double ccos = cos(temptPose.mean.phi());
631  const double ssin = sin(temptPose.mean.phi());
632 
633  CMatrixDouble22 Hc; // Jacobian wrt point_j
634  Hc.get_unsafe(1, 1) = ccos;
635  Hc.get_unsafe(0, 0) = ccos;
636  Hc.get_unsafe(1, 0) = ssin;
637  Hc.get_unsafe(0, 1) = -ssin;
638 
639  CMatrixFixedNumeric<double, 2, 3>
640  Hq; // Jacobian wrt transformation q
641  Hq(0, 0) = 1;
642  Hq(1, 1) = 1;
643 
644  TPoint2D p2_j_local;
645  vector<float> matches_dist;
646  std::vector<size_t> matches_idx;
647 
648  CPoint2DPDFGaussian pdf_M2_j;
649  CPoint2DPDFGaussian pdf_M1_i;
650 
651 // Use integral-of-product vs. mahalanobis distances to match:
652 #define GRIDMAP_USE_PROD_INTEGRAL
653 
654 #ifdef GRIDMAP_USE_PROD_INTEGRAL
655  double best_pair_value = 0;
656 #else
657  double best_pair_value =
658  std::numeric_limits<double>::max();
659 #endif
660  double best_pair_d2 =
661  std::numeric_limits<double>::max();
662  pair<size_t, size_t> best_pair_ij;
663 
664 //#define SHOW_CORRS
665 
666 #ifdef SHOW_CORRS
667  CDisplayWindowPlots win("Matches");
668 #endif
669  for (size_t j = 0; j < nLM2; j++)
670  {
671  if (used_landmarks2[j]) continue;
672 
673  lm2_pnts.getPoint(
674  j, p2_j_local); // In local coords.
675  pdf_M2_j.mean = mrpt::poses::CPoint2D(
676  temptPose.mean +
677  p2_j_local); // In (temptative) global coords:
678  pdf_M2_j.cov.get_unsafe(0, 0) =
679  pdf_M2_j.cov.get_unsafe(1, 1) =
680  square(options.ransac_SOG_sigma_m);
681 
682 #ifdef SHOW_CORRS
683  win.plotEllipse(
684  pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
685  pdf_M2_j.cov, 2, "b-",
686  format("M2_%u", (unsigned)j), true);
687 #endif
688 
689  static const unsigned int N_KDTREE_SEARCHED = 3;
690 
691  // Look for a few close features which may be
692  // potential matches:
693  lm1_pnts.kdTreeNClosestPoint2DIdx(
694  pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
695  N_KDTREE_SEARCHED, matches_idx, matches_dist);
696 
697  // And for each one, compute the product-integral:
698  for (size_t u = 0; u < matches_idx.size(); u++)
699  {
700  if (used_landmarks1[matches_idx[u]]) continue;
701 
702  // Jacobian wrt transformation q
703  Hq.get_unsafe(0, 2) =
704  -p2_j_local.x * ssin - p2_j_local.y * ccos;
705  Hq.get_unsafe(1, 2) =
706  p2_j_local.x * ccos - p2_j_local.y * ssin;
707 
708  // COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t
709  Hc.multiply_HCHt(COV_pnt, pdf_M1_i.cov);
710  Hq.multiply_HCHt(
711  temptPose.cov, pdf_M1_i.cov, true);
712 
713  float px, py;
714  lm1_pnts.getPoint(matches_idx[u], px, py);
715  pdf_M1_i.mean.x(px);
716  pdf_M1_i.mean.y(py);
717 
718 #ifdef SHOW_CORRS
719  win.plotEllipse(
720  pdf_M1_i.mean.x(), pdf_M1_i.mean.y(),
721  pdf_M1_i.cov, 2, "r-",
722  format("M1_%u", (unsigned)matches_idx[u]),
723  true);
724 #endif
725 
726 // And now compute the product integral:
727 #ifdef GRIDMAP_USE_PROD_INTEGRAL
728  const double prod_ij =
729  pdf_M1_i.productIntegralWith(pdf_M2_j);
730  // const double prod_ij_d2 = square(
731  // pdf_M1_i.mahalanobisDistanceTo(pdf_M2_j) );
732 
733  if (prod_ij > best_pair_value)
734 #else
735  const double prod_ij =
736  pdf_M1_i.mean.distanceTo(pdf_M2_j.mean);
737  if (prod_ij < best_pair_value)
738 #endif
739  {
740  // const double prodint_ij =
741  // pdf_M1_i.productIntegralWith2D(pdf_M2_j);
742 
743  best_pair_value = prod_ij;
744  best_pair_ij.first = matches_idx[u];
745  best_pair_ij.second = j;
746 
747  best_pair_d2 = square(
748  pdf_M1_i.mahalanobisDistanceTo(
749  pdf_M2_j));
750 
751  // cout << "P1: " << pdf_M1_i.mean << " C= "
752  // << pdf_M1_i.cov.inMatlabFormat() << endl;
753  // cout << "P2: " << pdf_M2_j.mean << " C= "
754  // << pdf_M2_j.cov.inMatlabFormat() << endl;
755  // cout << " -> " << format("%e",prod_ij)
756  // << " d2: " << best_pair_d2 << endl <<
757  // endl;
758  }
759  } // end for u (closest matches of LM2 in MAP 1)
760 
761 #ifdef SHOW_CORRS
762  win.axis_fit(true);
763  win.waitForKey();
764  win.clear();
765 #endif
766 
767  } // end for each LM2
768 
769  // Stop when the best choice has a bad mahal. dist.
770  keep_incorporating = false;
771 
772  // For the best (i,j), gate by the mahalanobis distance:
773  if (best_pair_d2 < chi2_thres_dim2)
774  {
775  // AND, also, check if the descriptors have some
776  // resemblance!!
777  // ----------------------------------------------------------------
778  // double feat_dist =
779  // lm1->landmarks.get(best_pair_ij.first)->features[0]->descriptorDistanceTo(*lm1->landmarks.get(best_pair_ij.second)->features[0]);
780  // if (feat_dist< options.threshold_max)
781  {
782  float p1_i_localx, p1_i_localy;
783  float p2_j_localx, p2_j_localy;
784  lm1_pnts.getPoint(
785  best_pair_ij.first, p1_i_localx,
786  p1_i_localy);
787  lm2_pnts.getPoint(
788  best_pair_ij.second, p2_j_localx,
789  p2_j_localy); // In local coords.
790 
791  used_landmarks1[best_pair_ij.first] = true;
792  used_landmarks2[best_pair_ij.second] = true;
793 
794  tentativeSubSet.push_back(
795  mrpt::utils::TMatchingPair(
796  best_pair_ij.first, best_pair_ij.second,
797  p1_i_localx, p1_i_localy, 0, // MAP1
798  p2_j_localx, p2_j_localy, 0 // MAP2
799  ));
800 
801  keep_incorporating = true;
802  }
803  }
804 
805  } while (keep_incorporating);
806 
807  // Consider this pairing?
808  const size_t ninliers = tentativeSubSet.size();
809  if (ninliers > minInliersTOaccept)
810  {
811  CPosePDFSOG::TGaussianMode newGauss;
812  newGauss.log_w = 0; // log(1); //
813  // std::log(static_cast<double>(nCoincidences));
814  newGauss.mean = temptPose.mean;
815  newGauss.cov = temptPose.cov;
816 
817  sog_modes[tentativeSubSet] = newGauss;
818 
819  // cout << "ITER: " << iter << " #inliers: " << ninliers
820  // << " q: " << temptPose.mean << endl;
821  }
822 
823  // Keep the largest consensus & dynamic # of steps:
824  if (largestConsensusCorrs.size() < ninliers)
825  {
826  largestConsensusCorrs = tentativeSubSet;
827 
828  // Update estimate of N, the number of trials to ensure
829  // we pick,
830  // with probability p, a data set with no outliers.
831  const double fracinliers =
832  ninliers /
833  static_cast<double>(std::min(nLM1, nLM2));
834  double pNoOutliers =
835  1 - pow(fracinliers,
836  static_cast<double>(
837  2.0)); // minimumSizeSamplesToFit
838 
839  pNoOutliers = std::max(
840  std::numeric_limits<double>::epsilon(),
841  pNoOutliers); // Avoid division by -Inf
842  pNoOutliers = std::min(
843  1.0 - std::numeric_limits<double>::epsilon(),
844  pNoOutliers); // Avoid division by 0.
845  // Number of
846  ransac_nSimulations =
847  log(1 - probability_find_good_model) /
848  log(pNoOutliers);
849 
850  if (ransac_nSimulations < ransac_min_nSimulations)
851  ransac_nSimulations = ransac_min_nSimulations;
852 
853  // if (verbose)
854  // cout << "[Align] Iter #" << iter << " Est. #iters: "
855  //<< ransac_nSimulations << " pNoOutliers=" <<
856  // pNoOutliers << " #inliers: " << ninliers << endl;
857  }
858 
859  } // end of RANSAC loop
860 
861  // Move SOG modes into pdf_SOG:
862  pdf_SOG->clear();
863  for (TMapMatchingsToPoseMode::const_iterator s =
864  sog_modes.begin();
865  s != sog_modes.end(); ++s)
866  {
867  cout << "SOG mode: " << s->second.mean
868  << " inliers: " << s->first.size() << endl;
869  pdf_SOG->push_back(s->second);
870  }
871 
872  // Normalize:
873  if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights();
874 
875  // Simplify the SOG by merging close modes:
876  // -------------------------------------------------
877  size_t nB = pdf_SOG->size();
878  outInfo.sog1 = pdf_SOG;
879 
880  CTicTac merge_clock;
881  pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
882  const double merge_clock_tim = merge_clock.Tac();
883 
884  outInfo.sog2 = pdf_SOG;
885  size_t nA = pdf_SOG->size();
886  MRPT_LOG_INFO(
887  mrpt::format(
888  "[CGridMapAligner] amModifiedRANSAC: %u SOG modes "
889  "merged to %u in %.03fsec\n",
890  (unsigned int)nB, (unsigned int)nA, merge_clock_tim));
891 
892  } // end of amModifiedRANSAC
893 
894  // Save best corrs:
895  if (options.debug_save_map_pairs)
896  {
897  static unsigned int NN = 0;
898  static const COccupancyGridMap2D* lastM1 = nullptr;
899  if (lastM1 != m1)
900  {
901  lastM1 = m1;
902  NN = 0;
903  }
904  printf(
905  " Largest consensus: %u\n",
906  static_cast<unsigned>(largestConsensusCorrs.size()));
907  CEnhancedMetaFile::LINUX_IMG_WIDTH(
908  m1->getSizeX() + m2->getSizeX() + 50);
909  CEnhancedMetaFile::LINUX_IMG_HEIGHT(
910  max(m1->getSizeY(), m2->getSizeY()) + 50);
911 
912  for (TMapMatchingsToPoseMode::const_iterator s =
913  sog_modes.begin();
914  s != sog_modes.end(); ++s)
915  {
916  COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
917  format("__debug_corrsGrid_%05u.emf", NN), m1, m2,
918  s->first);
919  COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
920  format("__debug_corrsGrid_%05u.png", NN), m1, m2,
921  s->first);
922  ++NN;
923  }
924  }
925 
926  // --------------------------------------------------------------------
927  // At this point:
928  // - "pdf_SOG": has the resulting PDF with the SOG (from whatever
929  // method)
930  // - "largestConsensusCorrs": The 'best' set of correspondences
931  //
932  // Now: If we had a multi-metric map, use the points map to improve
933  // the estimation with ICP.
934  // --------------------------------------------------------------------
935  if (multimap1 && multimap2 && !multimap1->m_pointsMaps.empty() &&
936  !multimap2->m_pointsMaps.empty() &&
937  multimap1->m_pointsMaps[0] && multimap2->m_pointsMaps[0])
938  {
939  CSimplePointsMap::Ptr pnts1 = multimap1->m_pointsMaps[0];
940  CSimplePointsMap::Ptr pnts2 = multimap2->m_pointsMaps[0];
941 
942  CICP icp;
943  CICP::TReturnInfo icpInfo;
944 
945  icp.options.maxIterations = 20;
946  icp.options.smallestThresholdDist = 0.05f;
947  icp.options.thresholdDist = 0.75f;
948 
949  // cout << "Points: " << pnts1->size() << " " << pnts2->size()
950  // << endl;
951 
952  // Invoke ICP once for each mode in the SOG:
953  size_t cnt = 0;
954  outInfo.icp_goodness_all_sog_modes.clear();
955  for (CPosePDFSOG::iterator i = pdf_SOG->begin();
956  i != pdf_SOG->end(); ++cnt)
957  {
958  CPosePDF::Ptr icp_est = icp.Align(
959  pnts1.get(), pnts2.get(), (i)->mean, nullptr, &icpInfo);
960 
961  //(i)->cov(0,0) += square( 0.05 );
962  //(i)->cov(1,1) += square( 0.05 );
963  //(i)->cov(2,2) += square( DEG2RAD(0.05) );
964 
965  CPosePDFGaussian i_gauss(i->mean, i->cov);
966  CPosePDFGaussian icp_gauss(icp_est->getMeanVal(), i->cov);
967 
968  const double icp_maha_dist =
969  i_gauss.mahalanobisDistanceTo(icp_gauss);
970 
971  cout << "ICP " << cnt << " log-w: " << i->log_w
972  << " Goodness: " << 100 * icpInfo.goodness
973  << " D_M= " << icp_maha_dist << endl;
974  cout << " final pos: " << icp_est->getMeanVal() << endl;
975  cout << " org pos: " << i->mean << endl;
976 
977  outInfo.icp_goodness_all_sog_modes.push_back(
978  icpInfo.goodness);
979 
980  // Discard?
981  if (icpInfo.goodness > options.min_ICP_goodness &&
982  icp_maha_dist < options.max_ICP_mahadist)
983  {
984  icp_est->getMean((i)->mean);
985  ++i;
986  }
987  else
988  {
989  // Delete:
990  i = pdf_SOG->erase(i);
991  }
992 
993  } // end for i
994 
995  // Merge:
996  outInfo.sog3 = pdf_SOG;
997 
998  pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
999  MRPT_LOG_DEBUG_STREAM(
1000  "[CGridMapAligner] " << pdf_SOG->size()
1001  << " SOG modes merged after ICP.");
1002 
1003  } // end multimapX
1004 
1005  } // end of, yes, we have enough correspondences
1006 
1007  } // end of: yes, there are landmarks in the grid maps!
1008 
1009  // Copy the output info if requested:
1010  // -------------------------------------------------
1011  MRPT_TODO(
1012  "Refactor `info` so it is polymorphic and can use dynamic_cast<> here");
1013  if (info)
1014  {
1015  TReturnInfo* info_ = static_cast<TReturnInfo*>(info);
1016  *info_ = outInfo;
1017  }
1018 
1019  if (runningTime)
1020  {
1021  *runningTime = tictac->Tac();
1022  delete tictac;
1023  }
1024 
1025  return pdf_SOG;
1026 
1027  MRPT_END
1028 }
1029 
1030 /*---------------------------------------------------------------
1031  AlignPDF_correlation
1032 ---------------------------------------------------------------*/
1033 CPosePDF::Ptr CGridMapAligner::AlignPDF_correlation(
1034  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
1035  const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
1036  void* info)
1037 {
1038  MRPT_UNUSED_PARAM(initialEstimationPDF);
1039  MRPT_UNUSED_PARAM(info);
1040 
1041  MRPT_START
1042 
1043  //#define CORRELATION_SHOW_DEBUG
1044 
1045  CTicTac* tictac = nullptr;
1046 
1047  // Asserts:
1048  // -----------------
1049  ASSERT_(mm1->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
1050  ASSERT_(mm2->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
1051  const COccupancyGridMap2D* m1 =
1052  static_cast<const COccupancyGridMap2D*>(mm1);
1053  const COccupancyGridMap2D* m2 =
1054  static_cast<const COccupancyGridMap2D*>(mm2);
1055 
1056  ASSERT_(m1->getResolution() == m2->getResolution());
1057 
1058  if (runningTime)
1059  {
1060  tictac = new CTicTac();
1061  tictac->Tic();
1062  }
1063 
1064  // The PDF to estimate:
1065  // ------------------------------------------------------
1066  CPosePDFGaussian::Ptr PDF = mrpt::make_aligned_shared<CPosePDFGaussian>();
1067 
1068  // Determine the extension to compute the correlation into:
1069  // ----------------------------------------------------------
1070  float phiResolution = DEG2RAD(0.2f);
1071  float phiMin = -M_PIf + 0.5f * phiResolution;
1072  float phiMax = M_PIf;
1073 
1074  // Compute the difference between maps for each (u,v) pair!
1075  // --------------------------------------------------------------
1076  float phi;
1077  float pivotPt_x = 0.5f * (m1->getXMax() + m1->getXMin());
1078  float pivotPt_y = 0.5f * (m1->getYMax() + m1->getYMin());
1079  COccupancyGridMap2D map2_mod;
1080  CImage map1_img, map2_img;
1081  float currentMaxCorr = -1e6f;
1082 
1083  m1->getAsImage(map1_img);
1084 
1085  map2_mod.setSize(
1086  m1->getXMin(), m1->getXMax(), m1->getYMin(), m1->getYMax(),
1087  m1->getResolution());
1088  size_t map2_lx = map2_mod.getSizeX();
1089  size_t map2_ly = map2_mod.getSizeY();
1090  CMatrix outCrossCorr, bestCrossCorr;
1091  float map2width_2 = 0.5f * (map2_mod.getXMax() - map2_mod.getXMin());
1092 
1093 #ifdef CORRELATION_SHOW_DEBUG
1094  CDisplayWindow* win = new CDisplayWindow("corr");
1095  CDisplayWindow* win2 = new CDisplayWindow("map2_mod");
1096 #endif
1097 
1098  // --------------------------------------------------------
1099  // Use FFT-based correlation for each orientation:
1100  // --------------------------------------------------------
1101  for (phi = phiMin; phi < phiMax; phi += phiResolution)
1102  {
1103  // Create the displaced map2 grid, for the size of map1:
1104  // --------------------------------------------------------
1105  CPose2D v2(
1106  pivotPt_x - cos(phi) * map2width_2,
1107  pivotPt_y - sin(phi) * map2width_2,
1108  phi); // Rotation point: the centre of img1:
1109  CPoint2D v1, v3;
1110  v2 = CPose2D(0, 0, 0) - v2; // Inverse
1111 
1112  for (unsigned int cy2 = 0; cy2 < map2_ly; cy2++)
1113  {
1114  COccupancyGridMap2D::cellType* row = map2_mod.getRow(cy2);
1115  for (unsigned int cx2 = 0; cx2 < map2_lx; cx2++)
1116  {
1117  v3 = v2 + CPoint2D(map2_mod.idx2x(cx2), map2_mod.idx2y(cy2));
1118  *row++ = m2->p2l(m2->getPos(v3.x(), v3.y()));
1119  }
1120  }
1121 
1122  map2_mod.getAsImage(map2_img);
1123  // map2_img.saveToBMP("__debug_map2mod.bmp");
1124 
1125  // Compute the correlation:
1126  map1_img.cross_correlation_FFT(
1127  map2_img, outCrossCorr, -1, -1, -1, -1,
1128  127, // Bias to be substracted
1129  127 // Bias to be substracted
1130  );
1131 
1132  float corrPeak = outCrossCorr.maxCoeff();
1133  printf("phi = %fdeg \tmax corr=%f\n", RAD2DEG(phi), corrPeak);
1134 
1135  // Keep the maximum:
1136  if (corrPeak > currentMaxCorr)
1137  {
1138  currentMaxCorr = corrPeak;
1139  bestCrossCorr = outCrossCorr;
1140  PDF->mean.phi(phi);
1141  }
1142 
1143 #ifdef CORRELATION_SHOW_DEBUG
1144  outCrossCorr.adjustRange(0, 1);
1145  CImage aux(outCrossCorr, true);
1146  win->showImage(aux);
1147  win2->showImage(map2_img);
1148  std::this_thread::sleep_for(5ms);
1149 #endif
1150 
1151  } // end for phi
1152 
1153  if (runningTime)
1154  {
1155  *runningTime = tictac->Tac();
1156  delete tictac;
1157  }
1158 
1159  bestCrossCorr.normalize(0, 1);
1160  CImage aux(bestCrossCorr, true);
1161  aux.saveToFile("_debug_best_corr.png");
1162 
1163 #ifdef CORRELATION_SHOW_DEBUG
1164  delete win;
1165  delete win2;
1166 #endif
1167 
1168  // Transform the best corr matrix peak into coordinates:
1169  CMatrix::Index uMax, vMax;
1170  currentMaxCorr = bestCrossCorr.maxCoeff(&uMax, &vMax);
1171 
1172  PDF->mean.x(m1->idx2x(uMax));
1173  PDF->mean.y(m1->idx2y(vMax));
1174 
1175  return PDF;
1176 
1177  // Done!
1178  MRPT_END
1179 }
1180 
1181 /*---------------------------------------------------------------
1182  TConfigParams
1183  ---------------------------------------------------------------*/
1184 CGridMapAligner::TConfigParams::TConfigParams()
1185  : methodSelection(CGridMapAligner::amModifiedRANSAC),
1186 
1187  feature_descriptor(descPolarImages),
1188  feature_detector_options(),
1189 
1190  ransac_minSetSizeRatio(0.20f),
1191  ransac_SOG_sigma_m(0.10f),
1192  ransac_mahalanobisDistanceThreshold(6.0f),
1193  ransac_chi2_quantile(0.99),
1194  ransac_prob_good_inliers(0.9999),
1195  featsPerSquareMeter(0.015f),
1196  threshold_max(0.15f),
1197  threshold_delta(0.10f),
1198  min_ICP_goodness(0.30f),
1199  max_ICP_mahadist(10.0),
1200  maxKLd_for_merge(0.9),
1201 
1202  save_feat_coors(false),
1203  debug_show_corrs(false),
1204  debug_save_map_pairs(false)
1205 {
1206 }
1207 
1208 /*---------------------------------------------------------------
1209  dumpToTextStream
1210  ---------------------------------------------------------------*/
1211 void CGridMapAligner::TConfigParams::dumpToTextStream(
1212  mrpt::utils::CStream& out) const
1213 {
1214  out.printf(
1215  "\n----------- [CGridMapAligner::TConfigParams] ------------ \n\n");
1216 
1217  LOADABLEOPTS_DUMP_VAR(methodSelection, int)
1218  LOADABLEOPTS_DUMP_VAR(featsPerSquareMeter, float)
1219  LOADABLEOPTS_DUMP_VAR(threshold_max, float)
1220  LOADABLEOPTS_DUMP_VAR(threshold_delta, float)
1221  LOADABLEOPTS_DUMP_VAR(min_ICP_goodness, float)
1222  LOADABLEOPTS_DUMP_VAR(max_ICP_mahadist, double)
1223  LOADABLEOPTS_DUMP_VAR(maxKLd_for_merge, float)
1224  LOADABLEOPTS_DUMP_VAR(ransac_minSetSizeRatio, float)
1225  LOADABLEOPTS_DUMP_VAR(ransac_mahalanobisDistanceThreshold, float)
1226  LOADABLEOPTS_DUMP_VAR(ransac_chi2_quantile, double)
1227  LOADABLEOPTS_DUMP_VAR(ransac_prob_good_inliers, double)
1228  LOADABLEOPTS_DUMP_VAR(ransac_SOG_sigma_m, float)
1229  LOADABLEOPTS_DUMP_VAR(save_feat_coors, bool)
1230  LOADABLEOPTS_DUMP_VAR(debug_show_corrs, bool)
1231  LOADABLEOPTS_DUMP_VAR(debug_save_map_pairs, bool)
1232 
1233  LOADABLEOPTS_DUMP_VAR(feature_descriptor, int)
1234 
1235  feature_detector_options.dumpToTextStream(out);
1236 
1237  out.printf("\n");
1238 }
1239 
1240 /*---------------------------------------------------------------
1241  loadFromConfigFile
1242  ---------------------------------------------------------------*/
1243 void CGridMapAligner::TConfigParams::loadFromConfigFile(
1244  const mrpt::utils::CConfigFileBase& iniFile, const std::string& section)
1245 {
1246  methodSelection =
1247  iniFile.read_enum(section, "methodSelection", methodSelection);
1248 
1249  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
1250  featsPerSquareMeter, float, iniFile, section)
1251  MRPT_LOAD_CONFIG_VAR(ransac_SOG_sigma_m, float, iniFile, section)
1252 
1253  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_max, float, iniFile, section)
1254  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_delta, float, iniFile, section)
1255 
1256  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(min_ICP_goodness, float, iniFile, section)
1257  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(max_ICP_mahadist, double, iniFile, section)
1258 
1259  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxKLd_for_merge, float, iniFile, section)
1260  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
1261  ransac_minSetSizeRatio, float, iniFile, section)
1262  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
1263  ransac_mahalanobisDistanceThreshold, float, iniFile, section)
1264  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
1265  ransac_chi2_quantile, double, iniFile, section)
1266  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
1267  ransac_prob_good_inliers, double, iniFile, section)
1268 
1269  MRPT_LOAD_CONFIG_VAR(save_feat_coors, bool, iniFile, section)
1270  MRPT_LOAD_CONFIG_VAR(debug_show_corrs, bool, iniFile, section)
1271  MRPT_LOAD_CONFIG_VAR(debug_save_map_pairs, bool, iniFile, section)
1272 
1273  feature_descriptor = iniFile.read_enum(
1274  section, "feature_descriptor", feature_descriptor, true);
1275  feature_detector_options.loadFromConfigFile(iniFile, section);
1276 }
1277 
1278 CPose3DPDF::Ptr CGridMapAligner::Align3DPDF(
1279  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
1280  const CPose3DPDFGaussian& initialEstimationPDF, float* runningTime,
1281  void* info)
1282 {
1283  MRPT_UNUSED_PARAM(m1);
1284  MRPT_UNUSED_PARAM(m2);
1285  MRPT_UNUSED_PARAM(initialEstimationPDF);
1286  MRPT_UNUSED_PARAM(runningTime);
1287  MRPT_UNUSED_PARAM(info);
1288  THROW_EXCEPTION("Align3D method not applicable to gridmap-aligner");
1289 }
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:25
CGridMapAligner.h
mrpt::random::CRandomGenerator::drawUniform32bit
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
Definition: RandomGenerator.cpp:42
mrpt::maps::COccupancyGridMap2D::getXMax
float getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: COccupancyGridMap2D.h:304
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
mrpt::maps::COccupancyGridMap2D::getArea
double getArea() const
Returns the area of the gridmap, in square meters.
Definition: COccupancyGridMap2D.h:292
mrpt::system::createDirectory
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
mrpt::poses::CPoint2DPDFGaussian::productIntegralWith
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
Definition: CPoint2DPDFGaussian.cpp:162
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:46
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::math::TPoint2D::x
double x
X,Y coordinates.
Definition: lightweight_geom_data.h:41
mrpt::poses::CPosePDFSOG::TGaussianMode
The struct for each mode:
Definition: CPosePDFSOG.h:43
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:310
mrpt::maps::CMultiMetricMap::m_pointsMaps
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMap::Ptr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
Definition: CMultiMetricMap.h:361
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:206
mrpt::maps::CSimplePointsMap::reserve
virtual void reserve(size_t newLength) override
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
Definition: CSimplePointsMap.cpp:72
mrpt::slam::CICP::TConfigParams::thresholdDist
float thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:122
mrpt::poses::CPoint2DPDFGaussian
A gaussian distribution for 2D points.
Definition: CPoint2DPDFGaussian.h:23
mrpt::slam::CGridMapAligner
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map...
Definition: CGridMapAligner.h:43
mrpt::utils::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:118
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
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.
Definition: CMetricMapsAlignmentAlgorithm.cpp:26
mrpt::slam::CICP
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:67
first
GLint * first
Definition: glext.h:3827
mrpt::tfest::TSE2RobustParams
Parameters for se2_l2_robust().
Definition: se2.h:74
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
mrpt::tfest::TSE2RobustResult::largestSubSet
mrpt::utils::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:150
n
GLenum GLsizei n
Definition: glext.h:5074
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
LOADABLEOPTS_DUMP_VAR
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
Definition: CLoadableOptions.h:103
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
ops_containers.h
This file implements several operations that operate element-wise on individual or pairs of container...
mrpt::tfest::TSE2RobustParams::ransac_fuseByCorrsMatch
bool ransac_fuseByCorrsMatch
(Default = true) If true, the weight of Gaussian modes will be increased when an exact match in the s...
Definition: se2.h:93
mrpt::slam::CGridMapAligner::Align3DPDF
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr)
Not applicable in this class, will launch an exception.
Definition: CGridMapAligner.cpp:1278
mrpt::tfest::TSE2RobustParams::ransac_nSimulations
unsigned int ransac_nSimulations
(Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations...
Definition: se2.h:86
mrpt::tfest::TSE2RobustParams::ransac_minSetSize
unsigned int ransac_minSetSize
(Default=3)
Definition: se2.h:77
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:42
ransac_mahalanobisDistanceThreshold
const float ransac_mahalanobisDistanceThreshold
Definition: se3_ransac_unittest.cpp:38
myVectorOrder
bool myVectorOrder(const pair< size_t, float > &o1, const pair< size_t, float > &o2)
Definition: CGridMapAligner.cpp:83
CICP.h
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
Definition: CSimplePointsMap.h:31
mrpt::maps::CPointsMap::insertPoint
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:581
mrpt::tfest::se2_l2
bool se2_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=nullptr)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Definition: se2_l2.cpp:46
std
STL namespace.
mrpt::tfest::se2_l2_robust
bool se2_l2_robust(const mrpt::utils::TMatchingPairList &in_correspondences, const double in_normalizationStd, const TSE2RobustParams &in_ransac_params, TSE2RobustResult &out_results)
Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference ...
Definition: se2_l2_ransac.cpp:78
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::poses::CPosePDFGaussian::mahalanobisDistanceTo
double mahalanobisDistanceTo(const CPosePDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
Definition: CPosePDFGaussian.cpp:388
mrpt::math::KDTreeCapable::kdTreeNClosestPoint2DIdx
void kdTreeNClosestPoint2DIdx(float x0, float y0, size_t knn, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes...
Definition: KDTreeCapable.h:360
s
GLdouble s
Definition: glext.h:3676
mrpt::slam::CGridMapAligner::TReturnInfo
The ICP algorithm return information.
Definition: CGridMapAligner.h:153
mrpt::slam::CICP::options
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:185
mrpt::utils::CTicTac::Tic
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:82
mrpt::aligned_containers
Helper types for STL containers with Eigen memory allocators.
Definition: aligned_containers.h:30
mrpt::poses::CPosePDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPosePDFGaussian.h:48
mrpt::maps::COccupancyGridMap2D::getAsImage
void getAsImage(utils::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
Definition: COccupancyGridMap2D_getAs.cpp:29
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
MRPT_TODO
#define MRPT_TODO(x)
Definition: CColouredPointsMap.h:83
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:39
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
mrpt::vector_bool
std::vector< bool > vector_bool
A type for passing a vector of bools.
Definition: types_simple.h:31
mrpt::tfest::TSE2RobustParams::verbose
bool verbose
(Default=false)
Definition: se2.h:112
mrpt::utils::CImage::cross_correlation_FFT
void cross_correlation_FFT(const CImage &in_img, math::CMatrixFloat &out_corr, int u_search_ini=-1, int v_search_ini=-1, int u_search_size=-1, int v_search_size=-1, float biasThisImg=0, float biasInImg=0) const
Computes the correlation matrix between this image and another one.
Definition: CImage.cpp:1750
mrpt::math::minimum
CONTAINER::Scalar minimum(const CONTAINER &v)
Definition: ops_containers.h:144
CEnhancedMetaFile.h
mrpt::maps::CLandmarksMap::Ptr
std::shared_ptr< CLandmarksMap > Ptr
Definition: CLandmarksMap.h:77
mrpt::slam::CICP::TReturnInfo::goodness
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:202
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:41
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:39
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:44
mrpt::utils::CImage::getWidth
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:869
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
mrpt::tfest::TSE2RobustParams::ransac_maxSetSize
unsigned int ransac_maxSetSize
(Default = 20)
Definition: se2.h:79
M_PIf
#define M_PIf
Definition: CColouredPointsMap.h:441
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:75
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFGaussian.h:31
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
mrpt::maps::COccupancyGridMap2D::getXMin
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: COccupancyGridMap2D.h:302
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
mrpt::maps::COccupancyGridMap2D::idx2x
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
Definition: COccupancyGridMap2D.h:331
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:93
mrpt::slam::CICP::TConfigParams::maxIterations
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:109
mrpt::utils::CTicTac
This class implements a high-performance stopwatch.
Definition: CTicTac.h:23
mrpt::tfest::TSE2RobustParams::ransac_algorithmForLandmarks
bool ransac_algorithmForLandmarks
(Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
Definition: se2.h:100
mrpt::poses::CPosePDFSOG::TGaussianMode::cov
mrpt::math::CMatrixDouble33 cov
Definition: CPosePDFSOG.h:47
mrpt::utils::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:897
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:45
v3
GLfloat GLfloat GLfloat GLfloat v3
Definition: glext.h:4109
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:19
mrpt::maps::COccupancyGridMap2D::getSizeX
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: COccupancyGridMap2D.h:298
mrpt::maps::CMultiMetricMap::m_gridMaps
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2D::Ptr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
Definition: CMultiMetricMap.h:364
mrpt::vision
Classes for computer vision, detectors, features, etc.
Definition: bundle_adjustment.h:25
mrpt::poses::CPosePDFGaussian::Ptr
std::shared_ptr< CPosePDFGaussian > Ptr
Definition: CPosePDFGaussian.h:33
mrpt::poses::CPosePDFSOG::Ptr
std::shared_ptr< CPosePDFSOG > Ptr
Definition: CPosePDFSOG.h:38
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
CPoint2DPDFGaussian.h
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::tfest::TSE2RobustParams::ransac_mahalanobisDistanceThreshold
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:81
mrpt::poses::CPoint2D
A class used to store a 2D point.
Definition: CPoint2D.h:36
mrpt::tfest::TSE2RobustParams::ransac_fuseMaxDiffPhi
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:97
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
COccupancyGridMap2D.h
mrpt::utils::CConfigFileBase::read_enum
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
Definition: CConfigFileBase.h:242
mrpt::utils::CCanvas::filledRectangle
virtual void filledRectangle(int x0, int y0, int x1, int y1, const mrpt::utils::TColor color)
Draws a filled rectangle.
Definition: CCanvas.cpp:216
CPose3DPDFGaussian.h
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:64
MRPT_START
#define MRPT_START
Definition: CColouredPointsMap.h:426
mrpt::slam::CGridMapAligner::TConfigParams::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
Definition: CGridMapAligner.cpp:1211
CPosePDFGaussian.h
mrpt::maps::CPointsMap::getPoint
unsigned long getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:222
RAD2DEG
#define RAD2DEG
Definition: CColouredPointsMap.h:115
CMultiMetricMap.h
MRPT_LOG_INFO_STREAM
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
mrpt::math::chi2inv
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:998
v1
GLfloat GLfloat v1
Definition: glext.h:4105
mrpt::maps::COccupancyGridMap2D::p2l
static cellType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
Definition: COccupancyGridMap2D.h:362
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:55
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
mrpt::maps::COccupancyGridMap2D::getRow
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
Definition: COccupancyGridMap2D.h:387
mrpt::slam::CICP::TConfigParams::smallestThresholdDist
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:127
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:91
IS_CLASS
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
mrpt::maps::CSimplePointsMap::Ptr
std::shared_ptr< CSimplePointsMap > Ptr
Definition: CSimplePointsMap.h:33
row
GLenum GLenum GLvoid * row
Definition: glext.h:3576
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:195
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:194
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:234
mrpt::utils::CTicTac::Tac
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:97
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::maps::COccupancyGridMap2D::idx2y
float idx2y(const size_t cy) const
Definition: COccupancyGridMap2D.h:335
mrpt::tfest::TSE2RobustResult::transformation
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:148
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:40
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:25
mrpt::vision::descPolarImages
Polar image descriptor.
Definition: vision/include/mrpt/vision/types.h:107
mrpt::utils::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:301
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
Definition: CConfigFileBase.h:375
mrpt::system::deleteFilesInDirectory
bool deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists, or path is a file).
Definition: filesystem.cpp:214
mrpt::maps::COccupancyGridMap2D::getPos
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
Definition: COccupancyGridMap2D.h:412
mrpt::math::distanceBetweenPoints
T distanceBetweenPoints(const T x1, const T y1, const T x2, const T y2)
Returns the distance between 2 points in 2D.
Definition: geometry.h:954
v2
GLfloat GLfloat GLfloat v2
Definition: glext.h:4107
mrpt::poses::CPosePDFSOG::iterator
CListGaussianModes::iterator iterator
Definition: CPosePDFSOG.h:70
mrpt::maps::COccupancyGridMap2D::setSize
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
Definition: COccupancyGridMap2D_common.cpp:159
MRPT_LOAD_CONFIG_VAR
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
Definition: CConfigFileBase.h:279
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:141
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: COccupancyGridMap2D.h:300
mrpt::math::CMatrix
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:25
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:34
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: CColouredPointsMap.h:307
mrpt::poses::CPoint2DPDFGaussian::cov
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
Definition: CPoint2DPDFGaussian.h:39
mrpt::maps::COccupancyGridMap2D::cellType
int16_t cellType
The type of the map cells:
Definition: COccupancyGridMap2D.h:79
mrpt::tfest::TSE2RobustResult
Output placeholder for se2_l2_robust()
Definition: se2.h:145
mrpt::utils::CCanvas::drawImage
virtual void drawImage(int x, int y, const utils::CImage &img)
Draws an image as a bitmap at a given position.
Definition: CCanvas.cpp:256
mrpt::utils::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::slam::CGridMapAligner::TConfigParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::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:1243
mrpt::tfest::TSE2RobustParams::ransac_fuseMaxDiffXY
double ransac_fuseMaxDiffXY
(Default = 0.01)
Definition: se2.h:95
CLandmarksMap.h
mrpt::utils::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:597
mrpt::tfest::TSE2RobustParams::probability_find_good_model
double probability_find_good_model
(Default = 0.999) See parameter ransac_nSimulations.
Definition: se2.h:104
mrpt::poses::CPoint2DPDFGaussian::mahalanobisDistanceTo
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...
Definition: CPoint2DPDFGaussian.cpp:237


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019