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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019