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 );
}
}
// ------------------------------------------------------
// 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;
}
/*---------------------------------------------------------------
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() );
}
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 );
}
}
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));
}
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;
}
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;
}
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;
}
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();
}
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;
}
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);
}
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() );
}
}
/*----------------------------------------------------------------------------
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
}
/*---------------------------------------------------------------
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
}
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;
}
}
