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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019