コード例 #1
0
TEST(Matrices,multiply_A_skew3)
{
	{
		const double dat_A[] = {
			1,2,3,
			4,5,6 };
		const CMatrixDouble  A(2,3,dat_A);
		const std::vector<double>  v = make_vector<3>(1.0,2.0,3.0);
		const CMatrixDouble  S = CMatrixDouble( mrpt::math::skew_symmetric3(v) );

		CMatrixDouble  R;
		R.multiply_A_skew3(A,v);
		EXPECT_EQ(R, (A*S).eval() );
	}
	{
		const double dat_A[] = {
			1,2,3,
			4,5,6 };
		const double dat_v[] = { 1,2,3 };
		const CMatrixFixedNumeric<double,2,3>  A(dat_A);
		const CArrayDouble<3> v(dat_v);
		const CMatrixFixedNumeric<double,3,3>  S = mrpt::math::skew_symmetric3(v);

		CMatrixFixedNumeric<double,2,3> R;
		R.multiply_A_skew3(A,v);
		EXPECT_TRUE(R== A*S );
	}
}
コード例 #2
0
ファイル: test.cpp プロジェクト: 3660628/mrpt
// ------------------------------------------------------
//				TestHCH
// ------------------------------------------------------
void TestHCH()
{
	CMatrixFloat		H,C,RES;

	cout << "reading H.txt...";
	H.loadFromTextFile(myDataDir+string("H.txt"));
	cout << "ok"<<endl;

	cout << "reading C.txt...";
	C.loadFromTextFile(myDataDir+string("C.txt"));
	cout << "ok"<<endl;

	// RES = H * C * ~H
	H.multiply_HCHt(C, RES);
	cout << "Saving RES.txt ...";
	RES.saveToTextFile("RES.txt");
	cout << "ok"<<endl;

	// The same for a column vector:
	H.loadFromTextFile(myDataDir+string("H_col.txt"));
	cout << "H*C*(~H) = " << H.multiply_HCHt_scalar(C) << endl;
	cout << "Should be= 31.434 "<< endl;

	// The same for a row vector:
	H.loadFromTextFile(myDataDir+string("H_row.txt"));
	cout << "Loaded H: "  << endl << H;
	cout << "H*C*(~H) = " << H.multiply_HCHt_scalar(C) << endl;
	cout << "Should be= 31.434"<< endl;

	CMatrixFixedNumeric<double,1,5>  Hfix;
	Hfix.loadFromTextFile(myDataDir+string("H_row.txt"));
	cout << "Again, loaded as a fixed matrix: "  << endl << Hfix;
}
コード例 #3
0
/*---------------------------------------------------------------
					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() );
}
コード例 #4
0
TEST(Matrices,multiply_skew3_A)
{
	{
		const double dat_A[] = {
			1,2,
			3,4,
			5,6 };
		const CMatrixDouble  A(3,2,dat_A);
		const std::vector<double>  v = make_vector<3>(1.0,2.0,3.0);
		const CMatrixDouble  S = CMatrixDouble( mrpt::math::skew_symmetric3(v) );

		CMatrixDouble  R;
		R.multiply_skew3_A(v,A);
		EXPECT_TRUE(R == S*A );
	}
	{
		const double dat_A[] = {
			1,2,
			3,4,
			5,6 };
		const double dat_v[] = { 1,2,3 };
		const CMatrixFixedNumeric<double,3,2>  A(dat_A);
		const CArrayDouble<3> v(dat_v);
		const CMatrixFixedNumeric<double,3,3>  S = mrpt::math::skew_symmetric3(v);

		CMatrixFixedNumeric<double,3,2> R;
		R.multiply_skew3_A(v,A);
		EXPECT_TRUE(R == S*A );
	}
}
コード例 #5
0
ファイル: matrix_ops5_unittest.cpp プロジェクト: jolting/mrpt
TEST(Matrices, loadFromArray)
{
	alignas(16)
		const double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};

	CMatrixFixedNumeric<double, 3, 4> mat;
	mat.loadFromArray(nums);

	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 4; c++) EXPECT_EQ(nums[4 * r + c], mat(r, c));
}
コード例 #6
0
ファイル: perf-matrix2.cpp プロジェクト: gamman/MRPT
double matrix_test_chol_fix(int a1, int a2)
{
	CMatrixFixedNumeric<T,DIM1,DIM1>	A = randomGenerator.drawDefinitePositiveMatrix(DIM1, 0.2);
	CMatrixFixedNumeric<T,DIM1,DIM1>	chol_U;

	const long N = 100;
	CTicTac	 tictac;
	for (long i=0;i<N;i++)
	{
		A.chol(chol_U);
	}
	return tictac.Tac()/N;
}
コード例 #7
0
ファイル: perf-matrix1.cpp プロジェクト: Insomnia-/mrpt
double matrix_test_unit_fix(int a1, int a2)
{
	CMatrixFixedNumeric<T,DIM,DIM>	C;

	const long N = 1000000;
	CTicTac	 tictac;
	for (long i=0;i<N;i++)
	{
		C.resize(DIM,DIM);
		C.setIdentity();
	}
	return tictac.Tac()/N;
}
コード例 #8
0
ファイル: perf-matrix1.cpp プロジェクト: Insomnia-/mrpt
double matrix_test_det_fix(int a1, int a2)
{
	CMatrixFixedNumeric<T,DIM1,DIM1>	A;
	randomGenerator.drawGaussian1DMatrix(A,T(0),T(1));

	const long N = 10000;
	CTicTac	 tictac;
	for (long i=0;i<N;i++)
	{
		A.det();
	}
	return tictac.Tac()/N;
}
コード例 #9
0
ファイル: perf-matrix2.cpp プロジェクト: gamman/MRPT
double matrix_test_loadFromArray(int N, int a2)
{
	EIGEN_ALIGN16 double nums[4*4] = {
	 0,1,2,3,
	 4,5,6,7,
	 8,9,10,11,
	 12,13,14,15 };

	CMatrixFixedNumeric<double,4,4> M;

	CTicTac	 tictac;
	M.loadFromArray(nums);
	return tictac.Tac();
}
コード例 #10
0
ファイル: perf-matrix1.cpp プロジェクト: Insomnia-/mrpt
double matrix_test_mult_fix(int a1, int a2)
{
	CMatrixFixedNumeric<T,DIM1,DIM2>	A;
	CMatrixFixedNumeric<T,DIM2,DIM3>	B;
	CMatrixFixedNumeric<T,DIM1,DIM3>	C;

	randomGenerator.drawGaussian1DMatrix(A,T(0),T(1));
	randomGenerator.drawGaussian1DMatrix(B,T(0),T(1));

	const long N = 10000;
	CTicTac	 tictac;
	for (long i=0;i<N;i++)
	{
		C.multiply(A,B);
	}
	return tictac.Tac()/N;
}
コード例 #11
0
ファイル: perf-random.cpp プロジェクト: Triocrossing/mrpt
double random_test_8(int a1, int a2)
{
	CRandomGenerator rg;

	CMatrixFixedNumeric<double, DIM, DIM> R;
	rg.drawGaussian1DMatrix(R, 0.0, 1.0);

	CMatrixFixedNumeric<double, DIM, DIM> COV;
	COV.multiply_AAt(R);

	const size_t NSAMPS = 1000;

	// test 8:
	// ----------------------------------------
	const long N = 1000;
	CTicTac tictac;
	std::vector<CVectorDouble> res;
	for (long i = 0; i < N; i++)
	{
		rg.drawGaussianMultivariateMany(res, NSAMPS, COV);
	}
	return tictac.Tac() / (N * NSAMPS);
}
コード例 #12
0
TEST(Matrices, setSize)
{
	{
		CMatrixFixedNumeric<double,6,6>  M;
		EXPECT_TRUE( (M.array() == 0).all() );
	}
	{
		CMatrixDouble  M(5,5);
		EXPECT_TRUE( (M.array() == 0).all() );
	}
	{
		CMatrixDouble  M(5,5);
		M.setSize(6,5);
		EXPECT_TRUE( (M.array() == 0).all() );
	}
	{
		CMatrixDouble  M(5,5);
		M.setSize(10,5);
		EXPECT_TRUE( (M.array() == 0).all() );
	}
	{
		CMatrixDouble  M(5,5);
		M.setSize(5,6);
		EXPECT_TRUE( (M.array() == 0).all() );
	}
	{
		CMatrixDouble  M(5,5);
		M.setSize(6,6);
		EXPECT_TRUE( (M.array() == 0).all() );
	}
	{
		CMatrixDouble  M(5,5);
		M.setSize(10,10);
		EXPECT_TRUE( (M.array() == 0).all() );
	}
}
コード例 #13
0
ファイル: CICP.cpp プロジェクト: GYengera/mrpt
/*----------------------------------------------------------------------------

						ICP_Method_LM

  ----------------------------------------------------------------------------*/
