/*---------------------------------------------------------------
					inverse
 ---------------------------------------------------------------*/
void	 CPose3DQuatPDFGaussian::inverse(CPose3DQuatPDF &o) const
{
	ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DQuatPDFGaussian));
	CPose3DQuatPDFGaussian	&out = static_cast<CPose3DQuatPDFGaussian&>(o);

	// COV:
	CMatrixFixedNumeric<double,3,7>  df_dpose(UNINITIALIZED_MATRIX);
	double lx,ly,lz;
	mean.inverseComposePoint(0,0,0,lx,ly,lz, NULL, &df_dpose);


	CMatrixFixedNumeric<double,7,7>  jacob;
	jacob.insertMatrix(0,0, df_dpose );
	jacob.set_unsafe(3,3,  1);
	jacob.set_unsafe(4,4, -1);
	jacob.set_unsafe(5,5, -1);
	jacob.set_unsafe(6,6, -1);

	// C(0:2,0:2): H C H^t
	jacob.multiply_HCHt( this->cov, out.cov );

	// Mean:
	out.mean.x(lx);
	out.mean.y(ly);
	out.mean.z(lz);
	this->mean.quat().conj( out.mean.quat() );
}
示例#2
0
/*---------------------------------------------------------------
					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
}