29 #include <Eigen/Dense>
51 switch (options.methodSelection)
53 case CGridMapAligner::amCorrelation:
54 ret = AlignPDF_correlation(mm1, mm2, initialEstimationPDF, infoVal);
57 case CGridMapAligner::amModifiedRANSAC:
58 case CGridMapAligner::amRobustMatch:
60 ret = AlignPDF_robustMatch(mm1, mm2, initialEstimationPDF, infoVal);
69 if (
auto* o =
dynamic_cast<TReturnInfo*
>(&info.value().get()); o)
77 const pair<size_t, float>& o1,
const pair<size_t, float>& o2)
79 return o1.second < o2.second;
92 options.methodSelection == CGridMapAligner::amRobustMatch ||
93 options.methodSelection == CGridMapAligner::amModifiedRANSAC);
103 std::vector<size_t> idxs1, idxs2;
104 std::vector<size_t>::iterator it1, it2;
133 "Metric maps must be of classes COccupancyGridMap2D or "
142 "Different resolutions for m1, m2:\n"
143 "\tres(m1) = %f\n\tres(m2) = %f\n",
164 m_grid_feat_extr.extractFeatures(
165 *m1, *lm1, N1, options.feature_descriptor,
166 options.feature_detector_options);
167 m_grid_feat_extr.extractFeatures(
168 *m2, *lm2, N2, options.feature_descriptor,
169 options.feature_detector_options);
174 const size_t nLM1 = lm1->size();
175 const size_t nLM2 = lm2->size();
179 if (nLM1 < 2 || nLM2 < 2)
193 unsigned int corrsCount = 0;
194 std::vector<bool> hasCorr(nLM1,
false);
196 const double thres_max = options.threshold_max;
197 const double thres_delta = options.threshold_delta;
200 if (options.debug_show_corrs)
204 std::cerr <<
"Warning: options.debug_show_corrs has no effect "
205 "since MRPT 0.9.1\n";
208 for (
size_t idx1 = 0; idx1 < nLM1; idx1++)
211 vector<pair<size_t, float>> corrs_indiv;
215 vector<float> corrs_indiv_only;
216 corrs_indiv.reserve(nLM2);
217 corrs_indiv_only.reserve(nLM2);
219 for (
size_t idx2 = 0; idx2 < nLM2; idx2++)
223 lm1->landmarks.get(idx1)->features[0].descriptorDistanceTo(
224 lm2->landmarks.get(idx2)->features[0]);
226 corrs_indiv.emplace_back(idx2, minDist);
227 corrs_indiv_only.push_back(minDist);
233 std::sort(corrs_indiv.begin(), corrs_indiv.end(),
myVectorOrder);
237 const size_t nBestToKeep = corrs_indiv.size();
239 for (
size_t w = 0; w < nBestToKeep; w++)
241 if (corrs_indiv[w].second <= thres_max &&
242 corrs_indiv[w].second <= (corr_best + thres_delta))
244 idxs1.push_back(idx1);
245 idxs2.push_back(corrs_indiv[w].first);
247 idx1, corrs_indiv[w].first, corrs_indiv[w].second);
253 if (options.save_feat_coors)
258 std::map<size_t, std::set<size_t>> corrs_by_idx;
259 for (
size_t l = 0; l < idxs1.size(); l++)
260 corrs_by_idx[idxs1[l]].insert(idxs2[l]);
262 for (
auto& it : corrs_by_idx)
265 lm1->landmarks.get(it.first)
267 .getFirstDescriptorAsMatrix(descriptor1);
271 const size_t FEAT_W = im1.
getWidth();
273 const size_t nF = it.second.size();
275 CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF);
278 img_compose.
getHeight() - 1, TColor::black());
283 std::set<size_t>::iterator it_j;
285 format(
"grid_feats/_FEAT_MATCH_%03i", (
int)it.first);
287 for (j = 0, it_j = it.second.begin(); j < nF; ++j, ++it_j)
289 fil +=
format(
"_%u",
static_cast<unsigned int>(*it_j));
292 lm2->landmarks.get(*it_j)
294 .getFirstDescriptorAsMatrix(descriptor2);
297 10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
307 correspondences.clear();
308 for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end();
313 mp.
this_x = lm1->landmarks.get(*it1)->pose_mean.x;
314 mp.
this_y = lm1->landmarks.get(*it1)->pose_mean.y;
315 mp.
this_z = lm1->landmarks.get(*it1)->pose_mean.z;
318 mp.
other_x = lm2->landmarks.get(*it2)->pose_mean.x;
319 mp.
other_y = lm2->landmarks.get(*it2)->pose_mean.y;
320 mp.
other_z = lm2->landmarks.get(*it2)->pose_mean.z;
321 correspondences.push_back(mp);
325 hasCorr[*it1] =
true;
330 outInfo.
goodness = (2.0f * corrsCount) / (nLM1 + nLM2);
340 outInfo.
sog1 = pdf_SOG;
341 outInfo.
sog2 = pdf_SOG;
342 outInfo.
sog3 = pdf_SOG;
348 "[CGridMapAligner] Overall estimation(%u corrs, total: "
349 "%u): (%.03f,%.03f,%.03fdeg)\n",
350 corrsCount, (
unsigned)correspondences.size(),
356 using TMapMatchingsToPoseMode = std::map<
358 TMapMatchingsToPoseMode sog_modes;
365 if (options.methodSelection == CGridMapAligner::amRobustMatch)
373 const unsigned int min_inliers =
374 options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2;
380 options.ransac_mahalanobisDistanceThreshold;
387 options.ransac_prob_good_inliers;
392 correspondences, options.ransac_SOG_sigma_m, tfest_params,
401 size_t nB = pdf_SOG->size();
402 outInfo.
sog1 = pdf_SOG;
406 best_mode.
log_w = -std::numeric_limits<double>::max();
408 for (
size_t n = 0; n < pdf_SOG->size(); n++)
416 pdf_SOG->push_back(best_mode);
417 outInfo.
sog2 = pdf_SOG;
420 "[CGridMapAligner] amRobustMatch: "
421 << nB <<
" SOG modes reduced to " << pdf_SOG->size()
422 <<
" (most-likely) (min.inliers=" << min_inliers <<
")\n");
432 const size_t nCorrs = all_corrs.size();
441 for (
size_t i = 0; i < nLM1; i++)
442 lm1_pnts.
insertPoint(lm1->landmarks.get(i)->pose_mean);
444 for (
size_t i = 0; i < nLM2; i++)
445 lm2_pnts.
insertPoint(lm2->landmarks.get(i)->pose_mean);
449 const size_t minInliersTOaccept =
450 round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2));
452 const unsigned int ransac_min_nSimulations =
454 unsigned int ransac_nSimulations =
457 const double probability_find_good_model = 0.9999;
459 const double chi2_thres_dim1 =
461 const double chi2_thres_dim2 =
467 COV_pnt(0, 0) = COV_pnt(1, 1) =
468 square(options.ransac_SOG_sigma_m);
478 const unsigned int max_trials =
479 (nCorrs * (nCorrs - 1) / 2) *
482 unsigned int iter = 0;
484 unsigned int trials = 0;
486 while (iter < ransac_nSimulations &&
500 }
while (idx1 == idx2);
503 if (all_corrs[idx1].this_idx == all_corrs[idx2].this_idx ||
504 all_corrs[idx1].this_idx == all_corrs[idx2].other_idx)
506 if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx ||
507 all_corrs[idx1].other_idx == all_corrs[idx2].other_idx)
514 const double corrs_dist1 =
516 all_corrs[idx1].this_x, all_corrs[idx1].this_y,
517 all_corrs[idx1].this_z, all_corrs[idx2].this_x,
518 all_corrs[idx2].this_y, all_corrs[idx2].this_z);
520 const double corrs_dist2 =
522 all_corrs[idx1].other_x, all_corrs[idx1].other_y,
523 all_corrs[idx1].other_z, all_corrs[idx2].other_x,
524 all_corrs[idx2].other_y, all_corrs[idx2].other_z);
528 const double corrs_dist_chi2 =
530 (8.0 *
square(options.ransac_SOG_sigma_m) *
533 if (corrs_dist_chi2 > chi2_thres_dim1)
continue;
539 bool is_new_hyp =
true;
540 for (
auto& sog_mode : sog_modes)
542 if (sog_mode.first.contains(all_corrs[idx1]) &&
543 sog_mode.first.contains(all_corrs[idx2]))
546 sog_mode.second.log_w =
547 std::log(std::exp(sog_mode.second.log_w) + 1.0);
552 if (!is_new_hyp)
continue;
555 tentativeSubSet.push_back(all_corrs[idx1]);
556 tentativeSubSet.push_back(all_corrs[idx2]);
560 std::vector<bool> used_landmarks1(nLM1,
false);
561 std::vector<bool> used_landmarks2(nLM2,
false);
563 used_landmarks1[all_corrs[idx1].this_idx] =
true;
564 used_landmarks1[all_corrs[idx2].this_idx] =
true;
565 used_landmarks2[all_corrs[idx1].other_idx] =
true;
566 used_landmarks2[all_corrs[idx2].other_idx] =
true;
570 bool keep_incorporating =
true;
579 temptPose.
cov *=
square(options.ransac_SOG_sigma_m);
585 const double ccos = cos(temptPose.
mean.
phi());
586 const double ssin = sin(temptPose.
mean.
phi());
600 vector<float> matches_dist;
601 std::vector<size_t> matches_idx;
607 #define GRIDMAP_USE_PROD_INTEGRAL
609 #ifdef GRIDMAP_USE_PROD_INTEGRAL
610 double best_pair_value = 0;
612 double best_pair_value =
613 std::numeric_limits<double>::max();
615 double best_pair_d2 =
616 std::numeric_limits<double>::max();
617 pair<size_t, size_t> best_pair_ij;
622 CDisplayWindowPlots
win(
"Matches");
624 for (
size_t j = 0; j < nLM2; j++)
626 if (used_landmarks2[j])
continue;
633 pdf_M2_j.
cov(0, 0) = pdf_M2_j.
cov(1, 1) =
634 square(options.ransac_SOG_sigma_m);
639 pdf_M2_j.
cov, 2,
"b-",
640 format(
"M2_%u", (
unsigned)j),
true);
643 static const unsigned int N_KDTREE_SEARCHED = 3;
649 N_KDTREE_SEARCHED, matches_idx, matches_dist);
652 for (
unsigned long u : matches_idx)
654 if (used_landmarks1[u])
continue;
658 -p2_j_local.
x * ssin - p2_j_local.
y * ccos;
660 p2_j_local.
x * ccos - p2_j_local.
y * ssin;
676 pdf_M1_i.
cov, 2,
"r-",
677 format(
"M1_%u", (
unsigned)matches_idx[u]),
682 #ifdef GRIDMAP_USE_PROD_INTEGRAL
683 const double prod_ij =
686 if (prod_ij > best_pair_value)
688 const double prod_ij =
690 if (prod_ij < best_pair_value)
696 best_pair_value = prod_ij;
697 best_pair_ij.first = u;
698 best_pair_ij.second = j;
715 keep_incorporating =
false;
718 if (best_pair_d2 < chi2_thres_dim2)
727 float p1_i_localx, p1_i_localy;
728 float p2_j_localx, p2_j_localy;
730 best_pair_ij.first, p1_i_localx,
733 best_pair_ij.second, p2_j_localx,
736 used_landmarks1[best_pair_ij.first] =
true;
737 used_landmarks2[best_pair_ij.second] =
true;
739 tentativeSubSet.push_back(
741 best_pair_ij.first, best_pair_ij.second,
742 p1_i_localx, p1_i_localy, 0,
743 p2_j_localx, p2_j_localy, 0
746 keep_incorporating =
true;
750 }
while (keep_incorporating);
753 const size_t ninliers = tentativeSubSet.size();
754 if (ninliers > minInliersTOaccept)
760 newGauss.
cov = temptPose.
cov;
762 sog_modes[tentativeSubSet] = newGauss;
769 if (largestConsensusCorrs.size() < ninliers)
771 largestConsensusCorrs = tentativeSubSet;
776 const double fracinliers =
778 static_cast<double>(std::min(nLM1, nLM2));
784 pNoOutliers = std::max(
785 std::numeric_limits<double>::epsilon(),
787 pNoOutliers = std::min(
788 1.0 - std::numeric_limits<double>::epsilon(),
791 ransac_nSimulations =
792 log(1 - probability_find_good_model) /
795 if (ransac_nSimulations < ransac_min_nSimulations)
796 ransac_nSimulations = ransac_min_nSimulations;
808 for (
auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
811 "SOG mode: " << s->second.mean
812 <<
" inliers: " << s->first.size());
813 pdf_SOG->push_back(s->second);
817 if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights();
821 size_t nB = pdf_SOG->size();
822 outInfo.
sog1 = pdf_SOG;
825 pdf_SOG->mergeModes(options.maxKLd_for_merge,
false);
826 const double merge_clock_tim = merge_clock.
Tac();
828 outInfo.
sog2 = pdf_SOG;
829 size_t nA = pdf_SOG->size();
831 "[CGridMapAligner] amModifiedRANSAC: %u SOG modes "
832 "merged to %u in %.03fsec\n",
833 (
unsigned int)nB, (
unsigned int)nA, merge_clock_tim));
838 if (options.debug_save_map_pairs)
840 static unsigned int NN = 0;
848 "Largest consensus: %u",
849 static_cast<unsigned>(largestConsensusCorrs.size()));
850 CEnhancedMetaFile::LINUX_IMG_WIDTH(
852 CEnhancedMetaFile::LINUX_IMG_HEIGHT(
855 for (
auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
857 COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
858 format(
"__debug_corrsGrid_%05u.emf", NN), m1, m2,
860 COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
861 format(
"__debug_corrsGrid_%05u.png", NN), m1, m2,
876 if (multimap1 && multimap2 &&
886 icp.options.maxIterations = 20;
887 icp.options.smallestThresholdDist = 0.05f;
888 icp.options.thresholdDist = 0.75f;
893 for (
auto i = pdf_SOG->begin(); i != pdf_SOG->end(); ++cnt)
896 icp.Align(pnts1.get(), pnts2.get(), (i)->
mean, icpInfo);
905 const double icp_maha_dist =
909 "ICP " << cnt <<
" log-w: " << i->log_w
910 <<
" Goodness: " << 100 * icpInfo.
goodness
911 <<
" D_M= " << icp_maha_dist);
913 "final pos: " << icp_est->getMeanVal());
920 if (icpInfo.
goodness > options.min_ICP_goodness &&
921 icp_maha_dist < options.max_ICP_mahadist)
923 icp_est->getMean((i)->
mean);
929 i = pdf_SOG->erase(i);
935 outInfo.
sog3 = pdf_SOG;
937 pdf_SOG->mergeModes(options.maxKLd_for_merge,
false);
939 "[CGridMapAligner] " << pdf_SOG->size()
940 <<
" SOG modes merged after ICP.");
985 float phiResolution =
DEG2RAD(0.2f);
986 float phiMin = -
M_PIf + 0.5f * phiResolution;
987 float phiMax =
M_PIf;
992 float pivotPt_x = 0.5f * (m1->getXMax() + m1->getXMin());
993 float pivotPt_y = 0.5f * (m1->getYMax() + m1->getYMin());
995 CImage map1_img, map2_img;
996 float currentMaxCorr = -1e6f;
998 m1->getAsImage(map1_img);
1001 m1->getXMin(), m1->getXMax(), m1->getYMin(), m1->getYMax(),
1002 m1->getResolution());
1003 size_t map2_lx = map2_mod.
getSizeX();
1004 size_t map2_ly = map2_mod.
getSizeY();
1005 CMatrixF outCrossCorr, bestCrossCorr;
1006 float map2width_2 = 0.5f * (map2_mod.
getXMax() - map2_mod.
getXMin());
1008 #ifdef CORRELATION_SHOW_DEBUG
1009 CDisplayWindow*
win =
new CDisplayWindow(
"corr");
1010 CDisplayWindow* win2 =
new CDisplayWindow(
"map2_mod");
1016 for (phi = phiMin; phi < phiMax; phi += phiResolution)
1021 pivotPt_x - cos(phi) * map2width_2,
1022 pivotPt_y - sin(phi) * map2width_2,
1027 for (
size_t cy2 = 0; cy2 < map2_ly; cy2++)
1030 for (
size_t cx2 = 0; cx2 < map2_lx; cx2++)
1033 *row++ = m2->p2l(m2->getPos(v3.
x(), v3.
y()));
1042 map2_img, outCrossCorr, -1, -1, -1, -1,
1047 float corrPeak = outCrossCorr.
maxCoeff();
1051 if (corrPeak > currentMaxCorr)
1053 currentMaxCorr = corrPeak;
1054 bestCrossCorr = outCrossCorr;
1058 #ifdef CORRELATION_SHOW_DEBUG
1059 outCrossCorr.adjustRange(0, 1);
1060 CImage aux(outCrossCorr,
true);
1061 win->showImage(aux);
1062 win2->showImage(map2_img);
1063 std::this_thread::sleep_for(5ms);
1070 #ifdef CORRELATION_SHOW_DEBUG
1079 std::size_t uMax, vMax;
1080 currentMaxCorr = bestCrossCorr.
maxCoeff(uMax, vMax);
1082 PDF->mean.x(m1->idx2x(uMax));
1083 PDF->mean.y(m1->idx2y(vMax));
1094 CGridMapAligner::TConfigParams::TConfigParams() : feature_detector_options() {}
1100 out <<
"\n----------- [CGridMapAligner::TConfigParams] ------------ \n\n";
1120 feature_detector_options.dumpToTextStream(
out);
1132 iniFile.read_enum(section,
"methodSelection", methodSelection);
1135 featsPerSquareMeter,
float,
iniFile, section)
1146 ransac_minSetSizeRatio,
float,
iniFile, section)
1150 ransac_chi2_quantile,
double,
iniFile, section)
1152 ransac_prob_good_inliers,
double,
iniFile, section)
1158 feature_descriptor =
iniFile.read_enum(
1159 section,
"feature_descriptor", feature_descriptor,
true);
1160 feature_detector_options.loadFromConfigFile(
iniFile, section);