CPosePDFPtr CICP::ICP_Method_LM(
		const mrpt::maps::CMetricMap		*mm1,
		const mrpt::maps::CMetricMap		*m2,
		const CPosePDFGaussian	&initialEstimationPDF,
		TReturnInfo				&outInfo )
{
	MRPT_START

	// The result can be either a Gaussian or a SOG:
	size_t              nCorrespondences=0;
	bool                keepIteratingICP;
	CPose2D	            grossEst = initialEstimationPDF.mean;
	TMatchingPairList   correspondences,old_correspondences;
	CPose2D             lastMeanPose;
	std::vector<float>  other_xs_trans,other_ys_trans; // temporary container of "other" map (map2) transformed by "q"
	CMatrixFloat dJ_dq;  // The jacobian
	CPose2D q;	// The updated 2D transformation estimate
	CPose2D q_new;

	const bool onlyUniqueRobust = options.onlyUniqueRobust;
	const bool onlyKeepTheClosest = options.onlyClosestCorrespondences;

	// Assure the class of the maps:
	ASSERT_(mm1->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
	const CPointsMap	*m1 = static_cast<const CPointsMap*>(mm1);

	// Asserts:
	// -----------------
	ASSERT_( options.ALFA>0 && options.ALFA<1 );

	// The algorithm output auxiliar info:
	// -------------------------------------------------
	outInfo.nIterations		= 0;
	outInfo.goodness		= 1.0f;

	TMatchingParams matchParams;
	TMatchingExtraResults matchExtraResults;

	matchParams.maxDistForCorrespondence = options.thresholdDist;			// Distance threshold
	matchParams.maxAngularDistForCorrespondence = options.thresholdAng;	// Angular threshold
	matchParams.angularDistPivotPoint = TPoint3D(q.x(),q.y(),0); // Pivot point for angular measurements
	matchParams.onlyKeepTheClosest = onlyKeepTheClosest;
	matchParams.onlyUniqueRobust = onlyUniqueRobust;
	matchParams.decimation_other_map_points = options.corresponding_points_decimation;

	// The gaussian PDF to estimate:
	// ------------------------------------------------------
	// First gross approximation:
	q = grossEst;

	// For LM inverse
	CMatrixFixedNumeric<float,3,3>	C;
	CMatrixFixedNumeric<float,3,3>	C_inv;		// This will keep the cov. matrix at the end

	// Asure maps are not empty!
	// ------------------------------------------------------
	if ( !m2->isEmpty() )
	{
		matchParams.offset_other_map_points = 0;
		// ------------------------------------------------------
		//					The ICP loop
		// ------------------------------------------------------
		do
		{
			// ------------------------------------------------------
			//		Find the matching (for a points map)
			// ------------------------------------------------------
			m1->determineMatching2D(
				m2,						// The other map
				q,						// The other map pose
				correspondences,
				matchParams, matchExtraResults);

			nCorrespondences = correspondences.size();


			if ( !nCorrespondences )
			{
				// Nothing we can do !!
				keepIteratingICP = false;
			}
			else
			{
				// Compute the estimated pose through iterative least-squares: Levenberg-Marquardt
				// ----------------------------------------------------------------------
				dJ_dq.setSize(3,nCorrespondences);  // The jacobian of the error function wrt the transformation q

				double  lambda = options.LM_initial_lambda;		// The LM parameter

				double  ccos = cos(q.phi());
				double	csin = sin(q.phi());

				double  w1,w2,w3;
				double  q1,q2,q3;
				double  A,B;
				const double  Axy = options.Axy_aprox_derivatives;		// For approximating the derivatives

				// Compute at once the square errors for each point with the current "q" and the transformed points:
				std::vector<float> sq_errors;
				correspondences.squareErrorVector( q, sq_errors, other_xs_trans,other_ys_trans);
				double OSE_initial = math::sum( sq_errors );

				// Compute "dJ_dq"
				// ------------------------------------
				double  rho2 = square( options.kernel_rho );
				mrpt::utils::TMatchingPairList::iterator	it;
				std::vector<float>::const_iterator other_x_trans,other_y_trans;
				size_t  i;

				for (i=0,
					it=correspondences.begin(),
					other_x_trans = other_xs_trans.begin(),
					other_y_trans = other_ys_trans.begin();
					i<nCorrespondences;
					++i, ++it,++other_x_trans,++other_y_trans )
				{
					// Jacobian: dJ_dx
					// --------------------------------------
//#define ICP_DISTANCES_TO_LINE

#ifndef ICP_DISTANCES_TO_LINE
					w1= *other_x_trans-Axy;
					q1 = m1->squareDistanceToClosestCorrespondence( w1, *other_y_trans );
					q1= kernel( q1, rho2 );

					w2= *other_x_trans;
					q2 = m1->squareDistanceToClosestCorrespondence( w2, *other_y_trans );
					q2= kernel( q2, rho2 );

					w3= *other_x_trans+Axy;
					q3 = m1->squareDistanceToClosestCorrespondence( w3, *other_y_trans );
					q3= kernel( q3, rho2 );
#else
					// The distance to the line that interpolates the TWO closest points:
					float  x1,y1, x2,y2, d1,d2;
					m1->kdTreeTwoClosestPoint2D(
						*other_x_trans, *other_y_trans, 	// The query
						x1, y1,   // Closest point #1
						x2, y2,   // Closest point #2
						d1,d2);

					w1= *other_x_trans-Axy;
					q1 = math::closestSquareDistanceFromPointToLine( w1, *other_y_trans,  x1,y1, x2,y2 );
					q1= kernel( q1, rho2 );

					w2= *other_x_trans;
					q2 = math::closestSquareDistanceFromPointToLine( w2, *other_y_trans,  x1,y1, x2,y2 );
					q2= kernel( q2, rho2 );

					w3= *other_x_trans+Axy;
					q3 = math::closestSquareDistanceFromPointToLine( w3, *other_y_trans,  x1,y1, x2,y2 );
					q3= kernel( q3, rho2 );
#endif
					//interpolate
					A=(  (q3-q2)/((w3-w2)*(w3-w1))  ) - (  (q1-q2)/((w1-w2)*(w3-w1))  );
					B=(   (q1-q2)+(A*((w2*w2)-(w1*w1)))   )/(w1-w2);

					dJ_dq.get_unsafe(0,i) = (2*A* *other_x_trans)+B;

					// Jacobian: dJ_dy
					// --------------------------------------
					w1= *other_y_trans-Axy;
#ifdef ICP_DISTANCES_TO_LINE
					q1 = math::closestSquareDistanceFromPointToLine( *other_x_trans, w1,  x1,y1, x2,y2 );
					q1= kernel( q1, rho2 );
#else
					q1= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w1 ),  rho2 );
#endif

					w2= *other_y_trans;
					// q2 is alreay computed from above!
					//q2 = m1->squareDistanceToClosestCorrespondence( *other_x_trans, w2 );
					//q2= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w2 ),  rho2 );

					w3= *other_y_trans+Axy;
#ifdef ICP_DISTANCES_TO_LINE
					q3 = math::closestSquareDistanceFromPointToLine( *other_x_trans, w3,  x1,y1, x2,y2 );
					q3= kernel( q3, rho2 );
#else
					q3= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w3 ),  rho2 );
#endif

					//interpolate
					A=(  (q3-q2)/((w3-w2)*(w3-w1))  ) - (  (q1-q2)/((w1-w2)*(w3-w1))  );
					B=(   (q1-q2)+(A*((w2*w2)-(w1*w1)))   )/(w1-w2);

					dJ_dq.get_unsafe(1,i) = (2*A* *other_y_trans)+B;

					// Jacobian: dR_dphi
					// --------------------------------------
					dJ_dq.get_unsafe(2,i) = dJ_dq.get_unsafe(0,i) * ( -csin * it->other_x - ccos * it->other_y )  +
								 dJ_dq.get_unsafe(1,i) * (  ccos * it->other_x - csin * it->other_y );

				} // end for each corresp.

				// Now we have the Jacobian in dJ_dq.

				// Compute the Hessian matrix H = dJ_dq * dJ_dq^T
				CMatrixFloat  H_(3,3);
				H_.multiply_AAt(dJ_dq);

				CMatrixFixedNumeric<float,3,3>  H = CMatrixFixedNumeric<float,3,3>(H_);

				bool  keepIteratingLM = true;

				// ---------------------------------------------------
				// Iterate the inner LM loop until convergence:
				// ---------------------------------------------------
				q_new = q;

				std::vector<float>  new_sq_errors, new_other_xs_trans, new_other_ys_trans;
				size_t   		nLMiters = 0;
				const size_t 	maxLMiters = 100;

				while ( keepIteratingLM &&  ++nLMiters<maxLMiters)
				{
					// The LM heuristic is:
					//  x_{k+1} = x_k  - ( H + \lambda diag(H) )^-1 * grad(J)
					//  grad(J) = dJ_dq * e (vector of errors)
					C = H;
					for (i=0;i<3;i++)
						C(i,i) *= (1+lambda);	// Levenberg-Maquardt heuristic

					C_inv = C.inv();

					// LM_delta = C_inv * dJ_dq * sq_errors
					Eigen::VectorXf  dJsq, LM_delta;
					dJ_dq.multiply_Ab( Eigen::Map<Eigen::VectorXf>(&sq_errors[0],sq_errors.size()), dJsq );
					C_inv.multiply_Ab(dJsq,LM_delta);

					q_new.x( q.x() - LM_delta[0] );
					q_new.y( q.y() - LM_delta[1] );
					q_new.phi( q.phi() - LM_delta[2] );

					// Compute the new square errors:
					// ---------------------------------------
					correspondences.squareErrorVector(
						q_new,
						new_sq_errors,
						new_other_xs_trans,
						new_other_ys_trans);

					float OSE_new = math::sum( new_sq_errors );

					bool improved = OSE_new < OSE_initial;

#if 0  // Debuggin'
					cout << "_____________" << endl;
					cout << "q -> q_new   : " << q << " -> " << q_new << endl;
					printf("err: %f  -> %f    lambda: %e\n", OSE_initial ,OSE_new, lambda );
					cout << "\\/J = "; utils::operator <<(cout,dJsq); cout << endl;
					mrpt::system::pause();
#endif

					keepIteratingLM =
						fabs(LM_delta[0])>options.minAbsStep_trans ||
						fabs(LM_delta[1])>options.minAbsStep_trans ||
						fabs(LM_delta[2])>options.minAbsStep_rot;

					if(improved)
					{
						//If resids have gone down, keep change and make lambda smaller by factor of 10
						lambda/=10;
						q=q_new;
						OSE_initial = OSE_new;
					}
					else
					{
						// Discard movement and try with larger lambda:
						lambda*=10;
					}

				} // end iterative LM

#if 0  // Debuggin'
				cout << "ICP loop: " << lastMeanPose  << " -> " << q << " LM iters: " << nLMiters  << " threshold: " << matchParams.maxDistForCorrespondence << endl;
#endif
				// --------------------------------------------------------
				// now the conditions for the outer ICP loop
				// --------------------------------------------------------
				keepIteratingICP = true;
				if	(fabs(lastMeanPose.x()-q.x())<options.minAbsStep_trans &&
					fabs(lastMeanPose.y()-q.y())<options.minAbsStep_trans &&
					fabs( math::wrapToPi( lastMeanPose.phi()-q.phi()) )<options.minAbsStep_rot)
				{
					matchParams.maxDistForCorrespondence		*= options.ALFA;
					matchParams.maxAngularDistForCorrespondence		*= options.ALFA;
					if (matchParams.maxDistForCorrespondence < options.smallestThresholdDist )
						keepIteratingICP = false;

					if (++matchParams.offset_other_map_points>=options.corresponding_points_decimation)
						matchParams.offset_other_map_points=0;
				}
				lastMeanPose = q;
			}	// end of "else, there are correspondences"

			// Next iteration:
			outInfo.nIterations++;

			if (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist)
			{
				matchParams.maxDistForCorrespondence		*= options.ALFA;
			}

		} while	( (keepIteratingICP && outInfo.nIterations<options.maxIterations) ||
					(outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) );

		outInfo.goodness = matchExtraResults.correspondencesRatio;


		//if (!options.skip_quality_calculation)
		{
			outInfo.quality= matchExtraResults.correspondencesRatio;
		}

	} // end of "if m2 is not empty"

	return CPosePDFGaussianPtr( new CPosePDFGaussian(q, C_inv.cast<double>() ) );
	MRPT_END
}
コード例 #14
0
ファイル: CGridMapAligner.cpp プロジェクト: jiapei100/mrpt
/*---------------------------------------------------------------
					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
}
コード例 #15
0
TEST(Matrices,loadFromTextFile)
{
	{
		const std::string s1 =
			"1 2 3\n"
			"4 5 6";
		std::stringstream  s(s1);
		CMatrixDouble M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
		EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
		EXPECT_EQ(M.rows(),2);
		EXPECT_EQ(M.cols(),3);
	}
	{
		const std::string s1 =
			"1 \t 2\n"
			"  4 \t\t 1    ";
		std::stringstream  s(s1);
		CMatrixDouble M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
		EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
		EXPECT_EQ(M.rows(),2);
		EXPECT_EQ(M.cols(),2);
	}
	{
		const std::string s1 =
			"1 2";
		std::stringstream  s(s1);
		CMatrixDouble M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
		EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
		EXPECT_EQ(M.rows(),1);
		EXPECT_EQ(M.cols(),2);
	}
	{
		const std::string s1 =
			"1 2 3\n"
			"4 5 6\n";
		std::stringstream  s(s1);
		CMatrixFixedNumeric<double,2,3> M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
		EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
		EXPECT_EQ(M.rows(),2);
		EXPECT_EQ(M.cols(),3);
	}
	{
		const std::string s1 =
			"1 2 3\n"
			"4 5\n";
		std::stringstream  s(s1);
		CMatrixFixedNumeric<double,2,3> M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
		EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
	}
	{
		const std::string s1 =
			"1 2 3\n"
			"4 5\n";
		std::stringstream  s(s1);
		CMatrixDouble M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
		EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
	}
	{
		const std::string s1 =
			"  \n";
		std::stringstream  s(s1);
		CMatrixFixedNumeric<double,2,3> M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
		EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
	}
	{
		const std::string s1 =
			"1 2 3\n"
			"1 2 3\n"
			"1 2 3";
		std::stringstream  s(s1);
		CMatrixFixedNumeric<double,2,3> M;
		bool retval = false;
		try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
		EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
	}
}