/*---------------------------------------------------------------
mahalanobisDistanceTo
---------------------------------------------------------------*/
double CPointPDFGaussian::mahalanobisDistanceTo( const CPointPDFGaussian & other, bool only_2D ) const
{
// The difference in means:
CMatrixDouble13 deltaX;
deltaX.get_unsafe(0,0) = other.mean.x() - mean.x();
deltaX.get_unsafe(0,1) = other.mean.y() - mean.y();
deltaX.get_unsafe(0,2) = other.mean.z() - mean.z();
// The inverse of the combined covs:
CMatrixDouble33 COV = other.cov;
COV += this->cov;
if (!only_2D)
{
CMatrixDouble33 COV_inv;
COV.inv(COV_inv);
return sqrt( deltaX.multiply_HCHt_scalar(COV_inv) );
}
else
{
CMatrixDouble22 C = COV.block(0,0,2,2);
CMatrixDouble22 COV_inv;
C.inv(COV_inv);
CMatrixDouble12 deltaX2 = deltaX.block(0,0,1,2);
return std::sqrt( deltaX2.multiply_HCHt_scalar(COV_inv) );
}
}
/*---------------------------------------------------------------
productIntegralWith2D
---------------------------------------------------------------*/
double CPointPDFGaussian::productIntegralWith2D( const CPointPDFGaussian &p) const
{
MRPT_START
// --------------------------------------------------------------
// 12/APR/2009 - Jose Luis Blanco:
// The integral over all the variable space of the product of two
// Gaussians variables amounts to simply the evaluation of
// a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
// ---------------------------------------------------------------
CMatrixDouble22 C = cov.block(0,0,2,2);
C+=p.cov.block(0,0,2,2); // Sum of covs:
CMatrixDouble22 C_inv;
C.inv(C_inv);
CMatrixDouble21 MU(UNINITIALIZED_MATRIX); // Diff. of means
MU.get_unsafe(0,0) = mean.x() - p.mean.x();
MU.get_unsafe(1,0) = mean.y() - p.mean.y();
return std::pow( M_2PI, -0.5*(state_length-1) )
* (1.0/std::sqrt( C.det() ))
* exp( -0.5* MU.multiply_HtCH_scalar(C_inv) );
MRPT_END
}
/*---------------------------------------------------------------
bayesianFusion
---------------------------------------------------------------*/
void CPoint2DPDFGaussian::bayesianFusion( const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2 )
{
MRPT_START
CMatrixDouble22 C1_inv;
p1.cov.inv(C1_inv);
CMatrixDouble22 C2_inv;
p2.cov.inv(C2_inv);
CMatrixDouble22 L = C1_inv;
L+= C2_inv;
L.inv(cov); // The new cov.
CMatrixDouble21 x1 = CMatrixDouble21(p1.mean);
CMatrixDouble21 x2 = CMatrixDouble21(p2.mean);
CMatrixDouble21 x = cov * ( C1_inv*x1 + C2_inv*x2 );
mean.x( x.get_unsafe(0,0) );
mean.y( x.get_unsafe(1,0) );
std::cout << "IN1: " << p1.mean << "\n" << p1.cov << "\n";
std::cout << "IN2: " << p2.mean << "\n" << p2.cov << "\n";
std::cout << "OUT: " << mean << "\n" << cov << "\n";
MRPT_END
}
/*---------------------------------------------------------------
changeCoordinatesReference
---------------------------------------------------------------*/
void CPoint2DPDFGaussian::changeCoordinatesReference(const CPose3D &newReferenceBase )
{
// Clip the 3x3 rotation matrix
const CMatrixDouble22 M = newReferenceBase.getRotationMatrix().block(0,0,2,2);
// The mean:
mean = CPoint2D( newReferenceBase + mean );
// The covariance:
cov = M*cov*M.transpose();
}
/*---------------------------------------------------------------
AlignPDF_robustMatch
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch(
const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
void* info)
{
MRPT_START
ASSERT_(
options.methodSelection == CGridMapAligner::amRobustMatch ||
options.methodSelection == CGridMapAligner::amModifiedRANSAC);
TReturnInfo outInfo;
mrpt::tfest::TMatchingPairList& correspondences =
outInfo.correspondences; // Use directly this placeholder to save 1
// variable & 1 copy.
mrpt::tfest::TMatchingPairList largestConsensusCorrs;
CTicTac* tictac = nullptr;
CPose2D grossEst = initialEstimationPDF.mean;
CLandmarksMap::Ptr lm1(new CLandmarksMap());
CLandmarksMap::Ptr lm2(new CLandmarksMap());
std::vector<size_t> idxs1, idxs2;
std::vector<size_t>::iterator it1, it2;
// Asserts:
// -----------------
const CMultiMetricMap* multimap1 = nullptr;
const CMultiMetricMap* multimap2 = nullptr;
const COccupancyGridMap2D* m1 = nullptr;
const COccupancyGridMap2D* m2 = nullptr;
if (IS_CLASS(mm1, CMultiMetricMap) && IS_CLASS(mm2, CMultiMetricMap))
{
multimap1 = static_cast<const CMultiMetricMap*>(mm1);
multimap2 = static_cast<const CMultiMetricMap*>(mm2);
ASSERT_(multimap1->m_gridMaps.size() && multimap1->m_gridMaps[0]);
ASSERT_(multimap2->m_gridMaps.size() && multimap2->m_gridMaps[0]);
m1 = multimap1->m_gridMaps[0].get();
m2 = multimap2->m_gridMaps[0].get();
}
else if (
IS_CLASS(mm1, COccupancyGridMap2D) &&
IS_CLASS(mm2, COccupancyGridMap2D))
{
m1 = static_cast<const COccupancyGridMap2D*>(mm1);
m2 = static_cast<const COccupancyGridMap2D*>(mm2);
}
else
THROW_EXCEPTION(
"Metric maps must be of classes COccupancyGridMap2D or "
"CMultiMetricMap");
ASSERTMSG_(
m1->getResolution() == m2->getResolution(),
mrpt::format(
"Different resolutions for m1, m2:\n"
"\tres(m1) = %f\n\tres(m2) = %f\n",
m1->getResolution(), m2->getResolution()));
// The algorithm output auxiliar info:
// -------------------------------------------------
outInfo.goodness = 1.0f;
outInfo.landmarks_map1 = lm1;
outInfo.landmarks_map2 = lm2;
// The PDF to estimate:
// ------------------------------------------------------
CPosePDFSOG::Ptr pdf_SOG = mrpt::make_aligned_shared<CPosePDFSOG>();
// Extract features from grid-maps:
// ------------------------------------------------------
const size_t N1 =
std::max(40, mrpt::round(m1->getArea() * options.featsPerSquareMeter));
const size_t N2 =
std::max(40, mrpt::round(m2->getArea() * options.featsPerSquareMeter));
m_grid_feat_extr.extractFeatures(
*m1, *lm1, N1, options.feature_descriptor,
options.feature_detector_options);
m_grid_feat_extr.extractFeatures(
*m2, *lm2, N2, options.feature_descriptor,
options.feature_detector_options);
if (runningTime)
{
tictac = new CTicTac();
tictac->Tic();
}
const size_t nLM1 = lm1->size();
const size_t nLM2 = lm2->size();
// At least two landmarks at each map!
// ------------------------------------------------------
if (nLM1 < 2 || nLM2 < 2)
{
outInfo.goodness = 0;
}
else
{
//#define METHOD_FFT
//#define DEBUG_SHOW_CORRELATIONS
// Compute correlation between landmarks:
// ---------------------------------------------
CMatrixFloat CORR(lm1->size(), lm2->size()), auxCorr;
CImage im1, im2; // Grayscale
CVectorFloat corr;
unsigned int corrsCount = 0;
std::vector<bool> hasCorr(nLM1, false);
const double thres_max = options.threshold_max;
const double thres_delta = options.threshold_delta;
// CDisplayWindowPlots::Ptr auxWin;
if (options.debug_show_corrs)
{
// auxWin = CDisplayWindowPlots::Ptr( new
// CDisplayWindowPlots("Individual corr.") );
std::cerr << "Warning: options.debug_show_corrs has no effect "
"since MRPT 0.9.1\n";
}
for (size_t idx1 = 0; idx1 < nLM1; idx1++)
{
// CVectorFloat corrs_indiv;
vector<pair<size_t, float>> corrs_indiv; // (index, distance);
// Index is used to
// recover the original
// index after sorting.
vector<float> corrs_indiv_only;
corrs_indiv.reserve(nLM2);
corrs_indiv_only.reserve(nLM2);
for (size_t idx2 = 0; idx2 < nLM2; idx2++)
{
float minDist;
minDist =
lm1->landmarks.get(idx1)->features[0]->descriptorDistanceTo(
*lm2->landmarks.get(idx2)->features[0]);
corrs_indiv.emplace_back(idx2, minDist);
corrs_indiv_only.push_back(minDist);
} // end for idx2
// double corr_mean,corr_std;
// mrpt::math::meanAndStd(corrs_indiv_only,corr_mean,corr_std);
const double corr_best = mrpt::math::minimum(corrs_indiv_only);
// cout << "M: " << corr_mean << " std: " << corr_std << " best: "
// << corr_best << endl;
// Sort the list and keep the N best features:
std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder);
// const size_t nBestToKeep = std::min( (size_t)30,
// corrs_indiv.size() );
const size_t nBestToKeep = corrs_indiv.size();
for (size_t w = 0; w < nBestToKeep; w++)
{
if (corrs_indiv[w].second <= thres_max &&
corrs_indiv[w].second <= (corr_best + thres_delta))
{
idxs1.push_back(idx1);
idxs2.push_back(corrs_indiv[w].first);
outInfo.correspondences_dists_maha.emplace_back(
idx1, corrs_indiv[w].first, corrs_indiv[w].second);
}
}
} // end for idx1
// Save an image with each feature and its matches:
if (options.save_feat_coors)
{
mrpt::system::deleteFilesInDirectory("grid_feats");
mrpt::system::createDirectory("grid_feats");
std::map<size_t, std::set<size_t>> corrs_by_idx;
for (size_t l = 0; l < idxs1.size(); l++)
corrs_by_idx[idxs1[l]].insert(idxs2[l]);
for (auto& it : corrs_by_idx)
{
CMatrixFloat descriptor1;
lm1->landmarks.get(it.first)
->features[0]
->getFirstDescriptorAsMatrix(descriptor1);
im1 = CImage(descriptor1, true);
const size_t FEAT_W = im1.getWidth();
const size_t FEAT_H = im1.getHeight();
const size_t nF = it.second.size();
CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF);
img_compose.filledRectangle(
0, 0, img_compose.getWidth() - 1,
img_compose.getHeight() - 1, TColor::black());
img_compose.drawImage(5, 5, im1);
size_t j;
std::set<size_t>::iterator it_j;
string fil =
format("grid_feats/_FEAT_MATCH_%03i", (int)it.first);
for (j = 0, it_j = it.second.begin(); j < nF; ++j, ++it_j)
{
fil += format("_%u", static_cast<unsigned int>(*it_j));
CMatrixFloat descriptor2;
lm2->landmarks.get(*it_j)
->features[0]
->getFirstDescriptorAsMatrix(descriptor2);
im2 = CImage(descriptor2, true);
img_compose.drawImage(
10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
}
fil += ".png";
img_compose.saveToFile(fil);
} // end for map
}
// ------------------------------------------------------------------
// Create the list of correspondences from the lists: idxs1 & idxs2
// ------------------------------------------------------------------
correspondences.clear();
for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end();
++it1, ++it2)
{
mrpt::tfest::TMatchingPair mp;
mp.this_idx = *it1;
mp.this_x = lm1->landmarks.get(*it1)->pose_mean.x;
mp.this_y = lm1->landmarks.get(*it1)->pose_mean.y;
mp.this_z = lm1->landmarks.get(*it1)->pose_mean.z;
mp.other_idx = *it2;
mp.other_x = lm2->landmarks.get(*it2)->pose_mean.x;
mp.other_y = lm2->landmarks.get(*it2)->pose_mean.y;
mp.other_z = lm2->landmarks.get(*it2)->pose_mean.z;
correspondences.push_back(mp);
if (!hasCorr[*it1])
{
hasCorr[*it1] = true;
corrsCount++;
}
} // end for corres.
outInfo.goodness = (2.0f * corrsCount) / (nLM1 + nLM2);
// Compute the estimation using ALL the correspondences (NO ROBUST):
// ----------------------------------------------------------------------
mrpt::math::TPose2D noRobustEst;
if (!mrpt::tfest::se2_l2(correspondences, noRobustEst))
{
// There's no way to match the maps! e.g. no correspondences
outInfo.goodness = 0;
pdf_SOG->clear();
outInfo.sog1 = pdf_SOG;
outInfo.sog2 = pdf_SOG;
outInfo.sog3 = pdf_SOG;
}
else
{
outInfo.noRobustEstimation = mrpt::poses::CPose2D(noRobustEst);
MRPT_LOG_INFO(mrpt::format(
"[CGridMapAligner] Overall estimation(%u corrs, total: "
"%u): (%.03f,%.03f,%.03fdeg)\n",
corrsCount, (unsigned)correspondences.size(),
outInfo.noRobustEstimation.x(), outInfo.noRobustEstimation.y(),
RAD2DEG(outInfo.noRobustEstimation.phi())));
// The list of SOG modes & their corresponding sub-sets of
// matchings:
using TMapMatchingsToPoseMode = mrpt::aligned_std_map<
mrpt::tfest::TMatchingPairList, CPosePDFSOG::TGaussianMode>;
TMapMatchingsToPoseMode sog_modes;
// ---------------------------------------------------------------
// Now, we have to choose between the methods:
// - CGridMapAligner::amRobustMatch ("normal" RANSAC)
// - CGridMapAligner::amModifiedRANSAC
// ---------------------------------------------------------------
if (options.methodSelection == CGridMapAligner::amRobustMatch)
{
// ====================================================
// METHOD: "Normal" RANSAC
// ====================================================
// RANSAC over the found correspondences:
// -------------------------------------------------
const unsigned int min_inliers =
options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2;
mrpt::tfest::TSE2RobustParams tfest_params;
tfest_params.ransac_minSetSize = min_inliers;
tfest_params.ransac_maxSetSize = nLM1 + nLM2;
tfest_params.ransac_mahalanobisDistanceThreshold =
options.ransac_mahalanobisDistanceThreshold;
tfest_params.ransac_nSimulations = 0; // 0=auto
tfest_params.ransac_fuseByCorrsMatch = true;
tfest_params.ransac_fuseMaxDiffXY = 0.01;
tfest_params.ransac_fuseMaxDiffPhi = DEG2RAD(0.1);
tfest_params.ransac_algorithmForLandmarks = true;
tfest_params.probability_find_good_model =
options.ransac_prob_good_inliers;
tfest_params.verbose = false;
mrpt::tfest::TSE2RobustResult tfest_result;
mrpt::tfest::se2_l2_robust(
correspondences, options.ransac_SOG_sigma_m, tfest_params,
tfest_result);
ASSERT_(pdf_SOG);
*pdf_SOG = tfest_result.transformation;
largestConsensusCorrs = tfest_result.largestSubSet;
// Simplify the SOG by merging close modes:
// -------------------------------------------------
size_t nB = pdf_SOG->size();
outInfo.sog1 = pdf_SOG;
// Keep only the most-likely Gaussian mode:
CPosePDFSOG::TGaussianMode best_mode;
best_mode.log_w = -std::numeric_limits<double>::max();
for (size_t n = 0; n < pdf_SOG->size(); n++)
{
const CPosePDFSOG::TGaussianMode& m = (*pdf_SOG)[n];
if (m.log_w > best_mode.log_w) best_mode = m;
}
pdf_SOG->clear();
pdf_SOG->push_back(best_mode);
outInfo.sog2 = pdf_SOG;
MRPT_LOG_INFO_STREAM(
"[CGridMapAligner] amRobustMatch: "
<< nB << " SOG modes reduced to " << pdf_SOG->size()
<< " (most-likely) (min.inliers=" << min_inliers << ")\n");
} // end of amRobustMatch
else
{
// ====================================================
// METHOD: "Modified" RANSAC
// ====================================================
mrpt::tfest::TMatchingPairList all_corrs = correspondences;
const size_t nCorrs = all_corrs.size();
ASSERT_(nCorrs >= 2);
pdf_SOG->clear(); // Start with 0 Gaussian modes
// Build a points-map for exploiting its KD-tree:
// ----------------------------------------------------
CSimplePointsMap lm1_pnts, lm2_pnts;
lm1_pnts.reserve(nLM1);
for (size_t i = 0; i < nLM1; i++)
lm1_pnts.insertPoint(lm1->landmarks.get(i)->pose_mean);
lm2_pnts.reserve(nLM2);
for (size_t i = 0; i < nLM2; i++)
lm2_pnts.insertPoint(lm2->landmarks.get(i)->pose_mean);
// RANSAC loop
// ---------------------
const size_t minInliersTOaccept =
round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2));
// Set an initial # of iterations:
const unsigned int ransac_min_nSimulations =
2 * (nLM1 + nLM2); // 1000;
unsigned int ransac_nSimulations =
10; // It doesn't matter actually, since will be changed in
// the first loop
const double probability_find_good_model = 0.9999;
const double chi2_thres_dim1 =
mrpt::math::chi2inv(options.ransac_chi2_quantile, 1);
const double chi2_thres_dim2 =
mrpt::math::chi2inv(options.ransac_chi2_quantile, 2);
// Generic 2x2 covariance matrix for all features in their local
// coords:
CMatrixDouble22 COV_pnt;
COV_pnt.get_unsafe(0, 0) = COV_pnt.get_unsafe(1, 1) =
square(options.ransac_SOG_sigma_m);
// The absolute number of trials.
// in practice it's only important for a reduced number of
// correspondences, to avoid a dead-lock:
// It's the binomial coefficient:
// / n \ n! n * (n-1)
// | | = ----------- = -----------
// \ 2 / 2! (n-2)! 2
//
const unsigned int max_trials =
(nCorrs * (nCorrs - 1) / 2) *
5; // "*5" is just for safety...
unsigned int iter = 0; // Valid iterations (those passing the
// first mahalanobis test)
unsigned int trials = 0; // counter of all iterations,
// including "iter" + failing ones.
while (iter < ransac_nSimulations &&
trials <
max_trials) // ransac_nSimulations can be dynamic
{
trials++;
mrpt::tfest::TMatchingPairList tentativeSubSet;
// Pick 2 random correspondences:
uint32_t idx1, idx2;
idx1 = getRandomGenerator().drawUniform32bit() % nCorrs;
do
{
idx2 = getRandomGenerator().drawUniform32bit() % nCorrs;
} while (idx1 == idx2); // Avoid a degenerated case!
// Uniqueness of features:
if (all_corrs[idx1].this_idx == all_corrs[idx2].this_idx ||
all_corrs[idx1].this_idx == all_corrs[idx2].other_idx)
continue;
if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx ||
all_corrs[idx1].other_idx == all_corrs[idx2].other_idx)
continue;
// Check the feasibility of this pair "idx1"-"idx2":
// The distance between the pair of points in MAP1 must be
// very close
// to that of their correspondences in MAP2:
const double corrs_dist1 =
mrpt::math::distanceBetweenPoints(
all_corrs[idx1].this_x, all_corrs[idx1].this_y,
all_corrs[idx1].this_z, all_corrs[idx2].this_x,
all_corrs[idx2].this_y, all_corrs[idx2].this_z);
const double corrs_dist2 =
mrpt::math::distanceBetweenPoints(
all_corrs[idx1].other_x, all_corrs[idx1].other_y,
all_corrs[idx1].other_z, all_corrs[idx2].other_x,
all_corrs[idx2].other_y, all_corrs[idx2].other_z);
// Is is a consistent possibility?
// We use a chi2 test (see paper for the derivation)
const double corrs_dist_chi2 =
square(square(corrs_dist1) - square(corrs_dist2)) /
(8.0 * square(options.ransac_SOG_sigma_m) *
(square(corrs_dist1) + square(corrs_dist2)));
if (corrs_dist_chi2 > chi2_thres_dim1) continue; // Nope
iter++; // Do not count iterations if they fail the test
// above.
// before proceeding with this hypothesis, is it an old one?
bool is_new_hyp = true;
for (auto& sog_mode : sog_modes)
{
if (sog_mode.first.contains(all_corrs[idx1]) &&
sog_mode.first.contains(all_corrs[idx2]))
{
// Increment weight:
sog_mode.second.log_w =
std::log(std::exp(sog_mode.second.log_w) + 1.0);
is_new_hyp = false;
break;
}
}
if (!is_new_hyp) continue;
// Ok, it's a new hypothesis:
tentativeSubSet.push_back(all_corrs[idx1]);
tentativeSubSet.push_back(all_corrs[idx2]);
// Maintain a list of already used landmarks IDs in both
// maps to avoid repetitions:
std::vector<bool> used_landmarks1(nLM1, false);
std::vector<bool> used_landmarks2(nLM2, false);
used_landmarks1[all_corrs[idx1].this_idx] = true;
used_landmarks1[all_corrs[idx2].this_idx] = true;
used_landmarks2[all_corrs[idx1].other_idx] = true;
used_landmarks2[all_corrs[idx2].other_idx] = true;
// Build the transformation for these temptative
// correspondences:
bool keep_incorporating = true;
CPosePDFGaussian temptPose;
do // Incremently incorporate inliers:
{
if (!mrpt::tfest::se2_l2(tentativeSubSet, temptPose))
continue; // Invalid matching...
// The computed cov is "normalized", i.e. must be
// multiplied by std^2_xy
temptPose.cov *= square(options.ransac_SOG_sigma_m);
// cout << "q: " << temptPose << endl;
// Find the landmark in MAP2 with the best (maximum)
// product-integral:
// (i^* , j^*) = arg max_(i,j) \int p_i()p_j()
//----------------------------------------------------------------------
const double ccos = cos(temptPose.mean.phi());
const double ssin = sin(temptPose.mean.phi());
CMatrixDouble22 Hc; // Jacobian wrt point_j
Hc.get_unsafe(1, 1) = ccos;
Hc.get_unsafe(0, 0) = ccos;
Hc.get_unsafe(1, 0) = ssin;
Hc.get_unsafe(0, 1) = -ssin;
CMatrixFixedNumeric<double, 2, 3>
Hq; // Jacobian wrt transformation q
Hq(0, 0) = 1;
Hq(1, 1) = 1;
TPoint2D p2_j_local;
vector<float> matches_dist;
std::vector<size_t> matches_idx;
CPoint2DPDFGaussian pdf_M2_j;
CPoint2DPDFGaussian pdf_M1_i;
// Use integral-of-product vs. mahalanobis distances to match:
#define GRIDMAP_USE_PROD_INTEGRAL
#ifdef GRIDMAP_USE_PROD_INTEGRAL
double best_pair_value = 0;
#else
double best_pair_value =
std::numeric_limits<double>::max();
#endif
double best_pair_d2 =
std::numeric_limits<double>::max();
pair<size_t, size_t> best_pair_ij;
//#define SHOW_CORRS
#ifdef SHOW_CORRS
CDisplayWindowPlots win("Matches");
#endif
for (size_t j = 0; j < nLM2; j++)
{
if (used_landmarks2[j]) continue;
lm2_pnts.getPoint(
j, p2_j_local); // In local coords.
pdf_M2_j.mean = mrpt::poses::CPoint2D(
temptPose.mean +
p2_j_local); // In (temptative) global coords:
pdf_M2_j.cov.get_unsafe(0, 0) =
pdf_M2_j.cov.get_unsafe(1, 1) =
square(options.ransac_SOG_sigma_m);
#ifdef SHOW_CORRS
win.plotEllipse(
pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
pdf_M2_j.cov, 2, "b-",
format("M2_%u", (unsigned)j), true);
#endif
static const unsigned int N_KDTREE_SEARCHED = 3;
// Look for a few close features which may be
// potential matches:
lm1_pnts.kdTreeNClosestPoint2DIdx(
pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
N_KDTREE_SEARCHED, matches_idx, matches_dist);
// And for each one, compute the product-integral:
for (unsigned long u : matches_idx)
{
if (used_landmarks1[u]) continue;
// Jacobian wrt transformation q
Hq.get_unsafe(0, 2) =
-p2_j_local.x * ssin - p2_j_local.y * ccos;
Hq.get_unsafe(1, 2) =
p2_j_local.x * ccos - p2_j_local.y * ssin;
// COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t
Hc.multiply_HCHt(COV_pnt, pdf_M1_i.cov);
Hq.multiply_HCHt(
temptPose.cov, pdf_M1_i.cov, true);
float px, py;
lm1_pnts.getPoint(u, px, py);
pdf_M1_i.mean.x(px);
pdf_M1_i.mean.y(py);
#ifdef SHOW_CORRS
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);
#endif
// And now compute the product integral:
#ifdef GRIDMAP_USE_PROD_INTEGRAL
const double prod_ij =
pdf_M1_i.productIntegralWith(pdf_M2_j);
// const double prod_ij_d2 = square(
// pdf_M1_i.mahalanobisDistanceTo(pdf_M2_j) );
if (prod_ij > best_pair_value)
#else
const double prod_ij =
pdf_M1_i.mean.distanceTo(pdf_M2_j.mean);
if (prod_ij < best_pair_value)
#endif
{
// const double prodint_ij =
// pdf_M1_i.productIntegralWith2D(pdf_M2_j);
best_pair_value = prod_ij;
best_pair_ij.first = u;
best_pair_ij.second = j;
best_pair_d2 =
square(pdf_M1_i.mahalanobisDistanceTo(
pdf_M2_j));
// cout << "P1: " << pdf_M1_i.mean << " C= "
// << pdf_M1_i.cov.inMatlabFormat() << endl;
// cout << "P2: " << pdf_M2_j.mean << " C= "
// << pdf_M2_j.cov.inMatlabFormat() << endl;
// cout << " -> " << format("%e",prod_ij)
// << " d2: " << best_pair_d2 << endl <<
// endl;
}
} // end for u (closest matches of LM2 in MAP 1)
#ifdef SHOW_CORRS
win.axis_fit(true);
win.waitForKey();
win.clear();
#endif
} // end for each LM2
// Stop when the best choice has a bad mahal. dist.
keep_incorporating = false;
// For the best (i,j), gate by the mahalanobis distance:
if (best_pair_d2 < chi2_thres_dim2)
{
// AND, also, check if the descriptors have some
// resemblance!!
// ----------------------------------------------------------------
// double feat_dist =
// lm1->landmarks.get(best_pair_ij.first)->features[0]->descriptorDistanceTo(*lm1->landmarks.get(best_pair_ij.second)->features[0]);
// if (feat_dist< options.threshold_max)
{
float p1_i_localx, p1_i_localy;
float p2_j_localx, p2_j_localy;
lm1_pnts.getPoint(
best_pair_ij.first, p1_i_localx,
p1_i_localy);
lm2_pnts.getPoint(
best_pair_ij.second, p2_j_localx,
p2_j_localy); // In local coords.
used_landmarks1[best_pair_ij.first] = true;
used_landmarks2[best_pair_ij.second] = true;
tentativeSubSet.push_back(
mrpt::tfest::TMatchingPair(
best_pair_ij.first, best_pair_ij.second,
p1_i_localx, p1_i_localy, 0, // MAP1
p2_j_localx, p2_j_localy, 0 // MAP2
));
keep_incorporating = true;
}
}
} while (keep_incorporating);
// Consider this pairing?
const size_t ninliers = tentativeSubSet.size();
if (ninliers > minInliersTOaccept)
{
CPosePDFSOG::TGaussianMode newGauss;
newGauss.log_w = 0; // log(1); //
// std::log(static_cast<double>(nCoincidences));
newGauss.mean = temptPose.mean;
newGauss.cov = temptPose.cov;
sog_modes[tentativeSubSet] = newGauss;
// cout << "ITER: " << iter << " #inliers: " << ninliers
// << " q: " << temptPose.mean << endl;
}
// Keep the largest consensus & dynamic # of steps:
if (largestConsensusCorrs.size() < ninliers)
{
largestConsensusCorrs = tentativeSubSet;
// Update estimate of N, the number of trials to ensure
// we pick,
// with probability p, a data set with no outliers.
const double fracinliers =
ninliers /
static_cast<double>(std::min(nLM1, nLM2));
double pNoOutliers =
1 - pow(fracinliers,
static_cast<double>(
2.0)); // minimumSizeSamplesToFit
pNoOutliers = std::max(
std::numeric_limits<double>::epsilon(),
pNoOutliers); // Avoid division by -Inf
pNoOutliers = std::min(
1.0 - std::numeric_limits<double>::epsilon(),
pNoOutliers); // Avoid division by 0.
// Number of
ransac_nSimulations =
log(1 - probability_find_good_model) /
log(pNoOutliers);
if (ransac_nSimulations < ransac_min_nSimulations)
ransac_nSimulations = ransac_min_nSimulations;
// if (verbose)
// cout << "[Align] Iter #" << iter << " Est. #iters: "
//<< ransac_nSimulations << " pNoOutliers=" <<
// pNoOutliers << " #inliers: " << ninliers << endl;
}
} // end of RANSAC loop
// Move SOG modes into pdf_SOG:
pdf_SOG->clear();
for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
{
cout << "SOG mode: " << s->second.mean
<< " inliers: " << s->first.size() << endl;
pdf_SOG->push_back(s->second);
}
// Normalize:
if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights();
// Simplify the SOG by merging close modes:
// -------------------------------------------------
size_t nB = pdf_SOG->size();
outInfo.sog1 = pdf_SOG;
CTicTac merge_clock;
pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
const double merge_clock_tim = merge_clock.Tac();
outInfo.sog2 = pdf_SOG;
size_t nA = pdf_SOG->size();
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));
} // end of amModifiedRANSAC
// Save best corrs:
if (options.debug_save_map_pairs)
{
static unsigned int NN = 0;
static const COccupancyGridMap2D* lastM1 = nullptr;
if (lastM1 != m1)
{
lastM1 = m1;
NN = 0;
}
printf(
" Largest consensus: %u\n",
static_cast<unsigned>(largestConsensusCorrs.size()));
CEnhancedMetaFile::LINUX_IMG_WIDTH(
m1->getSizeX() + m2->getSizeX() + 50);
CEnhancedMetaFile::LINUX_IMG_HEIGHT(
max(m1->getSizeY(), m2->getSizeY()) + 50);
for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
{
COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
format("__debug_corrsGrid_%05u.emf", NN), m1, m2,
s->first);
COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
format("__debug_corrsGrid_%05u.png", NN), m1, m2,
s->first);
++NN;
}
}
// --------------------------------------------------------------------
// At this point:
// - "pdf_SOG": has the resulting PDF with the SOG (from whatever
// method)
// - "largestConsensusCorrs": The 'best' set of correspondences
//
// Now: If we had a multi-metric map, use the points map to improve
// the estimation with ICP.
// --------------------------------------------------------------------
if (multimap1 && multimap2 && !multimap1->m_pointsMaps.empty() &&
!multimap2->m_pointsMaps.empty() &&
multimap1->m_pointsMaps[0] && multimap2->m_pointsMaps[0])
{
CSimplePointsMap::Ptr pnts1 = multimap1->m_pointsMaps[0];
CSimplePointsMap::Ptr pnts2 = multimap2->m_pointsMaps[0];
CICP icp;
CICP::TReturnInfo icpInfo;
icp.options.maxIterations = 20;
icp.options.smallestThresholdDist = 0.05f;
icp.options.thresholdDist = 0.75f;
// cout << "Points: " << pnts1->size() << " " << pnts2->size()
// << endl;
// Invoke ICP once for each mode in the SOG:
size_t cnt = 0;
outInfo.icp_goodness_all_sog_modes.clear();
for (auto i = pdf_SOG->begin(); i != pdf_SOG->end(); ++cnt)
{
CPosePDF::Ptr icp_est = icp.Align(
pnts1.get(), pnts2.get(), (i)->mean, nullptr, &icpInfo);
//(i)->cov(0,0) += square( 0.05 );
//(i)->cov(1,1) += square( 0.05 );
//(i)->cov(2,2) += square( DEG2RAD(0.05) );
CPosePDFGaussian i_gauss(i->mean, i->cov);
CPosePDFGaussian icp_gauss(icp_est->getMeanVal(), i->cov);
const double icp_maha_dist =
i_gauss.mahalanobisDistanceTo(icp_gauss);
cout << "ICP " << cnt << " log-w: " << i->log_w
<< " Goodness: " << 100 * icpInfo.goodness
<< " D_M= " << icp_maha_dist << endl;
cout << " final pos: " << icp_est->getMeanVal() << endl;
cout << " org pos: " << i->mean << endl;
outInfo.icp_goodness_all_sog_modes.push_back(
icpInfo.goodness);
// Discard?
if (icpInfo.goodness > options.min_ICP_goodness &&
icp_maha_dist < options.max_ICP_mahadist)
{
icp_est->getMean((i)->mean);
++i;
}
else
{
// Delete:
i = pdf_SOG->erase(i);
}
} // end for i
// Merge:
outInfo.sog3 = pdf_SOG;
pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
MRPT_LOG_DEBUG_STREAM(
"[CGridMapAligner] " << pdf_SOG->size()
<< " SOG modes merged after ICP.");
} // end multimapX
} // end of, yes, we have enough correspondences
} // end of: yes, there are landmarks in the grid maps!
// Copy the output info if requested:
// -------------------------------------------------
MRPT_TODO(
"Refactor `info` so it is polymorphic and can use dynamic_cast<> here");
if (info)
{
auto* info_ = static_cast<TReturnInfo*>(info);
*info_ = outInfo;
}
if (runningTime)
{
*runningTime = tictac->Tac();
delete tictac;
}
return pdf_SOG;
MRPT_END
}
// ------------------------------------------------------
// TestRandomGenerators
// ------------------------------------------------------
void TestRandomGenerators()
{
vector_double x,y;
randomGenerator.randomize();
// Uniform numbers integers:
CDisplayWindowPlots win1("Unif(0,5) (integers)");
win1.setPos(10,10);
win1.resize(400,400);
{
//vector_double v1(100000);
vector_size_t v1(100000);
randomGenerator.drawUniformVector(v1 ,0, 5.999);
CHistogram hist(-2,15, 100);
hist.add(v1);
hist.getHistogramNormalized(x,y);
win1.plot(x,y,"b");
win1.axis_fit();
}
// Normalized Gauss:
CDisplayWindowPlots win2("N(mean=0,std=1)");
win2.setPos(420,10);
win2.resize(400,400);
{
vector_double v1(100000);
randomGenerator.drawGaussian1DVector(v1 ,0,1);
CHistogram hist(-5,5,100);
hist.add(v1);
hist.getHistogramNormalized(x,y);
win2.plot(x,y,"b");
vector_double y_real(y.size());
for (vector_double::Index k=0; k<y_real.size(); k++)
y_real[k] = mrpt::math::normalPDF(x[k],0,1);
win2.plot(x,y_real,"k-","real");
win2.axis_fit();
}
// Example Gauss:
CDisplayWindowPlots win3("N(mean=3,std=2)");
win3.setPos(10,430);
win3.resize(400,400);
{
vector_double v1(100000);
randomGenerator.drawGaussian1DVector(v1 ,3,2);
CHistogram hist(-5,15,100);
hist.add(v1);
hist.getHistogramNormalized(x,y);
win3.plot(x,y,"b");
vector_double y_real(y.size());
for (vector_double::Index k=0; k<y_real.size(); k++)
y_real[k] = mrpt::math::normalPDF(x[k],3,2);
win3.plot(x,y_real,"k-","real");
win3.axis_fit();
}
// Example multi-variate Gauss:
CDisplayWindowPlots win4("N(mean=[3 4],var=[4 -2;-2 4])");
win4.setPos(420,430);
win4.resize(400,400);
{
vector<vector_double> v1;
vector_double Mean(2);
Mean[0] = 3;
Mean[1] = 2;
//CMatrixDouble cov(2,2);
CMatrixDouble22 cov;
cov.fromMatlabStringFormat("[7.5 -7;-7 8]");
randomGenerator.drawGaussianMultivariateMany(v1,10000,cov,&Mean);
#if 0
vector_double m;
CMatrixDouble c;
mrpt::math::meanAndCov(v1,m,c);
cout << "Mean: " << m << endl;
cout << "Std: " << endl << c << endl;
#endif
// pass to (x,y) vectors:
vector_double x(v1.size()), y(v1.size());
for (size_t i=0; i<v1.size(); i++)
{
x[i] = v1[i][0];
y[i] = v1[i][1];
}
win4.plot(x,y,"b.3");
win4.plotEllipse(Mean[0],Mean[1],cov,3.0,"k-2","99% ellipse",true);
win4.axis_fit();
}
mrpt::system::pause();
}