bool mrpt::utils::operator == (const TMatchingPairList& a,const TMatchingPairList& b)
{
if (a.size()!=b.size())
return false;
for (TMatchingPairList::const_iterator it1=a.begin(),it2=b.begin();it1!=a.end();it1++,it2++)
if (! ( (*it1)==(*it2)))
return false;
return true;
}
/*---------------------------------------------------------------
robustRigidTransformation
This works as follows:
- Repeat "ransac_nSimulations" times:
- Randomly pick TWO correspondences from the set "in_correspondences".
- Compute the associated rigid transformation.
- For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
- If if is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow the "consensus set"
- If not, do not add it.
---------------------------------------------------------------*/
void scanmatching::robustRigidTransformation(
TMatchingPairList &in_correspondences,
poses::CPosePDFSOG &out_transformation,
float normalizationStd,
unsigned int ransac_minSetSize,
unsigned int ransac_maxSetSize,
float ransac_mahalanobisDistanceThreshold,
unsigned int ransac_nSimulations,
TMatchingPairList *out_largestSubSet,
bool ransac_fuseByCorrsMatch,
float ransac_fuseMaxDiffXY,
float ransac_fuseMaxDiffPhi,
bool ransac_algorithmForLandmarks,
double probability_find_good_model,
unsigned int ransac_min_nSimulations
)
{
size_t i,N = in_correspondences.size();
unsigned int maxThis=0, maxOther=0;
CPosePDFGaussian temptativeEstimation, referenceEstimation;
TMatchingPairList::iterator matchIt;
std::vector<bool> alreadySelectedThis;
std::vector<bool> alreadySelectedOther;
//#define DEBUG_OUT
MRPT_START
// Asserts:
if( N < ransac_minSetSize )
{
// Nothing to do!
out_transformation.clear();
return;
}
// Find the max. index of "this" and "other:
for (matchIt=in_correspondences.begin();matchIt!=in_correspondences.end(); matchIt++)
{
maxThis = max(maxThis , matchIt->this_idx );
maxOther= max(maxOther, matchIt->other_idx );
}
// Fill out 2 arrays indicating whether each element has a correspondence:
std::vector<bool> hasCorrThis(maxThis+1,false);
std::vector<bool> hasCorrOther(maxOther+1,false);
unsigned int howManyDifCorrs = 0;
//for (i=0;i<N;i++)
for (matchIt=in_correspondences.begin();matchIt!=in_correspondences.end(); matchIt++)
{
if (!hasCorrThis[matchIt->this_idx] &&
!hasCorrOther[matchIt->other_idx] )
{
hasCorrThis[matchIt->this_idx] = true;
hasCorrOther[matchIt->other_idx] = true;
howManyDifCorrs++;
}
}
// Clear the set of output particles:
out_transformation.clear();
// The max. number of corrs!
//ransac_maxSetSize = min(ransac_maxSetSize, max(2,(howManyDifCorrs-1)));
ransac_maxSetSize = min(ransac_maxSetSize, max((unsigned int)2,howManyDifCorrs) );
//printf("howManyDifCorrs=%u ransac_maxSetSize=%u\n",howManyDifCorrs,ransac_maxSetSize);
//ASSERT_( ransac_maxSetSize>=ransac_minSetSize );
if ( ransac_maxSetSize < ransac_minSetSize )
{
// Nothing we can do here!!! :~$
if (out_largestSubSet!=NULL)
{
TMatchingPairList emptySet;
*out_largestSubSet = emptySet;
}
out_transformation.clear();
return;
}
//#define AVOID_MULTIPLE_CORRESPONDENCES
#ifdef AVOID_MULTIPLE_CORRESPONDENCES
unsigned k;
// Find duplicated landmarks (from SIFT features with different descriptors,etc...)
// this is to avoid establishing multiple correspondences for the same physical point!
// ------------------------------------------------------------------------------------------------
std::vector<vector_int> listDuplicatedLandmarksThis(maxThis+1);
ASSERT_(N>=1);
for (k=0;k<N-1;k++)
{
vector_int duplis;
for (unsigned j=k;j<N-1;j++)
{
if ( in_correspondences[k].this_x == in_correspondences[j].this_x &&
in_correspondences[k].this_y == in_correspondences[j].this_y &&
in_correspondences[k].this_z == in_correspondences[j].this_z )
duplis.push_back(in_correspondences[j].this_idx);
}
listDuplicatedLandmarksThis[in_correspondences[k].this_idx] = duplis;
}
std::vector<vector_int> listDuplicatedLandmarksOther(maxOther+1);
for (k=0;k<N-1;k++)
{
vector_int duplis;
for (unsigned j=k;j<N-1;j++)
{
if ( in_correspondences[k].other_x == in_correspondences[j].other_x &&
in_correspondences[k].other_y == in_correspondences[j].other_y &&
in_correspondences[k].other_z == in_correspondences[j].other_z )
duplis.push_back(in_correspondences[j].other_idx);
}
listDuplicatedLandmarksOther[in_correspondences[k].other_idx] = duplis;
}
#endif
std::deque<TMatchingPairList> alreadyAddedSubSets;
std::vector<size_t> corrsIdxs( N), corrsIdxsPermutation;
for (i=0;i<N;i++) corrsIdxs[i]= i;
// If we put this out of the loop, each correspondence will be used just ONCE!
/**/
alreadySelectedThis.clear();
alreadySelectedThis.resize(maxThis+1,false);
alreadySelectedOther.clear();
alreadySelectedOther.resize(maxOther+1, false);
/**/
//~ CPosePDFGaussian temptativeEstimation;
// -------------------------
// The RANSAC loop
// -------------------------
size_t largest_consensus_yet = 0; // Used for dynamic # of steps
const bool use_dynamic_iter_number = ransac_nSimulations==0;
if (use_dynamic_iter_number)
{
ASSERT_(probability_find_good_model>0 && probability_find_good_model<1);
// Set an initial # of iterations:
ransac_nSimulations = 10; // It doesn't matter actually, since will be changed in the first loop
}
i = 0;
while (i<ransac_nSimulations) // ransac_nSimulations can be dynamic
{
i++;
TMatchingPairList subSet,temptativeSubSet;
// Select a subset of correspondences at random:
if (ransac_algorithmForLandmarks)
{
alreadySelectedThis.clear();
alreadySelectedThis.resize(maxThis+1,false);
alreadySelectedOther.clear();
alreadySelectedOther.resize(maxOther+1, false);
}
else
{
// For points: Do not repeat the corrs, and take the numer of corrs as weights
}
// Try to build a subsetof "ransac_maxSetSize" (maximum) elements that achieve consensus:
// ------------------------------------------------------------------------------------------
// First: Build a permutation of the correspondences to pick from it sequentially:
randomGenerator.permuteVector(corrsIdxs,corrsIdxsPermutation );
for (unsigned int j=0;j<ransac_maxSetSize;j++)
{
ASSERT_(j<corrsIdxsPermutation.size())
size_t idx = corrsIdxsPermutation[j];
matchIt = in_correspondences.begin() + idx;
ASSERT_( matchIt->this_idx < alreadySelectedThis.size() );
ASSERT_( matchIt->other_idx < alreadySelectedOther.size() );
if ( !(alreadySelectedThis [ matchIt->this_idx ] &&
alreadySelectedOther[ matchIt->other_idx]) )
// if ( !alreadySelectedThis [ matchIt->this_idx ] &&
// !alreadySelectedOther[ matchIt->other_idx] )
{
// mark as "selected" for this pair not to be selected again:
// ***NOTE***: That the expresion of the "if" above requires the
// same PAIR not to be selected again, but one of the elements
// may be selected again with a diferent matching! This improves the
// robustness and posibilities of the algorithm! (JLBC - NOV/2006)
#ifndef AVOID_MULTIPLE_CORRESPONDENCES
alreadySelectedThis[ matchIt->this_idx ]= true;
alreadySelectedOther[ matchIt->other_idx ] = true;
#else
for (vector_int::iterator it1 = listDuplicatedLandmarksThis[matchIt->this_idx].begin();it1!=listDuplicatedLandmarksThis[matchIt->this_idx].end();it1++)
alreadySelectedThis[ *it1 ] = true;
for (vector_int::iterator it2 = listDuplicatedLandmarksOther[matchIt->other_idx].begin();it2!=listDuplicatedLandmarksOther[matchIt->other_idx].end();it2++)
alreadySelectedOther[ *it2 ] = true;
#endif
if (subSet.size()<2)
{
// ------------------------------------------------------------------------------------------------------
// If we are within the first two correspondences, just add them to the subset:
// ------------------------------------------------------------------------------------------------------
subSet.push_back( *matchIt );
if (subSet.size()==2)
{
temptativeSubSet = subSet;
// JLBC: Modification DEC/2007: If we leave only ONE correspondence in the ref. set
// the algorithm will be pretty much sensible to reject bad correspondences:
temptativeSubSet.erase( temptativeSubSet.begin() + (temptativeSubSet.size() -1) );
// Perform estimation:
scanmatching::leastSquareErrorRigidTransformation(
subSet,
referenceEstimation.mean,
&referenceEstimation.cov );
// Normalized covariance: scale!
referenceEstimation.cov *= square(normalizationStd);
// Additional filter:
// If the correspondences as such the transformation has a high ambiguity, we discard it!
if ( referenceEstimation.cov(2,2)>=square(DEG2RAD(5.0f)) )
{
// Remove this correspondence & try again with a different pair:
subSet.erase( subSet.begin() + (subSet.size() -1) );
}
else
{
}
}
}
else
{
// ------------------------------------------------------------------------------------------------------
// The normal case:
// - test for "consensus" with the current group:
// - If it is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow the "consensus set"
// - If not, do not add it.
// ------------------------------------------------------------------------------------------------------
// Compute the temptative new estimation (matchIt will be removed after the test!):
temptativeSubSet.push_back( *matchIt );
scanmatching::leastSquareErrorRigidTransformation(
temptativeSubSet,
temptativeEstimation.mean,
&temptativeEstimation.cov );
// Normalized covariance: scale!
temptativeEstimation.cov *= square(normalizationStd);
// Additional filter:
// If the correspondences as such the transformation has a high ambiguity, we discard it!
if ( temptativeEstimation.cov(2,2)<square(DEG2RAD(5.0f)) )
{
// ASSERT minimum covariance!!
/*temptativeEstimation.cov(0,0) = max( temptativeEstimation.cov(0,0), square( 0.03f ) );
temptativeEstimation.cov(1,1) = max( temptativeEstimation.cov(1,1), square( 0.03f ) );
referenceEstimation.cov(0,0) = max( referenceEstimation.cov(0,0), square( 0.03f ) );
referenceEstimation.cov(1,1) = max( referenceEstimation.cov(1,1), square( 0.03f ) ); */
temptativeEstimation.cov(2,2) = max( temptativeEstimation.cov(2,2), square( DEG2RAD(0.2) ) );
referenceEstimation.cov(2,2) = max( referenceEstimation.cov(2,2), square( DEG2RAD(0.2) ) );
// Test for compatibility:
bool passTest;
if (ransac_algorithmForLandmarks)
{
// Compatibility test: Mahalanobis distance between Gaussians:
double mahaDist = temptativeEstimation.mahalanobisDistanceTo( referenceEstimation );
passTest = mahaDist < ransac_mahalanobisDistanceThreshold;
}
else
{
// Compatibility test: Euclidean distances
double diffXY = referenceEstimation.mean.distanceTo( temptativeEstimation.mean );
double diffPhi = fabs( math::wrapToPi( referenceEstimation.mean.phi() - temptativeEstimation.mean.phi() ) );
passTest = diffXY < 0.02f && diffPhi < DEG2RAD(2.0f);
//FILE *f=os::fopen("hist.txt","at");
//fprintf(f,"%f %f\n",diffXY, RAD2DEG(diffPhi) );
//fclose(f);
}
if ( passTest )
{
// OK, consensus passed!!
subSet.push_back( *matchIt );
referenceEstimation = temptativeEstimation;
}
else
{
// Test failed!
//printf("Discarded!:\n");
//std::cout << "temptativeEstimation:" << temptativeEstimation << " referenceEstimation:" << referenceEstimation << " mahaDist:" << mahaDist << "\n";
}
}
else
{
// Test failed!
//printf("Discarded! stdPhi=%f\n",RAD2DEG(sqrt(temptativeEstimation.cov(2,2))));
}
// Remove the temporaryy added last correspondence:
temptativeSubSet.pop_back();
} // end else "normal case"
} // end "if" the randomly selected item is new
} // end for j
// Save the estimation result as a "particle", only if the subSet contains
// "ransac_minSetSize" elements at least:
if (subSet.size()>=ransac_minSetSize)
{
// If this subset was previously added to the SOG, just increment its weight
// and do not add a new mode:
int indexFound = -1;
// JLBC Added DEC-2007: An alternative (optional) method to fuse Gaussian modes:
if (!ransac_fuseByCorrsMatch)
{
// Find matching by approximate match in the X,Y,PHI means
// -------------------------------------------------------------------
// Recompute referenceEstimation from all the corrs:
scanmatching::leastSquareErrorRigidTransformation(
subSet,
referenceEstimation.mean,
&referenceEstimation.cov );
// Normalized covariance: scale!
referenceEstimation.cov *= square(normalizationStd);
for (size_t i=0;i<out_transformation.size();i++)
{
double diffXY = out_transformation.get(i).mean.distanceTo( referenceEstimation.mean );
double diffPhi = fabs( math::wrapToPi( out_transformation.get(i).mean.phi() - referenceEstimation.mean.phi() ) );
if ( diffXY < ransac_fuseMaxDiffXY && diffPhi < ransac_fuseMaxDiffPhi )
{
//printf("Match by distance found: distXY:%f distPhi=%f deg\n",diffXY,RAD2DEG(diffPhi));
indexFound = i;
break;
}
}
}
else
{
// Find matching mode by exact match in the list of correspondences:
// -------------------------------------------------------------------
// Sort "subSet" in order to compare them easily!
//std::sort( subSet.begin(), subSet.end() );
// Try to find matching corrs:
for (size_t i=0;i<alreadyAddedSubSets.size();i++)
{
if ( subSet == alreadyAddedSubSets[i] )
{
indexFound = i;
break;
}
}
}
if (indexFound!=-1)
{
// This is an alrady added mode:
if (ransac_algorithmForLandmarks)
out_transformation.get(indexFound).log_w = log(1+ exp(out_transformation.get(indexFound).log_w));
else out_transformation.get(indexFound).log_w = log(subSet.size()+ exp(out_transformation.get(indexFound).log_w));
}
else
{
// Add a new mode to the SOG:
alreadyAddedSubSets.push_back( subSet );
CPosePDFSOG::TGaussianMode newSOGMode;
if (ransac_algorithmForLandmarks)
newSOGMode.log_w = 0; //log(1);
else newSOGMode.log_w = log(static_cast<double>(subSet.size()));
scanmatching::leastSquareErrorRigidTransformation(
subSet,
newSOGMode.mean,
&newSOGMode.cov );
// Normalized covariance: scale!
newSOGMode.cov *= square(normalizationStd);
// Add a new mode to the SOG!
out_transformation.push_back(newSOGMode);
}
} // end if subSet.size()>=ransac_minSetSize
// Dynamic # of steps:
if (use_dynamic_iter_number)
{
const size_t ninliers = subSet.size();
if (largest_consensus_yet<ninliers )
{
largest_consensus_yet = ninliers;
// 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>(howManyDifCorrs); // corrsIdxs.size());
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 << "[scanmatching::RANSAC] Iter #" << i << " Estimated number of iters: " << ransac_nSimulations << " pNoOutliers = " << pNoOutliers << " #inliers: " << ninliers << endl;
}
}
// Save the largest subset:
if (out_largestSubSet!=NULL)
{
if (subSet.size()>out_largestSubSet->size())
{
*out_largestSubSet = subSet;
}
}
#ifdef DEBUG_OUT
printf("[RANSAC] Sim #%i/%i \t--> |subSet|=%u \n",
(int)i,
(int)ransac_nSimulations,
(unsigned)subSet.size()
);
#endif
} // end for i
// Set the weights of the particles to sum the unity:
out_transformation.normalizeWeights();
// Now the estimation is in the particles set!
// Done!
MRPT_END_WITH_CLEAN_UP( \
printf("maxThis=%u, maxOther=%u\n",static_cast<unsigned int>(maxThis), static_cast<unsigned int>(maxOther)); \
printf("N=%u\n",static_cast<unsigned int>(N)); \
printf("Saving '_debug_in_correspondences.txt'..."); \
in_correspondences.dumpToFile("_debug_in_correspondences.txt"); \
printf("Ok\n"); \
printf("Saving '_debug_out_transformation.txt'..."); \
out_transformation.saveToTextFile("_debug_out_transformation.txt"); \
printf("Ok\n"); );
/*---------------------------------------------------------------
robustRigidTransformation
The technique was described in the paper:
J.L. Blanco, J. González-Jimenez and J.A. Fernandez-Madrigal.
"A robust, multi-hypothesis approach to matching occupancy grid maps".
Robotica, available on CJO2013. doi:10.1017/S0263574712000732.
http://journals.cambridge.org/action/displayAbstract?aid=8815308
This works as follows:
- Repeat "ransac_nSimulations" times:
- Randomly pick TWO correspondences from the set "in_correspondences".
- Compute the associated rigid transformation.
- For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
- If if is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow the "consensus set"
- If not, do not add it.
---------------------------------------------------------------*/
void scanmatching::robustRigidTransformation(
TMatchingPairList &in_correspondences,
poses::CPosePDFSOG &out_transformation,
float normalizationStd,
unsigned int ransac_minSetSize,
unsigned int ransac_maxSetSize,
float ransac_mahalanobisDistanceThreshold,
unsigned int ransac_nSimulations,
TMatchingPairList *out_largestSubSet,
bool ransac_fuseByCorrsMatch,
float ransac_fuseMaxDiffXY,
float ransac_fuseMaxDiffPhi,
bool ransac_algorithmForLandmarks,
double probability_find_good_model,
unsigned int ransac_min_nSimulations,
const bool verbose,
double max_rmse_to_end
)
{
//#define DO_PROFILING
#ifdef DO_PROFILING
CTimeLogger timlog;
#endif
const size_t nCorrs = in_correspondences.size();
// Default: 2 * normalizationStd ("noise level")
const double MAX_RMSE_TO_END = (max_rmse_to_end<=0 ? 2*normalizationStd : max_rmse_to_end);
MRPT_START
// Asserts:
if( nCorrs < ransac_minSetSize )
{
// Nothing to do!
out_transformation.clear();
if (out_largestSubSet!=NULL)
{
TMatchingPairList emptySet;
*out_largestSubSet = emptySet;
}
return;
}
#ifdef DO_PROFILING
timlog.enter("ransac.find_max*");
#endif
// Find the max. index of "this" and "other:
unsigned int maxThis=0, maxOther=0;
for (TMatchingPairList::iterator matchIt=in_correspondences.begin();matchIt!=in_correspondences.end(); ++matchIt)
{
maxThis = max(maxThis , matchIt->this_idx );
maxOther= max(maxOther, matchIt->other_idx );
}
#ifdef DO_PROFILING
timlog.leave("ransac.find_max*");
#endif
#ifdef DO_PROFILING
timlog.enter("ransac.count_unique_corrs");
#endif
// Fill out 2 arrays indicating whether each element has a correspondence:
std::vector<bool> hasCorrThis(maxThis+1,false);
std::vector<bool> hasCorrOther(maxOther+1,false);
unsigned int howManyDifCorrs = 0;
for (TMatchingPairList::iterator matchIt=in_correspondences.begin();matchIt!=in_correspondences.end(); ++matchIt)
{
if (!hasCorrThis[matchIt->this_idx] &&
!hasCorrOther[matchIt->other_idx] )
{
hasCorrThis[matchIt->this_idx] = true;
hasCorrOther[matchIt->other_idx] = true;
howManyDifCorrs++;
}
}
#ifdef DO_PROFILING
timlog.leave("ransac.count_unique_corrs");
#endif
// Clear the set of output particles:
out_transformation.clear();
// If there are less different correspondences than the minimum required, quit:
if ( howManyDifCorrs < ransac_minSetSize )
{
// Nothing we can do here!!! :~$
if (out_largestSubSet!=NULL)
{
TMatchingPairList emptySet;
*out_largestSubSet = emptySet;
}
out_transformation.clear();
return;
}
#ifdef AVOID_MULTIPLE_CORRESPONDENCES
unsigned k;
// Find duplicated landmarks (from SIFT features with different descriptors,etc...)
// this is to avoid establishing multiple correspondences for the same physical point!
// ------------------------------------------------------------------------------------------------
std::vector<vector_int> listDuplicatedLandmarksThis(maxThis+1);
ASSERT_(nCorrs>=1);
for (k=0;k<nCorrs-1;k++)
{
vector_int duplis;
for (unsigned j=k;j<nCorrs-1;j++)
{
if ( in_correspondences[k].this_x == in_correspondences[j].this_x &&
in_correspondences[k].this_y == in_correspondences[j].this_y &&
in_correspondences[k].this_z == in_correspondences[j].this_z )
duplis.push_back(in_correspondences[j].this_idx);
}
listDuplicatedLandmarksThis[in_correspondences[k].this_idx] = duplis;
}
std::vector<vector_int> listDuplicatedLandmarksOther(maxOther+1);
for (k=0;k<nCorrs-1;k++)
{
vector_int duplis;
for (unsigned j=k;j<nCorrs-1;j++)
{
if ( in_correspondences[k].other_x == in_correspondences[j].other_x &&
in_correspondences[k].other_y == in_correspondences[j].other_y &&
in_correspondences[k].other_z == in_correspondences[j].other_z )
duplis.push_back(in_correspondences[j].other_idx);
}
listDuplicatedLandmarksOther[in_correspondences[k].other_idx] = duplis;
}
#endif
std::deque<TMatchingPairList> alreadyAddedSubSets;
CPosePDFGaussian referenceEstimation;
CPoint2DPDFGaussian pt_this;
const double ransac_consistency_test_chi2_quantile = 0.99;
const double chi2_thres_dim1 = mrpt::math::chi2inv(ransac_consistency_test_chi2_quantile, 1);
// -------------------------
// The RANSAC loop
// -------------------------
size_t largest_consensus_yet = 0; // Used for dynamic # of steps
double largestSubSet_RMSE = std::numeric_limits<double>::max();
const bool use_dynamic_iter_number = ransac_nSimulations==0;
if (use_dynamic_iter_number)
{
ASSERT_(probability_find_good_model>0 && probability_find_good_model<1);
// Set an initial # of iterations:
ransac_nSimulations = 10; // It doesn't matter actually, since will be changed in the first loop
}
std::vector<bool> alreadySelectedThis, alreadySelectedOther;
if (!ransac_algorithmForLandmarks)
{
alreadySelectedThis.assign(maxThis+1,false);
alreadySelectedOther.assign(maxOther+1, false);
}
// else -> It will be done anyway inside the for() below
// First: Build a permutation of the correspondences to pick from it sequentially:
std::vector<size_t> corrsIdxs(nCorrs), corrsIdxsPermutation;
for (size_t i=0;i<nCorrs;i++) corrsIdxs[i]= i;
size_t iter_idx;
for (iter_idx = 0;iter_idx<ransac_nSimulations; iter_idx++) // ransac_nSimulations can be dynamic
{
#ifdef DO_PROFILING
CTimeLoggerEntry tle(timlog,"ransac.iter");
#endif
#ifdef DO_PROFILING
timlog.enter("ransac.permute");
#endif
randomGenerator.permuteVector(corrsIdxs,corrsIdxsPermutation );
#ifdef DO_PROFILING
timlog.leave("ransac.permute");
#endif
TMatchingPairList subSet;
// Select a subset of correspondences at random:
if (ransac_algorithmForLandmarks)
{
#ifdef DO_PROFILING
timlog.enter("ransac.reset_selection_marks");
#endif
alreadySelectedThis.assign(maxThis+1,false);
alreadySelectedOther.assign(maxOther+1, false);
#ifdef DO_PROFILING
timlog.leave("ransac.reset_selection_marks");
#endif
}
else
{
// For points: Do not repeat the corrs, and take the number of corrs as weights
}
// Try to build a subsetof "ransac_maxSetSize" (maximum) elements that achieve consensus:
// ------------------------------------------------------------------------------------------
#ifdef DO_PROFILING
timlog.enter("ransac.inner_loops");
#endif
for (unsigned int j=0;j<nCorrs && subSet.size()<ransac_maxSetSize;j++)
{
const size_t idx = corrsIdxsPermutation[j];
const TMatchingPair & corr_j = in_correspondences[idx];
// Don't pick the same features twice!
if (alreadySelectedThis [corr_j.this_idx] || alreadySelectedOther[corr_j.other_idx])
continue;
if (subSet.size()<2)
{
// ------------------------------------------------------------------------------------------------------
// If we are within the first two correspondences, just add them to the subset:
// ------------------------------------------------------------------------------------------------------
subSet.push_back( corr_j );
markAsPicked(corr_j, alreadySelectedThis,alreadySelectedOther);
if (subSet.size()==2)
{
// Consistency Test: From
// 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(
subSet[0].this_x, subSet[0].this_y,
subSet[1].this_x, subSet[1].this_y );
const double corrs_dist2 = mrpt::math::distanceBetweenPoints(
subSet[0].other_x, subSet[0].other_y,
subSet[1].other_x, subSet[1].other_y );
// 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(normalizationStd) * (square(corrs_dist1)+square(corrs_dist2)) );
bool is_acceptable = (corrs_dist_chi2 < chi2_thres_dim1 );
if (is_acceptable)
{
// Perform estimation:
scanmatching::leastSquareErrorRigidTransformation(
subSet,
referenceEstimation.mean,
&referenceEstimation.cov );
// Normalized covariance: scale!
referenceEstimation.cov *= square(normalizationStd);
// Additional filter:
// If the correspondences as such the transformation has a high ambiguity, we discard it!
is_acceptable = ( referenceEstimation.cov(2,2)<square(DEG2RAD(5.0f)) );
}
if (!is_acceptable)
{
// Remove this correspondence & try again with a different pair:
subSet.erase( subSet.begin() + (subSet.size() -1) );
}
else
{
// Only mark as picked if we're really keeping it:
markAsPicked(corr_j, alreadySelectedThis,alreadySelectedOther);
}
}
}
else
{
#ifdef DO_PROFILING
timlog.enter("ransac.test_consistency");
#endif
// ------------------------------------------------------------------------------------------------------
// The normal case:
// - test for "consensus" with the current group:
// - If it is compatible (ransac_maxErrorXY, ransac_maxErrorPHI), grow the "consensus set"
// - If not, do not add it.
// ------------------------------------------------------------------------------------------------------
// Test for the mahalanobis distance between:
// "referenceEstimation (+) point_other" AND "point_this"
referenceEstimation.composePoint( mrpt::math::TPoint2D(corr_j.other_x,corr_j.other_y), pt_this);
const double maha_dist = pt_this.mahalanobisDistanceToPoint(corr_j.this_x,corr_j.this_y);
const bool passTest = maha_dist < ransac_mahalanobisDistanceThreshold;
if ( passTest )
{
// OK, consensus passed:
subSet.push_back( corr_j );
markAsPicked(corr_j, alreadySelectedThis,alreadySelectedOther);
}
// else -> Test failed
#ifdef DO_PROFILING
timlog.leave("ransac.test_consistency");
#endif
} // end else "normal case"
} // end for j
#ifdef DO_PROFILING
timlog.leave("ransac.inner_loops");
#endif
const bool has_to_eval_RMSE = (subSet.size()>=ransac_minSetSize);
// Compute the RMSE of this matching and the corresponding transformation (only if we'll use this value below)
double this_subset_RMSE = 0;
if (has_to_eval_RMSE)
{
#ifdef DO_PROFILING
CTimeLoggerEntry tle(timlog,"ransac.comp_rmse");
#endif
// Recompute referenceEstimation from all the corrs:
scanmatching::leastSquareErrorRigidTransformation(
subSet,
referenceEstimation.mean,
&referenceEstimation.cov );
// Normalized covariance: scale!
referenceEstimation.cov *= square(normalizationStd);
for (size_t k=0;k<subSet.size();k++)
{
double gx,gy;
referenceEstimation.mean.composePoint(
subSet[k].other_x, subSet[k].other_y,
gx,gy);
this_subset_RMSE += mrpt::math::distanceSqrBetweenPoints<double>( subSet[k].this_x,subSet[k].this_y, gx,gy );
}
this_subset_RMSE /= std::max( static_cast<size_t>(1), subSet.size() );
}
else
{
this_subset_RMSE = std::numeric_limits<double>::max();
}
// Save the estimation result as a "particle", only if the subSet contains
// "ransac_minSetSize" elements at least:
if (subSet.size()>=ransac_minSetSize)
{
// If this subset was previously added to the SOG, just increment its weight
// and do not add a new mode:
int indexFound = -1;
// JLBC Added DEC-2007: An alternative (optional) method to fuse Gaussian modes:
if (!ransac_fuseByCorrsMatch)
{
// Find matching by approximate match in the X,Y,PHI means
// -------------------------------------------------------------------
for (size_t i=0;i<out_transformation.size();i++)
{
double diffXY = out_transformation.get(i).mean.distanceTo( referenceEstimation.mean );
double diffPhi = fabs( math::wrapToPi( out_transformation.get(i).mean.phi() - referenceEstimation.mean.phi() ) );
if ( diffXY < ransac_fuseMaxDiffXY && diffPhi < ransac_fuseMaxDiffPhi )
{
//printf("Match by distance found: distXY:%f distPhi=%f deg\n",diffXY,RAD2DEG(diffPhi));
indexFound = i;
break;
}
}
}
else
{
// Find matching mode by exact match in the list of correspondences:
// -------------------------------------------------------------------
// Sort "subSet" in order to compare them easily!
//std::sort( subSet.begin(), subSet.end() );
// Try to find matching corrs:
for (size_t i=0;i<alreadyAddedSubSets.size();i++)
{
if ( subSet == alreadyAddedSubSets[i] )
{
indexFound = i;
break;
}
}
}
if (indexFound!=-1)
{
// This is an already added mode:
if (ransac_algorithmForLandmarks)
out_transformation.get(indexFound).log_w = log(1+ exp(out_transformation.get(indexFound).log_w));
else out_transformation.get(indexFound).log_w = log(subSet.size()+ exp(out_transformation.get(indexFound).log_w));
}
else
{
// Add a new mode to the SOG:
alreadyAddedSubSets.push_back( subSet );
CPosePDFSOG::TGaussianMode newSOGMode;
if (ransac_algorithmForLandmarks)
newSOGMode.log_w = 0; //log(1);
else newSOGMode.log_w = log(static_cast<double>(subSet.size()));
newSOGMode.mean = referenceEstimation.mean;
newSOGMode.cov = referenceEstimation.cov;
// Add a new mode to the SOG!
out_transformation.push_back(newSOGMode);
}
} // end if subSet.size()>=ransac_minSetSize
const size_t ninliers = subSet.size();
if (largest_consensus_yet<ninliers )
{
largest_consensus_yet = ninliers;
// Dynamic # of steps:
if (use_dynamic_iter_number)
{
// Update estimate of nCorrs, the number of trials to ensure we pick,
// with probability p, a data set with no outliers.
const double fracinliers = ninliers/static_cast<double>(howManyDifCorrs); // corrsIdxs.size());
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);
ransac_nSimulations = std::max(ransac_nSimulations, ransac_min_nSimulations);
if (verbose)
cout << "[scanmatching::RANSAC] Iter #" << iter_idx << ":est. # iters=" << ransac_nSimulations << " pNoOutliers=" << pNoOutliers << " #inliers: " << ninliers << endl;
}
}
// Save the largest subset:
if (out_largestSubSet!=NULL)
{
if (subSet.size()>=ransac_minSetSize &&
this_subset_RMSE<largestSubSet_RMSE )
{
if (verbose)
cout << "[scanmatching::RANSAC] Iter #" << iter_idx << " Better subset: " << subSet.size() << " inliers, RMSE=" << this_subset_RMSE << endl;
*out_largestSubSet = subSet;
largestSubSet_RMSE = this_subset_RMSE;
}
}
// Is the found subset good enough?
if (subSet.size()>=ransac_minSetSize &&
this_subset_RMSE<MAX_RMSE_TO_END)
{
break; // end RANSAC iterations.
}
#ifdef DO_PROFILING
timlog.leave("ransac.iter");
#endif
} // end for each iteration
if (verbose)
cout << "[scanmatching::RANSAC] Finished after " << iter_idx << " iterations.\n";
// Set the weights of the particles to sum the unity:
out_transformation.normalizeWeights();
// Now the estimation is in the particles set!
// Done!
MRPT_END_WITH_CLEAN_UP( \
printf("nCorrs=%u\n",static_cast<unsigned int>(nCorrs)); \
printf("Saving '_debug_in_correspondences.txt'..."); \
in_correspondences.dumpToFile("_debug_in_correspondences.txt"); \
printf("Ok\n"); \
printf("Saving '_debug_out_transformation.txt'..."); \
out_transformation.saveToTextFile("_debug_out_transformation.txt"); \
printf("Ok\n"); );
/*---------------------------------------------------------------
leastSquareErrorRigidTransformation
Compute the best transformation (x,y,phi) given a set of
correspondences between 2D points in two different maps.
This method is intensively used within ICP.
---------------------------------------------------------------*/
bool tfest::se2_l2(
const TMatchingPairList& in_correspondences, TPose2D& out_transformation,
CMatrixDouble33* out_estimateCovariance)
{
MRPT_START
const size_t N = in_correspondences.size();
if (N < 2) return false;
const float N_inv = 1.0f / N; // For efficiency, keep this value.
// ----------------------------------------------------------------------
// Compute the estimated pose. Notation from the paper:
// "Mobile robot motion estimation by 2d scan matching with genetic and
// iterative
// closest point algorithms", J.L. Martinez Rodriguez, A.J. Gonzalez, J. Morales
// Rodriguez, A. Mandow Andaluz, A. J. Garcia Cerezo,
// Journal of Field Robotics, 2006.
// ----------------------------------------------------------------------
// ----------------------------------------------------------------------
// For the formulas of the covariance, see:
// http://www.mrpt.org/Paper:Occupancy_Grid_Matching
// and Jose Luis Blanco's PhD thesis.
// ----------------------------------------------------------------------
#if MRPT_HAS_SSE2
// SSE vectorized version:
//{
// TMatchingPair dumm;
// MRPT_COMPILE_TIME_ASSERT(sizeof(dumm.this_x)==sizeof(float))
// MRPT_COMPILE_TIME_ASSERT(sizeof(dumm.other_x)==sizeof(float))
//}
__m128 sum_a_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f)
__m128 sum_b_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f)
// [ f0 f1 f2 f3 ]
// xa*xb ya*yb xa*yb xb*ya
__m128 sum_ab_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f)
for (TMatchingPairList::const_iterator corrIt = in_correspondences.begin();
corrIt != in_correspondences.end(); corrIt++)
{
// Get the pair of points in the correspondence:
// a_xyyx = [ xa ay | xa ya ]
// b_xyyx = [ xb yb | yb xb ]
// (product)
// [ xa*xb ya*yb xa*yb xb*ya
// LO0 LO1 HI2 HI3
// Note: _MM_SHUFFLE(hi3,hi2,lo1,lo0)
const __m128 a_xyz = _mm_loadu_ps(&corrIt->this_x); // *Unaligned* load
const __m128 b_xyz =
_mm_loadu_ps(&corrIt->other_x); // *Unaligned* load
const __m128 a_xyxy =
_mm_shuffle_ps(a_xyz, a_xyz, _MM_SHUFFLE(1, 0, 1, 0));
const __m128 b_xyyx =
_mm_shuffle_ps(b_xyz, b_xyz, _MM_SHUFFLE(0, 1, 1, 0));
// Compute the terms:
sum_a_xyz = _mm_add_ps(sum_a_xyz, a_xyz);
sum_b_xyz = _mm_add_ps(sum_b_xyz, b_xyz);
// [ f0 f1 f2 f3 ]
// xa*xb ya*yb xa*yb xb*ya
sum_ab_xyz = _mm_add_ps(sum_ab_xyz, _mm_mul_ps(a_xyxy, b_xyyx));
}
alignas(MRPT_MAX_ALIGN_BYTES) float sums_a[4], sums_b[4];
_mm_store_ps(sums_a, sum_a_xyz);
_mm_store_ps(sums_b, sum_b_xyz);
const float& SumXa = sums_a[0];
const float& SumYa = sums_a[1];
const float& SumXb = sums_b[0];
const float& SumYb = sums_b[1];
// Compute all four means:
const __m128 Ninv_4val =
_mm_set1_ps(N_inv); // load 4 copies of the same value
sum_a_xyz = _mm_mul_ps(sum_a_xyz, Ninv_4val);
sum_b_xyz = _mm_mul_ps(sum_b_xyz, Ninv_4val);
// means_a[0]: mean_x_a
// means_a[1]: mean_y_a
// means_b[0]: mean_x_b
// means_b[1]: mean_y_b
alignas(MRPT_MAX_ALIGN_BYTES) float means_a[4], means_b[4];
_mm_store_ps(means_a, sum_a_xyz);
_mm_store_ps(means_b, sum_b_xyz);
const float& mean_x_a = means_a[0];
const float& mean_y_a = means_a[1];
const float& mean_x_b = means_b[0];
const float& mean_y_b = means_b[1];
// Sxx Syy Sxy Syx
// xa*xb ya*yb xa*yb xb*ya
alignas(MRPT_MAX_ALIGN_BYTES) float cross_sums[4];
_mm_store_ps(cross_sums, sum_ab_xyz);
const float& Sxx = cross_sums[0];
const float& Syy = cross_sums[1];
const float& Sxy = cross_sums[2];
const float& Syx = cross_sums[3];
// Auxiliary variables Ax,Ay:
const float Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb;
const float Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa;
#else
// Non vectorized version:
float SumXa = 0, SumXb = 0, SumYa = 0, SumYb = 0;
float Sxx = 0, Sxy = 0, Syx = 0, Syy = 0;
for (TMatchingPairList::const_iterator corrIt = in_correspondences.begin();
corrIt != in_correspondences.end(); corrIt++)
{
// Get the pair of points in the correspondence:
const float xa = corrIt->this_x;
const float ya = corrIt->this_y;
const float xb = corrIt->other_x;
const float yb = corrIt->other_y;
// Compute the terms:
SumXa += xa;
SumYa += ya;
SumXb += xb;
SumYb += yb;
Sxx += xa * xb;
Sxy += xa * yb;
Syx += ya * xb;
Syy += ya * yb;
} // End of "for all correspondences"...
const float mean_x_a = SumXa * N_inv;
const float mean_y_a = SumYa * N_inv;
const float mean_x_b = SumXb * N_inv;
const float mean_y_b = SumYb * N_inv;
// Auxiliary variables Ax,Ay:
const float Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb;
const float Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa;
#endif
out_transformation.phi =
(Ax != 0 || Ay != 0)
? atan2(static_cast<double>(Ay), static_cast<double>(Ax))
: 0.0;
const double ccos = cos(out_transformation.phi);
const double csin = sin(out_transformation.phi);
out_transformation.x = mean_x_a - mean_x_b * ccos + mean_y_b * csin;
out_transformation.y = mean_y_a - mean_x_b * csin - mean_y_b * ccos;
if (out_estimateCovariance)
{
CMatrixDouble33* C = out_estimateCovariance; // less typing!
// Compute the normalized covariance matrix:
// -------------------------------------------
double var_x_a = 0, var_y_a = 0, var_x_b = 0, var_y_b = 0;
const double N_1_inv = 1.0 / (N - 1);
// 0) Precompute the unbiased variances estimations:
// ----------------------------------------------------
for (TMatchingPairList::const_iterator corrIt =
in_correspondences.begin();
corrIt != in_correspondences.end(); corrIt++)
{
var_x_a += square(corrIt->this_x - mean_x_a);
var_y_a += square(corrIt->this_y - mean_y_a);
var_x_b += square(corrIt->other_x - mean_x_b);
var_y_b += square(corrIt->other_y - mean_y_b);
}
var_x_a *= N_1_inv; // /= (N-1)
var_y_a *= N_1_inv;
var_x_b *= N_1_inv;
var_y_b *= N_1_inv;
// 1) Compute BETA = s_Delta^2 / s_p^2
// --------------------------------
const double BETA = (var_x_a + var_y_a + var_x_b + var_y_b) *
pow(static_cast<double>(N), 2.0) *
static_cast<double>(N - 1);
// 2) And the final covariance matrix:
// (remember: this matrix has yet to be
// multiplied by var_p to be the actual covariance!)
// -------------------------------------------------------
const double D = square(Ax) + square(Ay);
C->get_unsafe(0, 0) =
2.0 * N_inv + BETA * square((mean_x_b * Ay + mean_y_b * Ax) / D);
C->get_unsafe(1, 1) =
2.0 * N_inv + BETA * square((mean_x_b * Ax - mean_y_b * Ay) / D);
C->get_unsafe(2, 2) = BETA / D;
C->get_unsafe(0, 1) = C->get_unsafe(1, 0) =
-BETA * (mean_x_b * Ay + mean_y_b * Ax) *
(mean_x_b * Ax - mean_y_b * Ay) / square(D);
C->get_unsafe(0, 2) = C->get_unsafe(2, 0) =
BETA * (mean_x_b * Ay + mean_y_b * Ax) / pow(D, 1.5);
C->get_unsafe(1, 2) = C->get_unsafe(2, 1) =
BETA * (mean_y_b * Ay - mean_x_b * Ax) / pow(D, 1.5);
}
return true;
MRPT_END
}