Main MRPT website > C++ reference for MRPT 1.5.6
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 }
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
static CPosePDFGaussianPtr Create()
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
GLenum GLenum GLvoid * row
Definition: glew.h:2903
double y
X,Y coordinates.
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
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
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.
GLfloat GLfloat v1
Definition: glew.h:1759
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...
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
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
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
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)
GLfloat GLfloat GLfloat v2
Definition: glew.h:1763
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...
float getResolution() const
Returns the resolution of the grid map.
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.
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.
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.
GLdouble l
Definition: glew.h:5092
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
MRPT_TODO("Modify ping to run on Windows + Test this")
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
GLdouble s
Definition: glew.h:1295
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLsizei n
Definition: glew.h:5051
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
#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
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 ...
#define MRPT_LOG_INFO(_STRING)
#define MRPT_LOAD_CONFIG_VAR_CAST(variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
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
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
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
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
float getXMin() const
Returns the "x" coordinate of left side of grid map.
double getArea() const
Returns the area of the gridmap, in square meters.
#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.
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:66
A class used to store a 2D point.
Definition: CPoint2D.h:36
GLint * first
Definition: glew.h:1433
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:73
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
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
#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
#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...
float getXMax() const
Returns the "x" coordinate of right side of grid map.
#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)
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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
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
#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
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)
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 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
const GLdouble * m
Definition: glew.h:5094
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...
#define MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
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
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
GLfloat GLfloat GLfloat GLfloat v3
Definition: glew.h:1767
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
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.
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)
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
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
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
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it's evaluation at (0...
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
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.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
float idx2y(const size_t cy) const



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018