MRPT  2.0.4
CGridMapAligner.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 "slam-precomp.h" // Precompiled headers
11 
17 #include <mrpt/math/TPose2D.h>
19 #include <mrpt/math/geometry.h>
24 #include <mrpt/random.h>
26 #include <mrpt/slam/CICP.h>
27 #include <mrpt/system/filesystem.h>
28 #include <mrpt/tfest/se2.h>
29 #include <Eigen/Dense>
30 
31 using namespace mrpt::math;
32 using namespace mrpt::slam;
33 using namespace mrpt::maps;
34 using namespace mrpt::poses;
35 using namespace mrpt::random;
36 using namespace mrpt::img;
37 using namespace mrpt::system;
38 using namespace mrpt::vision;
39 using namespace std;
40 
41 CPosePDF::Ptr CGridMapAligner::AlignPDF(
42  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
43  const CPosePDFGaussian& initialEstimationPDF,
45 {
47 
48  TReturnInfo infoVal;
49  CPosePDF::Ptr ret;
50 
51  switch (options.methodSelection)
52  {
53  case CGridMapAligner::amCorrelation:
54  ret = AlignPDF_correlation(mm1, mm2, initialEstimationPDF, infoVal);
55  break;
56 
57  case CGridMapAligner::amModifiedRANSAC:
58  case CGridMapAligner::amRobustMatch:
59  // The same function has an internal switch for the specific method:
60  ret = AlignPDF_robustMatch(mm1, mm2, initialEstimationPDF, infoVal);
61  break;
62 
63  default:
64  THROW_EXCEPTION("Wrong value found in 'options.methodSelection'!!");
65  }
66 
67  // Copy the output info if requested:
68  if (info)
69  if (auto* o = dynamic_cast<TReturnInfo*>(&info.value().get()); o)
70  *o = infoVal;
71 
72  return ret;
73  MRPT_END
74 }
75 
76 static bool myVectorOrder(
77  const pair<size_t, float>& o1, const pair<size_t, float>& o2)
78 {
79  return o1.second < o2.second;
80 }
81 
82 /*---------------------------------------------------------------
83  AlignPDF_robustMatch
84 ---------------------------------------------------------------*/
85 CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch(
86  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
87  const CPosePDFGaussian& initialEstimationPDF, TReturnInfo& outInfo)
88 {
90 
91  ASSERT_(
92  options.methodSelection == CGridMapAligner::amRobustMatch ||
93  options.methodSelection == CGridMapAligner::amModifiedRANSAC);
94 
95  mrpt::tfest::TMatchingPairList& correspondences =
96  outInfo.correspondences; // Use directly this placeholder to save 1
97  // variable & 1 copy.
98  mrpt::tfest::TMatchingPairList largestConsensusCorrs;
99 
102 
103  std::vector<size_t> idxs1, idxs2;
104  std::vector<size_t>::iterator it1, it2;
105 
106  // Asserts:
107  // -----------------
108  const CMultiMetricMap* multimap1 = nullptr;
109  const CMultiMetricMap* multimap2 = nullptr;
110  const COccupancyGridMap2D* m1 = nullptr;
111  const COccupancyGridMap2D* m2 = nullptr;
112 
113  if (IS_CLASS(*mm1, CMultiMetricMap) && IS_CLASS(*mm2, CMultiMetricMap))
114  {
115  multimap1 = dynamic_cast<const CMultiMetricMap*>(mm1);
116  multimap2 = dynamic_cast<const CMultiMetricMap*>(mm2);
117 
120 
121  m1 = multimap1->mapByClass<COccupancyGridMap2D>().get();
122  m2 = multimap2->mapByClass<COccupancyGridMap2D>().get();
123  }
124  else if (
125  IS_CLASS(*mm1, COccupancyGridMap2D) &&
127  {
128  m1 = dynamic_cast<const COccupancyGridMap2D*>(mm1);
129  m2 = dynamic_cast<const COccupancyGridMap2D*>(mm2);
130  }
131  else
133  "Metric maps must be of classes COccupancyGridMap2D or "
134  "CMultiMetricMap");
135 
136  ASSERT_(m1);
137  ASSERT_(m2);
138 
139  ASSERTMSG_(
140  m1->getResolution() == m2->getResolution(),
141  mrpt::format(
142  "Different resolutions for m1, m2:\n"
143  "\tres(m1) = %f\n\tres(m2) = %f\n",
144  m1->getResolution(), m2->getResolution()));
145 
146  // The algorithm output auxiliar info:
147  // -------------------------------------------------
148  outInfo.goodness = 1.0f;
149 
150  outInfo.landmarks_map1 = lm1;
151  outInfo.landmarks_map2 = lm2;
152 
153  // The PDF to estimate:
154  // ------------------------------------------------------
155  CPosePDFSOG::Ptr pdf_SOG = std::make_shared<CPosePDFSOG>();
156 
157  // Extract features from grid-maps:
158  // ------------------------------------------------------
159  const size_t N1 =
160  std::max(40, mrpt::round(m1->getArea() * options.featsPerSquareMeter));
161  const size_t N2 =
162  std::max(40, mrpt::round(m2->getArea() * options.featsPerSquareMeter));
163 
164  m_grid_feat_extr.extractFeatures(
165  *m1, *lm1, N1, options.feature_descriptor,
166  options.feature_detector_options);
167  m_grid_feat_extr.extractFeatures(
168  *m2, *lm2, N2, options.feature_descriptor,
169  options.feature_detector_options);
170 
171  mrpt::system::CTicTac tictac;
172  tictac.Tic();
173 
174  const size_t nLM1 = lm1->size();
175  const size_t nLM2 = lm2->size();
176 
177  // At least two landmarks at each map!
178  // ------------------------------------------------------
179  if (nLM1 < 2 || nLM2 < 2)
180  {
181  outInfo.goodness = 0;
182  }
183  else
184  {
185  //#define METHOD_FFT
186  //#define DEBUG_SHOW_CORRELATIONS
187 
188  // Compute correlation between landmarks:
189  // ---------------------------------------------
190  CMatrixFloat CORR(lm1->size(), lm2->size()), auxCorr;
191  CImage im1, im2; // Grayscale
192  CVectorFloat corr;
193  unsigned int corrsCount = 0;
194  std::vector<bool> hasCorr(nLM1, false);
195 
196  const double thres_max = options.threshold_max;
197  const double thres_delta = options.threshold_delta;
198 
199  // CDisplayWindowPlots::Ptr auxWin;
200  if (options.debug_show_corrs)
201  {
202  // auxWin = CDisplayWindowPlots::Ptr( new
203  // CDisplayWindowPlots("Individual corr.") );
204  std::cerr << "Warning: options.debug_show_corrs has no effect "
205  "since MRPT 0.9.1\n";
206  }
207 
208  for (size_t idx1 = 0; idx1 < nLM1; idx1++)
209  {
210  // CVectorFloat corrs_indiv;
211  vector<pair<size_t, float>> corrs_indiv; // (index, distance);
212  // Index is used to
213  // recover the original
214  // index after sorting.
215  vector<float> corrs_indiv_only;
216  corrs_indiv.reserve(nLM2);
217  corrs_indiv_only.reserve(nLM2);
218 
219  for (size_t idx2 = 0; idx2 < nLM2; idx2++)
220  {
221  float minDist;
222  minDist =
223  lm1->landmarks.get(idx1)->features[0].descriptorDistanceTo(
224  lm2->landmarks.get(idx2)->features[0]);
225 
226  corrs_indiv.emplace_back(idx2, minDist);
227  corrs_indiv_only.push_back(minDist);
228  } // end for idx2
229 
230  const double corr_best = mrpt::math::minimum(corrs_indiv_only);
231 
232  // Sort the list and keep the N best features:
233  std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder);
234 
235  // const size_t nBestToKeep = std::min( (size_t)30,
236  // corrs_indiv.size() );
237  const size_t nBestToKeep = corrs_indiv.size();
238 
239  for (size_t w = 0; w < nBestToKeep; w++)
240  {
241  if (corrs_indiv[w].second <= thres_max &&
242  corrs_indiv[w].second <= (corr_best + thres_delta))
243  {
244  idxs1.push_back(idx1);
245  idxs2.push_back(corrs_indiv[w].first);
246  outInfo.correspondences_dists_maha.emplace_back(
247  idx1, corrs_indiv[w].first, corrs_indiv[w].second);
248  }
249  }
250  } // end for idx1
251 
252  // Save an image with each feature and its matches:
253  if (options.save_feat_coors)
254  {
256  mrpt::system::createDirectory("grid_feats");
257 
258  std::map<size_t, std::set<size_t>> corrs_by_idx;
259  for (size_t l = 0; l < idxs1.size(); l++)
260  corrs_by_idx[idxs1[l]].insert(idxs2[l]);
261 
262  for (auto& it : corrs_by_idx)
263  {
264  CMatrixFloat descriptor1;
265  lm1->landmarks.get(it.first)
266  ->features[0]
267  .getFirstDescriptorAsMatrix(descriptor1);
268 
269  im1.setFromMatrix(descriptor1, true /*normalized*/);
270 
271  const size_t FEAT_W = im1.getWidth();
272  const size_t FEAT_H = im1.getHeight();
273  const size_t nF = it.second.size();
274 
275  CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF);
276  img_compose.filledRectangle(
277  0, 0, img_compose.getWidth() - 1,
278  img_compose.getHeight() - 1, TColor::black());
279 
280  img_compose.drawImage(5, 5, im1);
281 
282  size_t j;
283  std::set<size_t>::iterator it_j;
284  string fil =
285  format("grid_feats/_FEAT_MATCH_%03i", (int)it.first);
286 
287  for (j = 0, it_j = it.second.begin(); j < nF; ++j, ++it_j)
288  {
289  fil += format("_%u", static_cast<unsigned int>(*it_j));
290 
291  CMatrixFloat descriptor2;
292  lm2->landmarks.get(*it_j)
293  ->features[0]
294  .getFirstDescriptorAsMatrix(descriptor2);
295  im2.setFromMatrix(descriptor2, true);
296  img_compose.drawImage(
297  10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
298  }
299  fil += ".png";
300  img_compose.saveToFile(fil);
301  } // end for map
302  }
303 
304  // ------------------------------------------------------------------
305  // Create the list of correspondences from the lists: idxs1 & idxs2
306  // ------------------------------------------------------------------
307  correspondences.clear();
308  for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end();
309  ++it1, ++it2)
310  {
312  mp.this_idx = *it1;
313  mp.this_x = lm1->landmarks.get(*it1)->pose_mean.x;
314  mp.this_y = lm1->landmarks.get(*it1)->pose_mean.y;
315  mp.this_z = lm1->landmarks.get(*it1)->pose_mean.z;
316 
317  mp.other_idx = *it2;
318  mp.other_x = lm2->landmarks.get(*it2)->pose_mean.x;
319  mp.other_y = lm2->landmarks.get(*it2)->pose_mean.y;
320  mp.other_z = lm2->landmarks.get(*it2)->pose_mean.z;
321  correspondences.push_back(mp);
322 
323  if (!hasCorr[*it1])
324  {
325  hasCorr[*it1] = true;
326  corrsCount++;
327  }
328  } // end for corres.
329 
330  outInfo.goodness = (2.0f * corrsCount) / (nLM1 + nLM2);
331 
332  // Compute the estimation using ALL the correspondences (NO ROBUST):
333  // ----------------------------------------------------------------------
334  mrpt::math::TPose2D noRobustEst;
335  if (!mrpt::tfest::se2_l2(correspondences, noRobustEst))
336  {
337  // There's no way to match the maps! e.g. no correspondences
338  outInfo.goodness = 0;
339  pdf_SOG->clear();
340  outInfo.sog1 = pdf_SOG;
341  outInfo.sog2 = pdf_SOG;
342  outInfo.sog3 = pdf_SOG;
343  }
344  else
345  {
346  outInfo.noRobustEstimation = mrpt::poses::CPose2D(noRobustEst);
348  "[CGridMapAligner] Overall estimation(%u corrs, total: "
349  "%u): (%.03f,%.03f,%.03fdeg)\n",
350  corrsCount, (unsigned)correspondences.size(),
351  outInfo.noRobustEstimation.x(), outInfo.noRobustEstimation.y(),
352  RAD2DEG(outInfo.noRobustEstimation.phi())));
353 
354  // The list of SOG modes & their corresponding sub-sets of
355  // matchings:
356  using TMapMatchingsToPoseMode = std::map<
358  TMapMatchingsToPoseMode sog_modes;
359 
360  // ---------------------------------------------------------------
361  // Now, we have to choose between the methods:
362  // - CGridMapAligner::amRobustMatch ("normal" RANSAC)
363  // - CGridMapAligner::amModifiedRANSAC
364  // ---------------------------------------------------------------
365  if (options.methodSelection == CGridMapAligner::amRobustMatch)
366  {
367  // ====================================================
368  // METHOD: "Normal" RANSAC
369  // ====================================================
370 
371  // RANSAC over the found correspondences:
372  // -------------------------------------------------
373  const unsigned int min_inliers =
374  options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2;
375 
376  mrpt::tfest::TSE2RobustParams tfest_params;
377  tfest_params.ransac_minSetSize = min_inliers;
378  tfest_params.ransac_maxSetSize = nLM1 + nLM2;
380  options.ransac_mahalanobisDistanceThreshold;
381  tfest_params.ransac_nSimulations = 0; // 0=auto
382  tfest_params.ransac_fuseByCorrsMatch = true;
383  tfest_params.ransac_fuseMaxDiffXY = 0.01;
384  tfest_params.ransac_fuseMaxDiffPhi = 0.1_deg;
385  tfest_params.ransac_algorithmForLandmarks = true;
386  tfest_params.probability_find_good_model =
387  options.ransac_prob_good_inliers;
388  tfest_params.verbose = false;
389 
390  mrpt::tfest::TSE2RobustResult tfest_result;
392  correspondences, options.ransac_SOG_sigma_m, tfest_params,
393  tfest_result);
394 
395  ASSERT_(pdf_SOG);
396  *pdf_SOG = tfest_result.transformation;
397  largestConsensusCorrs = tfest_result.largestSubSet;
398 
399  // Simplify the SOG by merging close modes:
400  // -------------------------------------------------
401  size_t nB = pdf_SOG->size();
402  outInfo.sog1 = pdf_SOG;
403 
404  // Keep only the most-likely Gaussian mode:
405  CPosePDFSOG::TGaussianMode best_mode;
406  best_mode.log_w = -std::numeric_limits<double>::max();
407 
408  for (size_t n = 0; n < pdf_SOG->size(); n++)
409  {
410  const CPosePDFSOG::TGaussianMode& m = (*pdf_SOG)[n];
411 
412  if (m.log_w > best_mode.log_w) best_mode = m;
413  }
414 
415  pdf_SOG->clear();
416  pdf_SOG->push_back(best_mode);
417  outInfo.sog2 = pdf_SOG;
418 
420  "[CGridMapAligner] amRobustMatch: "
421  << nB << " SOG modes reduced to " << pdf_SOG->size()
422  << " (most-likely) (min.inliers=" << min_inliers << ")\n");
423 
424  } // end of amRobustMatch
425  else
426  {
427  // ====================================================
428  // METHOD: "Modified" RANSAC
429  // ====================================================
430  mrpt::tfest::TMatchingPairList all_corrs = correspondences;
431 
432  const size_t nCorrs = all_corrs.size();
433  ASSERT_(nCorrs >= 2);
434 
435  pdf_SOG->clear(); // Start with 0 Gaussian modes
436 
437  // Build a points-map for exploiting its KD-tree:
438  // ----------------------------------------------------
439  CSimplePointsMap lm1_pnts, lm2_pnts;
440  lm1_pnts.reserve(nLM1);
441  for (size_t i = 0; i < nLM1; i++)
442  lm1_pnts.insertPoint(lm1->landmarks.get(i)->pose_mean);
443  lm2_pnts.reserve(nLM2);
444  for (size_t i = 0; i < nLM2; i++)
445  lm2_pnts.insertPoint(lm2->landmarks.get(i)->pose_mean);
446 
447  // RANSAC loop
448  // ---------------------
449  const size_t minInliersTOaccept =
450  round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2));
451  // Set an initial # of iterations:
452  const unsigned int ransac_min_nSimulations =
453  2 * (nLM1 + nLM2); // 1000;
454  unsigned int ransac_nSimulations =
455  10; // It doesn't matter actually, since will be changed in
456  // the first loop
457  const double probability_find_good_model = 0.9999;
458 
459  const double chi2_thres_dim1 =
460  mrpt::math::chi2inv(options.ransac_chi2_quantile, 1);
461  const double chi2_thres_dim2 =
462  mrpt::math::chi2inv(options.ransac_chi2_quantile, 2);
463 
464  // Generic 2x2 covariance matrix for all features in their local
465  // coords:
466  CMatrixDouble22 COV_pnt;
467  COV_pnt(0, 0) = COV_pnt(1, 1) =
468  square(options.ransac_SOG_sigma_m);
469 
470  // The absolute number of trials.
471  // in practice it's only important for a reduced number of
472  // correspondences, to avoid a dead-lock:
473  // It's the binomial coefficient:
474  // / n \ n! n * (n-1)
475  // | | = ----------- = -----------
476  // \ 2 / 2! (n-2)! 2
477  //
478  const unsigned int max_trials =
479  (nCorrs * (nCorrs - 1) / 2) *
480  5; // "*5" is just for safety...
481 
482  unsigned int iter = 0; // Valid iterations (those passing the
483  // first mahalanobis test)
484  unsigned int trials = 0; // counter of all iterations,
485  // including "iter" + failing ones.
486  while (iter < ransac_nSimulations &&
487  trials <
488  max_trials) // ransac_nSimulations can be dynamic
489  {
490  trials++;
491 
492  mrpt::tfest::TMatchingPairList tentativeSubSet;
493 
494  // Pick 2 random correspondences:
495  uint32_t idx1, idx2;
496  idx1 = getRandomGenerator().drawUniform32bit() % nCorrs;
497  do
498  {
499  idx2 = getRandomGenerator().drawUniform32bit() % nCorrs;
500  } while (idx1 == idx2); // Avoid a degenerated case!
501 
502  // Uniqueness of features:
503  if (all_corrs[idx1].this_idx == all_corrs[idx2].this_idx ||
504  all_corrs[idx1].this_idx == all_corrs[idx2].other_idx)
505  continue;
506  if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx ||
507  all_corrs[idx1].other_idx == all_corrs[idx2].other_idx)
508  continue;
509 
510  // Check the feasibility of this pair "idx1"-"idx2":
511  // The distance between the pair of points in MAP1 must be
512  // very close
513  // to that of their correspondences in MAP2:
514  const double corrs_dist1 =
516  all_corrs[idx1].this_x, all_corrs[idx1].this_y,
517  all_corrs[idx1].this_z, all_corrs[idx2].this_x,
518  all_corrs[idx2].this_y, all_corrs[idx2].this_z);
519 
520  const double corrs_dist2 =
522  all_corrs[idx1].other_x, all_corrs[idx1].other_y,
523  all_corrs[idx1].other_z, all_corrs[idx2].other_x,
524  all_corrs[idx2].other_y, all_corrs[idx2].other_z);
525 
526  // Is is a consistent possibility?
527  // We use a chi2 test (see paper for the derivation)
528  const double corrs_dist_chi2 =
529  square(square(corrs_dist1) - square(corrs_dist2)) /
530  (8.0 * square(options.ransac_SOG_sigma_m) *
531  (square(corrs_dist1) + square(corrs_dist2)));
532 
533  if (corrs_dist_chi2 > chi2_thres_dim1) continue; // Nope
534 
535  iter++; // Do not count iterations if they fail the test
536  // above.
537 
538  // before proceeding with this hypothesis, is it an old one?
539  bool is_new_hyp = true;
540  for (auto& sog_mode : sog_modes)
541  {
542  if (sog_mode.first.contains(all_corrs[idx1]) &&
543  sog_mode.first.contains(all_corrs[idx2]))
544  {
545  // Increment weight:
546  sog_mode.second.log_w =
547  std::log(std::exp(sog_mode.second.log_w) + 1.0);
548  is_new_hyp = false;
549  break;
550  }
551  }
552  if (!is_new_hyp) continue;
553 
554  // Ok, it's a new hypothesis:
555  tentativeSubSet.push_back(all_corrs[idx1]);
556  tentativeSubSet.push_back(all_corrs[idx2]);
557 
558  // Maintain a list of already used landmarks IDs in both
559  // maps to avoid repetitions:
560  std::vector<bool> used_landmarks1(nLM1, false);
561  std::vector<bool> used_landmarks2(nLM2, false);
562 
563  used_landmarks1[all_corrs[idx1].this_idx] = true;
564  used_landmarks1[all_corrs[idx2].this_idx] = true;
565  used_landmarks2[all_corrs[idx1].other_idx] = true;
566  used_landmarks2[all_corrs[idx2].other_idx] = true;
567 
568  // Build the transformation for these temptative
569  // correspondences:
570  bool keep_incorporating = true;
571  CPosePDFGaussian temptPose;
572  do // Incremently incorporate inliers:
573  {
574  if (!mrpt::tfest::se2_l2(tentativeSubSet, temptPose))
575  continue; // Invalid matching...
576 
577  // The computed cov is "normalized", i.e. must be
578  // multiplied by std^2_xy
579  temptPose.cov *= square(options.ransac_SOG_sigma_m);
580 
581  // Find the landmark in MAP2 with the best (maximum)
582  // product-integral:
583  // (i^* , j^*) = arg max_(i,j) \int p_i()p_j()
584  //----------------------------------------------------------------------
585  const double ccos = cos(temptPose.mean.phi());
586  const double ssin = sin(temptPose.mean.phi());
587 
588  CMatrixDouble22 Hc; // Jacobian wrt point_j
589  Hc(1, 1) = ccos;
590  Hc(0, 0) = ccos;
591  Hc(1, 0) = ssin;
592  Hc(0, 1) = -ssin;
593 
595  Hq; // Jacobian wrt transformation q
596  Hq(0, 0) = 1;
597  Hq(1, 1) = 1;
598 
599  TPoint2D p2_j_local;
600  vector<float> matches_dist;
601  std::vector<size_t> matches_idx;
602 
603  CPoint2DPDFGaussian pdf_M2_j;
604  CPoint2DPDFGaussian pdf_M1_i;
605 
606 // Use integral-of-product vs. mahalanobis distances to match:
607 #define GRIDMAP_USE_PROD_INTEGRAL
608 
609 #ifdef GRIDMAP_USE_PROD_INTEGRAL
610  double best_pair_value = 0;
611 #else
612  double best_pair_value =
613  std::numeric_limits<double>::max();
614 #endif
615  double best_pair_d2 =
616  std::numeric_limits<double>::max();
617  pair<size_t, size_t> best_pair_ij;
618 
619  //#define SHOW_CORRS
620 
621 #ifdef SHOW_CORRS
622  CDisplayWindowPlots win("Matches");
623 #endif
624  for (size_t j = 0; j < nLM2; j++)
625  {
626  if (used_landmarks2[j]) continue;
627 
628  lm2_pnts.getPoint(
629  j, p2_j_local); // In local coords.
630  pdf_M2_j.mean = mrpt::poses::CPoint2D(
631  temptPose.mean +
632  p2_j_local); // In (temptative) global coords:
633  pdf_M2_j.cov(0, 0) = pdf_M2_j.cov(1, 1) =
634  square(options.ransac_SOG_sigma_m);
635 
636 #ifdef SHOW_CORRS
637  win.plotEllipse(
638  pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
639  pdf_M2_j.cov, 2, "b-",
640  format("M2_%u", (unsigned)j), true);
641 #endif
642 
643  static const unsigned int N_KDTREE_SEARCHED = 3;
644 
645  // Look for a few close features which may be
646  // potential matches:
647  lm1_pnts.kdTreeNClosestPoint2DIdx(
648  pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
649  N_KDTREE_SEARCHED, matches_idx, matches_dist);
650 
651  // And for each one, compute the product-integral:
652  for (unsigned long u : matches_idx)
653  {
654  if (used_landmarks1[u]) continue;
655 
656  // Jacobian wrt transformation q
657  Hq(0, 2) =
658  -p2_j_local.x * ssin - p2_j_local.y * ccos;
659  Hq(1, 2) =
660  p2_j_local.x * ccos - p2_j_local.y * ssin;
661 
662  // COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t
663  pdf_M1_i.cov =
664  mrpt::math::multiply_HCHt(Hc, COV_pnt);
665  pdf_M1_i.cov += mrpt::math::multiply_HCHt(
666  Hq, temptPose.cov);
667 
668  float px, py;
669  lm1_pnts.getPoint(u, px, py);
670  pdf_M1_i.mean.x(px);
671  pdf_M1_i.mean.y(py);
672 
673 #ifdef SHOW_CORRS
674  win.plotEllipse(
675  pdf_M1_i.mean.x(), pdf_M1_i.mean.y(),
676  pdf_M1_i.cov, 2, "r-",
677  format("M1_%u", (unsigned)matches_idx[u]),
678  true);
679 #endif
680 
681 // And now compute the product integral:
682 #ifdef GRIDMAP_USE_PROD_INTEGRAL
683  const double prod_ij =
684  pdf_M1_i.productIntegralWith(pdf_M2_j);
685 
686  if (prod_ij > best_pair_value)
687 #else
688  const double prod_ij =
689  pdf_M1_i.mean.distanceTo(pdf_M2_j.mean);
690  if (prod_ij < best_pair_value)
691 #endif
692  {
693  // const double prodint_ij =
694  // pdf_M1_i.productIntegralWith2D(pdf_M2_j);
695 
696  best_pair_value = prod_ij;
697  best_pair_ij.first = u;
698  best_pair_ij.second = j;
699 
700  best_pair_d2 =
701  square(pdf_M1_i.mahalanobisDistanceTo(
702  pdf_M2_j));
703  }
704  } // end for u (closest matches of LM2 in MAP 1)
705 
706 #ifdef SHOW_CORRS
707  win.axis_fit(true);
708  win.waitForKey();
709  win.clear();
710 #endif
711 
712  } // end for each LM2
713 
714  // Stop when the best choice has a bad mahal. dist.
715  keep_incorporating = false;
716 
717  // For the best (i,j), gate by the mahalanobis distance:
718  if (best_pair_d2 < chi2_thres_dim2)
719  {
720  // AND, also, check if the descriptors have some
721  // resemblance!!
722  // ----------------------------------------------------------------
723  // double feat_dist =
724  // lm1->landmarks.get(best_pair_ij.first)->features[0]->descriptorDistanceTo(*lm1->landmarks.get(best_pair_ij.second)->features[0]);
725  // if (feat_dist< options.threshold_max)
726  {
727  float p1_i_localx, p1_i_localy;
728  float p2_j_localx, p2_j_localy;
729  lm1_pnts.getPoint(
730  best_pair_ij.first, p1_i_localx,
731  p1_i_localy);
732  lm2_pnts.getPoint(
733  best_pair_ij.second, p2_j_localx,
734  p2_j_localy); // In local coords.
735 
736  used_landmarks1[best_pair_ij.first] = true;
737  used_landmarks2[best_pair_ij.second] = true;
738 
739  tentativeSubSet.push_back(
741  best_pair_ij.first, best_pair_ij.second,
742  p1_i_localx, p1_i_localy, 0, // MAP1
743  p2_j_localx, p2_j_localy, 0 // MAP2
744  ));
745 
746  keep_incorporating = true;
747  }
748  }
749 
750  } while (keep_incorporating);
751 
752  // Consider this pairing?
753  const size_t ninliers = tentativeSubSet.size();
754  if (ninliers > minInliersTOaccept)
755  {
757  newGauss.log_w = 0; // log(1); //
758  // std::log(static_cast<double>(nCoincidences));
759  newGauss.mean = temptPose.mean;
760  newGauss.cov = temptPose.cov;
761 
762  sog_modes[tentativeSubSet] = newGauss;
763 
764  // cout << "ITER: " << iter << " #inliers: " << ninliers
765  // << " q: " << temptPose.mean << endl;
766  }
767 
768  // Keep the largest consensus & dynamic # of steps:
769  if (largestConsensusCorrs.size() < ninliers)
770  {
771  largestConsensusCorrs = tentativeSubSet;
772 
773  // Update estimate of N, the number of trials to ensure
774  // we pick,
775  // with probability p, a data set with no outliers.
776  const double fracinliers =
777  ninliers /
778  static_cast<double>(std::min(nLM1, nLM2));
779  double pNoOutliers =
780  1 - pow(fracinliers,
781  static_cast<double>(
782  2.0)); // minimumSizeSamplesToFit
783 
784  pNoOutliers = std::max(
785  std::numeric_limits<double>::epsilon(),
786  pNoOutliers); // Avoid division by -Inf
787  pNoOutliers = std::min(
788  1.0 - std::numeric_limits<double>::epsilon(),
789  pNoOutliers); // Avoid division by 0.
790  // Number of
791  ransac_nSimulations =
792  log(1 - probability_find_good_model) /
793  log(pNoOutliers);
794 
795  if (ransac_nSimulations < ransac_min_nSimulations)
796  ransac_nSimulations = ransac_min_nSimulations;
797 
798  // if (verbose)
799  // cout << "[Align] Iter #" << iter << " Est. #iters: "
800  //<< ransac_nSimulations << " pNoOutliers=" <<
801  // pNoOutliers << " #inliers: " << ninliers << endl;
802  }
803 
804  } // end of RANSAC loop
805 
806  // Move SOG modes into pdf_SOG:
807  pdf_SOG->clear();
808  for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
809  {
811  "SOG mode: " << s->second.mean
812  << " inliers: " << s->first.size());
813  pdf_SOG->push_back(s->second);
814  }
815 
816  // Normalize:
817  if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights();
818 
819  // Simplify the SOG by merging close modes:
820  // -------------------------------------------------
821  size_t nB = pdf_SOG->size();
822  outInfo.sog1 = pdf_SOG;
823 
824  CTicTac merge_clock;
825  pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
826  const double merge_clock_tim = merge_clock.Tac();
827 
828  outInfo.sog2 = pdf_SOG;
829  size_t nA = pdf_SOG->size();
831  "[CGridMapAligner] amModifiedRANSAC: %u SOG modes "
832  "merged to %u in %.03fsec\n",
833  (unsigned int)nB, (unsigned int)nA, merge_clock_tim));
834 
835  } // end of amModifiedRANSAC
836 
837  // Save best corrs:
838  if (options.debug_save_map_pairs)
839  {
840  static unsigned int NN = 0;
841  static const COccupancyGridMap2D* lastM1 = nullptr;
842  if (lastM1 != m1)
843  {
844  lastM1 = m1;
845  NN = 0;
846  }
848  "Largest consensus: %u",
849  static_cast<unsigned>(largestConsensusCorrs.size()));
850  CEnhancedMetaFile::LINUX_IMG_WIDTH(
851  m1->getSizeX() + m2->getSizeX() + 50);
852  CEnhancedMetaFile::LINUX_IMG_HEIGHT(
853  max(m1->getSizeY(), m2->getSizeY()) + 50);
854 
855  for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
856  {
857  COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
858  format("__debug_corrsGrid_%05u.emf", NN), m1, m2,
859  s->first);
860  COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
861  format("__debug_corrsGrid_%05u.png", NN), m1, m2,
862  s->first);
863  ++NN;
864  }
865  }
866 
867  // --------------------------------------------------------------------
868  // At this point:
869  // - "pdf_SOG": has the resulting PDF with the SOG (from whatever
870  // method)
871  // - "largestConsensusCorrs": The 'best' set of correspondences
872  //
873  // Now: If we had a multi-metric map, use the points map to improve
874  // the estimation with ICP.
875  // --------------------------------------------------------------------
876  if (multimap1 && multimap2 &&
877  multimap1->countMapsByClass<CSimplePointsMap>() != 0 &&
878  multimap2->countMapsByClass<CSimplePointsMap>() != 0)
879  {
880  auto pnts1 = multimap1->mapByClass<CSimplePointsMap>();
881  auto pnts2 = multimap2->mapByClass<CSimplePointsMap>();
882 
883  CICP icp;
884  CICP::TReturnInfo icpInfo;
885 
886  icp.options.maxIterations = 20;
887  icp.options.smallestThresholdDist = 0.05f;
888  icp.options.thresholdDist = 0.75f;
889 
890  // Invoke ICP once for each mode in the SOG:
891  size_t cnt = 0;
892  outInfo.icp_goodness_all_sog_modes.clear();
893  for (auto i = pdf_SOG->begin(); i != pdf_SOG->end(); ++cnt)
894  {
895  CPosePDF::Ptr icp_est =
896  icp.Align(pnts1.get(), pnts2.get(), (i)->mean, icpInfo);
897 
898  //(i)->cov(0,0) += square( 0.05 );
899  //(i)->cov(1,1) += square( 0.05 );
900  //(i)->cov(2,2) += square( 0.05_deg );
901 
902  CPosePDFGaussian i_gauss(i->mean, i->cov);
903  CPosePDFGaussian icp_gauss(icp_est->getMeanVal(), i->cov);
904 
905  const double icp_maha_dist =
906  i_gauss.mahalanobisDistanceTo(icp_gauss);
907 
909  "ICP " << cnt << " log-w: " << i->log_w
910  << " Goodness: " << 100 * icpInfo.goodness
911  << " D_M= " << icp_maha_dist);
913  "final pos: " << icp_est->getMeanVal());
914  MRPT_LOG_INFO_STREAM(" org pos: " << i->mean);
915 
916  outInfo.icp_goodness_all_sog_modes.push_back(
917  icpInfo.goodness);
918 
919  // Discard?
920  if (icpInfo.goodness > options.min_ICP_goodness &&
921  icp_maha_dist < options.max_ICP_mahadist)
922  {
923  icp_est->getMean((i)->mean);
924  ++i;
925  }
926  else
927  {
928  // Delete:
929  i = pdf_SOG->erase(i);
930  }
931 
932  } // end for i
933 
934  // Merge:
935  outInfo.sog3 = pdf_SOG;
936 
937  pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
939  "[CGridMapAligner] " << pdf_SOG->size()
940  << " SOG modes merged after ICP.");
941 
942  } // end multimapX
943 
944  } // end of, yes, we have enough correspondences
945 
946  } // end of: yes, there are landmarks in the grid maps!
947 
948  outInfo.executionTime = tictac.Tac();
949 
950  return pdf_SOG;
951 
952  MRPT_END
953 }
954 
955 /*---------------------------------------------------------------
956  AlignPDF_correlation
957 ---------------------------------------------------------------*/
958 CPosePDF::Ptr CGridMapAligner::AlignPDF_correlation(
959  const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
960  [[maybe_unused]] const CPosePDFGaussian& initialEstimationPDF,
961  TReturnInfo& outInfo)
962 {
963  MRPT_START
964 
965  //#define CORRELATION_SHOW_DEBUG
966 
967  mrpt::system::CTicTac tictac;
968  tictac.Tic();
969 
970  // Asserts:
971  // -----------------
974  const auto* m1 = dynamic_cast<const COccupancyGridMap2D*>(mm1);
975  const auto* m2 = dynamic_cast<const COccupancyGridMap2D*>(mm2);
976 
977  ASSERT_EQUAL_(m1->getResolution(), m2->getResolution());
978 
979  // The PDF to estimate:
980  // ------------------------------------------------------
981  CPosePDFGaussian::Ptr PDF = std::make_shared<CPosePDFGaussian>();
982 
983  // Determine the extension to compute the correlation into:
984  // ----------------------------------------------------------
985  float phiResolution = DEG2RAD(0.2f);
986  float phiMin = -M_PIf + 0.5f * phiResolution;
987  float phiMax = M_PIf;
988 
989  // Compute the difference between maps for each (u,v) pair!
990  // --------------------------------------------------------------
991  float phi;
992  float pivotPt_x = 0.5f * (m1->getXMax() + m1->getXMin());
993  float pivotPt_y = 0.5f * (m1->getYMax() + m1->getYMin());
994  COccupancyGridMap2D map2_mod;
995  CImage map1_img, map2_img;
996  float currentMaxCorr = -1e6f;
997 
998  m1->getAsImage(map1_img);
999 
1000  map2_mod.setSize(
1001  m1->getXMin(), m1->getXMax(), m1->getYMin(), m1->getYMax(),
1002  m1->getResolution());
1003  size_t map2_lx = map2_mod.getSizeX();
1004  size_t map2_ly = map2_mod.getSizeY();
1005  CMatrixF outCrossCorr, bestCrossCorr;
1006  float map2width_2 = 0.5f * (map2_mod.getXMax() - map2_mod.getXMin());
1007 
1008 #ifdef CORRELATION_SHOW_DEBUG
1009  CDisplayWindow* win = new CDisplayWindow("corr");
1010  CDisplayWindow* win2 = new CDisplayWindow("map2_mod");
1011 #endif
1012 
1013  // --------------------------------------------------------
1014  // Use FFT-based correlation for each orientation:
1015  // --------------------------------------------------------
1016  for (phi = phiMin; phi < phiMax; phi += phiResolution)
1017  {
1018  // Create the displaced map2 grid, for the size of map1:
1019  // --------------------------------------------------------
1020  CPose2D v2(
1021  pivotPt_x - cos(phi) * map2width_2,
1022  pivotPt_y - sin(phi) * map2width_2,
1023  phi); // Rotation point: the centre of img1:
1024  CPoint2D v1, v3;
1025  v2 = CPose2D(0, 0, 0) - v2; // Inverse
1026 
1027  for (size_t cy2 = 0; cy2 < map2_ly; cy2++)
1028  {
1029  COccupancyGridMap2D::cellType* row = map2_mod.getRow(cy2);
1030  for (size_t cx2 = 0; cx2 < map2_lx; cx2++)
1031  {
1032  v3 = v2 + CPoint2D(map2_mod.idx2x(cx2), map2_mod.idx2y(cy2));
1033  *row++ = m2->p2l(m2->getPos(v3.x(), v3.y()));
1034  }
1035  }
1036 
1037  map2_mod.getAsImage(map2_img);
1038  // map2_img.saveToBMP("__debug_map2mod.bmp");
1039 
1040  // Compute the correlation:
1041  map1_img.cross_correlation_FFT(
1042  map2_img, outCrossCorr, -1, -1, -1, -1,
1043  127, // Bias to be substracted
1044  127 // Bias to be substracted
1045  );
1046 
1047  float corrPeak = outCrossCorr.maxCoeff();
1048  MRPT_LOG_INFO_FMT("phi = %fdeg \tmax corr=%f", RAD2DEG(phi), corrPeak);
1049 
1050  // Keep the maximum:
1051  if (corrPeak > currentMaxCorr)
1052  {
1053  currentMaxCorr = corrPeak;
1054  bestCrossCorr = outCrossCorr;
1055  PDF->mean.phi(phi);
1056  }
1057 
1058 #ifdef CORRELATION_SHOW_DEBUG
1059  outCrossCorr.adjustRange(0, 1);
1060  CImage aux(outCrossCorr, true);
1061  win->showImage(aux);
1062  win2->showImage(map2_img);
1063  std::this_thread::sleep_for(5ms);
1064 #endif
1065 
1066  } // end for phi
1067 
1068  outInfo.executionTime = tictac.Tac();
1069 
1070 #ifdef CORRELATION_SHOW_DEBUG
1071  CImage aux;
1072  aux.setFromMatrix(bestCrossCorr, false /* do normalization [0,1]*/);
1073  aux.saveToFile("_debug_best_corr.png");
1074  delete win;
1075  delete win2;
1076 #endif
1077 
1078  // Transform the best corr matrix peak into coordinates:
1079  std::size_t uMax, vMax;
1080  currentMaxCorr = bestCrossCorr.maxCoeff(uMax, vMax);
1081 
1082  PDF->mean.x(m1->idx2x(uMax));
1083  PDF->mean.y(m1->idx2y(vMax));
1084 
1085  return PDF;
1086 
1087  // Done!
1088  MRPT_END
1089 }
1090 
1091 /*---------------------------------------------------------------
1092  TConfigParams
1093  ---------------------------------------------------------------*/
1094 CGridMapAligner::TConfigParams::TConfigParams() : feature_detector_options() {}
1095 /*---------------------------------------------------------------
1096  dumpToTextStream
1097  ---------------------------------------------------------------*/
1099 {
1100  out << "\n----------- [CGridMapAligner::TConfigParams] ------------ \n\n";
1101 
1102  LOADABLEOPTS_DUMP_VAR(methodSelection, int)
1103  LOADABLEOPTS_DUMP_VAR(featsPerSquareMeter, float)
1104  LOADABLEOPTS_DUMP_VAR(threshold_max, float)
1105  LOADABLEOPTS_DUMP_VAR(threshold_delta, float)
1106  LOADABLEOPTS_DUMP_VAR(min_ICP_goodness, float)
1107  LOADABLEOPTS_DUMP_VAR(max_ICP_mahadist, double)
1108  LOADABLEOPTS_DUMP_VAR(maxKLd_for_merge, float)
1109  LOADABLEOPTS_DUMP_VAR(ransac_minSetSizeRatio, float)
1111  LOADABLEOPTS_DUMP_VAR(ransac_chi2_quantile, double)
1112  LOADABLEOPTS_DUMP_VAR(ransac_prob_good_inliers, double)
1113  LOADABLEOPTS_DUMP_VAR(ransac_SOG_sigma_m, float)
1114  LOADABLEOPTS_DUMP_VAR(save_feat_coors, bool)
1115  LOADABLEOPTS_DUMP_VAR(debug_show_corrs, bool)
1116  LOADABLEOPTS_DUMP_VAR(debug_save_map_pairs, bool)
1117 
1118  LOADABLEOPTS_DUMP_VAR(feature_descriptor, int)
1119 
1120  feature_detector_options.dumpToTextStream(out);
1121 
1122  out << "\n";
1123 }
1124 
1125 /*---------------------------------------------------------------
1126  loadFromConfigFile
1127  ---------------------------------------------------------------*/
1129  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
1130 {
1131  methodSelection =
1132  iniFile.read_enum(section, "methodSelection", methodSelection);
1133 
1135  featsPerSquareMeter, float, iniFile, section)
1136  MRPT_LOAD_CONFIG_VAR(ransac_SOG_sigma_m, float, iniFile, section)
1137 
1138  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_max, float, iniFile, section)
1139  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_delta, float, iniFile, section)
1140 
1141  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(min_ICP_goodness, float, iniFile, section)
1142  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(max_ICP_mahadist, double, iniFile, section)
1143 
1144  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxKLd_for_merge, float, iniFile, section)
1146  ransac_minSetSizeRatio, float, iniFile, section)
1150  ransac_chi2_quantile, double, iniFile, section)
1152  ransac_prob_good_inliers, double, iniFile, section)
1153 
1154  MRPT_LOAD_CONFIG_VAR(save_feat_coors, bool, iniFile, section)
1155  MRPT_LOAD_CONFIG_VAR(debug_show_corrs, bool, iniFile, section)
1156  MRPT_LOAD_CONFIG_VAR(debug_save_map_pairs, bool, iniFile, section)
1157 
1158  feature_descriptor = iniFile.read_enum(
1159  section, "feature_descriptor", feature_descriptor, true);
1160  feature_detector_options.loadFromConfigFile(iniFile, section);
1161 }
1162 
1164  [[maybe_unused]] const mrpt::maps::CMetricMap* m1,
1165  [[maybe_unused]] const mrpt::maps::CMetricMap* m2,
1166  [[maybe_unused]] const CPose3DPDFGaussian& initialEstimationPDF,
1167  [[maybe_unused]] mrpt::optional_ref<TMetricMapAlignmentResult> outInfo)
1168 {
1169  THROW_EXCEPTION("Align3D method not applicable to CGridMapAligner");
1170 }
Scalar maxCoeff() const
Maximum value in the matrix/vector.
A namespace of pseudo-random numbers generators of diferent distributions.
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
float getXMax() const
Returns the "x" coordinate of right side of grid map.
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
void 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:198
double getArea() const
Returns the area of the gridmap, in square meters.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
CPose2D mean
The mean value.
std::vector< TPairPlusDistance > correspondences_dists_maha
Mahalanobis distance for each potential correspondence.
The struct for each mode:
Definition: CPosePDFSOG.h:41
float getResolution() const
Returns the resolution of the grid map.
Template for column vectors of dynamic size, compatible with Eigen.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
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.
A gaussian distribution for 2D points.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool se2_l2(const mrpt::tfest::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:92
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
Parameters for se2_l2_robust().
Definition: se2.h:73
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
This file implements several operations that operate element-wise on individual or pairs of container...
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:92
unsigned int ransac_nSimulations
(Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations...
Definition: se2.h:85
void drawImage(int x, int y, const mrpt::img::CImage &img) override
Draws an image as a bitmap at a given position.
Definition: CImage.cpp:1142
A high-performance stopwatch, with typical resolution of nanoseconds.
unsigned int ransac_minSetSize
(Default=3)
Definition: se2.h:76
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog3
const float ransac_mahalanobisDistanceThreshold
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
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:658
STL namespace.
double mahalanobisDistanceTo(const CPosePDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
T::Ptr mapByClass(size_t ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
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...
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
The ICP algorithm return information.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
bool verbose
(Default=false)
Definition: se2.h:111
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
static bool myVectorOrder(const pair< size_t, float > &o1, const pair< size_t, float > &o2)
mrpt::tfest::TMatchingPairList correspondences
All the found correspondences (not consistent)
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
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.
bool se2_l2_robust(const mrpt::tfest::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 ...
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
unsigned int ransac_maxSetSize
(Default = 20)
Definition: se2.h:78
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
float getXMin() const
Returns the "x" coordinate of left side of grid map.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
constexpr double DEG2RAD(const double x)
Degrees to radians.
virtual void filledRectangle(int x0, int y0, int x1, int y1, const mrpt::img::TColor color)
Draws a filled rectangle.
Definition: CCanvas.cpp:205
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
bool ransac_algorithmForLandmarks
(Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
Definition: se2.h:99
mrpt::math::CMatrixDouble33 cov
Definition: CPosePDFSOG.h:45
#define M_PIf
Definition: common.h:61
string iniFile(myDataDir+string("benchmark-options.ini"))
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
void setFromMatrix(const MAT &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: img/CImage.h:844
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
#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
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:200
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
mrpt::gui::CDisplayWindow3D::Ptr win
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:80
A class used to store a 2D point.
Definition: CPoint2D.h:32
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:96
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
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:1435
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
return_t square(const num_t x)
Inline function for the square of a number.
#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...
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
CONTAINER::Scalar minimum(const CONTAINER &v)
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") ...
A class for storing an occupancy grid map.
void getAsImage(mrpt::img::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 ...
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...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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:42
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
Not applicable in this class, will launch an exception if used.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
mrpt::vision::TStereoCalibResults out
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
The ICP algorithm return information.
Definition: CICP.h:190
Lightweight 2D pose.
Definition: TPose2D.h:22
double mean(const CONTAINER &v)
Computes the mean value of a vector.
float idx2y(const size_t cy) const
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:133
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
OccGridCellTraits::cellType cellType
The type of the map cells:
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:218
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
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:902
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
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.
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog2
mrpt::tfest::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:135
This class stores any customizable set of metric maps.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::poses::CPose2D noRobustEstimation
The "brute" estimation from using all the available correspondences, provided just for comparison pur...
This template class provides the basic functionality for a general 2D any-size, resizable container o...
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog1
The different SOG densities at different steps of the algorithm:
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
Output placeholder for se2_l2_robust()
Definition: se2.h:130
#define MRPT_LOG_INFO_FMT(_FMT_STRING,...)
double ransac_fuseMaxDiffXY
(Default = 0.01)
Definition: se2.h:94
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
double probability_find_good_model
(Default = 0.999) See parameter ransac_nSimulations.
Definition: se2.h:103
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it&#39;s evaluation at (0...
#define MRPT_LOG_INFO(_STRING)
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020