tuple CICP_AlignPDF2(CICP &self, CSimplePointsMap &m1, CSimplePointsMap &m2, CPosePDFGaussian &initialEstimationPDF)
{
CPosePDFGaussian posePDF;
float runningTime;
CICP::TReturnInfo info;
CPosePDFPtr posePDFPtr = self.AlignPDF(&m1, &m2, initialEstimationPDF, &runningTime, &info);
posePDF.copyFrom(*posePDFPtr);
boost::python::list ret_val;
ret_val.append(posePDF);
ret_val.append(runningTime);
ret_val.append(info);
return tuple(ret_val);
}
/*---------------------------------------------------------------
Copy Constructor from 2D PDF
---------------------------------------------------------------*/
CPose3DPDFGaussian::CPose3DPDFGaussian( const CPosePDFGaussian &o )
: mean( o.mean.x(),o.mean.y(),0,o.mean.phi(),0,0 ),
cov()
{
for (size_t i=0;i<3;i++)
{
const size_t ii= (i==2) ? 3 : i;
for (size_t j=0;j<3;j++)
{
const size_t jj= (j==2) ? 3 : j;
cov(ii,jj) = o.cov(i,j);
}
}
}
/** The PF algorithm implementation for "optimal sampling" approximated with scan matching (Stachniss method)
*/
void CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal(
CLocalMetricHypothesis *LMH,
const mrpt::slam::CActionCollection * actions,
const mrpt::slam::CSensoryFrame * sf,
const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
{
MRPT_START
CTicTac tictac;
// Get the current robot pose estimation:
TPoseID currentPoseID = LMH->m_currentRobotPose;
// ----------------------------------------------------------------------
// We can execute optimal PF only when we have both, an action, and
// a valid observation from which to compute the likelihood:
// Accumulate odometry/actions until we have a valid observation, then
// process them simultaneously.
// ----------------------------------------------------------------------
bool SFhasValidObservations = false;
// A valid action?
if (actions!=NULL)
{
CActionRobotMovement2DPtr act = actions->getBestMovementEstimation(); // Find a robot movement estimation:
if (!act) THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D derived object!");
if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
{
act->poseChange->getMean( LMH->m_accumRobotMovement.rawOdometryIncrementReading );
LMH->m_accumRobotMovement.motionModelConfiguration = act->motionModelConfiguration;
}
else
LMH->m_accumRobotMovement.rawOdometryIncrementReading =
LMH->m_accumRobotMovement.rawOdometryIncrementReading +
act->poseChange->getMeanVal();
LMH->m_accumRobotMovementIsValid = true;
}
if (sf!=NULL)
{
ASSERT_(LMH->m_particles.size()>0);
SFhasValidObservations = (*LMH->m_particles.begin()).d->metricMaps.canComputeObservationsLikelihood( *sf );
}
// All the needed things?
if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
return; // Nothing we can do here...
ASSERT_(sf!=NULL);
ASSERT_(!PF_options.adaptiveSampleSize);
// OK, we have all we need, let's start!
// The odometry-based increment since last step is
// in: LMH->m_accumRobotMovement.rawOdometryIncrementReading
const CPose2D initialPoseEstimation = LMH->m_accumRobotMovement.rawOdometryIncrementReading;
LMH->m_accumRobotMovementIsValid = false; // To reset odometry at next iteration!
// ----------------------------------------------------------------------
// 1) FIXED SAMPLE SIZE VERSION
// ----------------------------------------------------------------------
// ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
CICP icp;
CICP::TReturnInfo icpInfo;
// ICP options
// ------------------------------
icp.options.maxIterations = 80;
icp.options.thresholdDist = 0.50f;
icp.options.thresholdAng = DEG2RAD( 20 );
icp.options.smallestThresholdDist = 0.05f;
icp.options.ALFA = 0.5f;
icp.options.onlyClosestCorrespondences = true;
icp.options.doRANSAC = false;
// Build the map of points to align:
CSimplePointsMap localMapPoints;
ASSERT_( LMH->m_particles[0].d->metricMaps.m_gridMaps.size() > 0);
//float minDistBetweenPointsInLocalMaps = 0.02f; //3.0f * m_particles[0].d->metricMaps.m_gridMaps[0]->getResolution();
// Build local map:
localMapPoints.clear();
localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02;
sf->insertObservationsInto( &localMapPoints );
// Process the particles
const size_t M = LMH->m_particles.size();
LMH->m_log_w_metric_history.resize(M);
for (size_t i=0;i<M;i++)
{
CLocalMetricHypothesis::CParticleData &part = LMH->m_particles[i];
CPose3D *part_pose = LMH->getCurrentPose(i);
if ( LMH->m_SFs.empty() )
{
// The first robot pose in the SLAM execution: All m_particles start
// at the same point (this is the lowest bound of subsequent uncertainty):
// New pose = old pose.
// part_pose: Unmodified
}
else
{
// ICP and add noise:
CPosePDFGaussian icpEstimation;
// Select map to use with ICP:
CMetricMap *mapalign;
if (!part.d->metricMaps.m_pointsMaps.empty())
mapalign = part.d->metricMaps.m_pointsMaps[0].pointer();
else if (!part.d->metricMaps.m_gridMaps.empty())
mapalign = part.d->metricMaps.m_gridMaps[0].pointer();
else
THROW_EXCEPTION("There is no point or grid map. At least one needed for ICP.");
// Use ICP to align to each particle's map:
CPosePDFPtr alignEst = icp.Align(
mapalign,
&localMapPoints,
initialPoseEstimation,
NULL,
&icpInfo);
icpEstimation.copyFrom( *alignEst );
#if 0
// HACK:
CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
double Ap_dist = Ap.norm();
finalEstimatedPoseGauss.cov.zeros();
finalEstimatedPoseGauss.cov(0,0) = square( fabs(Ap_dist)*0.01 );
finalEstimatedPoseGauss.cov(1,1) = square( fabs(Ap_dist)*0.01 );
finalEstimatedPoseGauss.cov(2,2) = square( fabs(Ap.yaw())*0.02 );
#endif
// Generate gaussian-distributed 2D-pose increments according to "finalEstimatedPoseGauss":
// -------------------------------------------------------------------------------------------
// Set the gaussian pose:
CPose3DPDFGaussian finalEstimatedPoseGauss( icpEstimation );
CPose3D noisy_increment;
finalEstimatedPoseGauss.drawSingleSample(noisy_increment);
CPose3D new_pose;
new_pose.composeFrom(*part_pose,noisy_increment);
CPose2D new_pose2d = CPose2D(new_pose);
// Add the pose to the path:
part.d->robotPoses[ LMH->m_currentRobotPose ] = new_pose;
// Update the weight:
// ---------------------------------------------------------------------------
const double log_lik =
PF_options.powFactor *
auxiliarComputeObservationLikelihood(
PF_options,
LMH,
i,
sf,
&new_pose2d);
part.log_w += log_lik;
// Add to historic record of log_w weights:
LMH->m_log_w_metric_history[i][currentPoseID] += log_lik;
} // end else we can do ICP
} // end for each particle i
// Accumulate the log likelihood of this LMH as a whole:
double out_max_log_w;
LMH->normalizeWeights( &out_max_log_w ); // Normalize weights:
LMH->m_log_w += out_max_log_w;
printf("[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n",out_max_log_w);
printf("[CLSLAM_RBPF_2DLASER] Done in %.03fms\n",1e3*tictac.Tac());
MRPT_END
}
/*---------------------------------------------------------------
robustRigidTransformation
The technique was described in the paper:
J.L. Blanco, J. Gonzalez-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 "results.ransac_iters" 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.
---------------------------------------------------------------*/
bool tfest::se2_l2_robust(
const mrpt::tfest::TMatchingPairList& in_correspondences,
const double normalizationStd, const TSE2RobustParams& params,
TSE2RobustResult& results)
{
//#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 =
(params.max_rmse_to_end <= 0 ? 2 * normalizationStd
: params.max_rmse_to_end);
MRPT_START
// Asserts:
if (nCorrs < params.ransac_minSetSize)
{
// Nothing to do!
results.transformation.clear();
results.largestSubSet = TMatchingPairList();
return false;
}
#ifdef DO_PROFILING
timlog.enter("ransac.find_max*");
#endif
// Find the max. index of "this" and "other:
unsigned int maxThis = 0, maxOther = 0;
for (const auto& in_correspondence : in_correspondences)
{
maxThis = max(maxThis, in_correspondence.this_idx);
maxOther = max(maxOther, in_correspondence.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 (const auto& in_correspondence : in_correspondences)
{
if (!hasCorrThis[in_correspondence.this_idx] &&
!hasCorrOther[in_correspondence.other_idx])
{
hasCorrThis[in_correspondence.this_idx] = true;
hasCorrOther[in_correspondence.other_idx] = true;
howManyDifCorrs++;
}
}
#ifdef DO_PROFILING
timlog.leave("ransac.count_unique_corrs");
#endif
// Clear the set of output particles:
results.transformation.clear();
// If there are less different correspondences than the minimum required,
// quit:
if (howManyDifCorrs < params.ransac_minSetSize)
{
// Nothing we can do here!!! :~$
results.transformation.clear();
results.largestSubSet = TMatchingPairList();
return false;
}
#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<std::vector<int>> listDuplicatedLandmarksThis(maxThis + 1);
ASSERT_(nCorrs >= 1);
for (k = 0; k < nCorrs - 1; k++)
{
std::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<std::vector<int>> listDuplicatedLandmarksOther(maxOther + 1);
for (k = 0; k < nCorrs - 1; k++)
{
std::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();
results.ransac_iters = params.ransac_nSimulations;
const bool use_dynamic_iter_number = results.ransac_iters == 0;
if (use_dynamic_iter_number)
{
ASSERT_(
params.probability_find_good_model > 0 &&
params.probability_find_good_model < 1);
// Set an initial # of iterations:
results.ransac_iters = 10; // It doesn't matter actually, since will be
// changed in the first loop
}
std::vector<bool> alreadySelectedThis, alreadySelectedOther;
if (!params.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 < results.ransac_iters;
iter_idx++) // results.ransac_iters can be dynamic
{
#ifdef DO_PROFILING
CTimeLoggerEntry tle(timlog, "ransac.iter");
#endif
#ifdef DO_PROFILING
timlog.enter("ransac.permute");
#endif
getRandomGenerator().permuteVector(corrsIdxs, corrsIdxsPermutation);
#ifdef DO_PROFILING
timlog.leave("ransac.permute");
#endif
TMatchingPairList subSet;
// Select a subset of correspondences at random:
if (params.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() < params.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;
// Additional user-provided filter:
if (params.user_individual_compat_callback)
{
mrpt::tfest::TPotentialMatch pm;
pm.idx_this = corr_j.this_idx;
pm.idx_other = corr_j.other_idx;
if (!params.user_individual_compat_callback(pm))
continue; // Skip this one!
}
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:
tfest::se2_l2(subSet, referenceEstimation);
// 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 < params.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() >= params.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:
tfest::se2_l2(subSet, referenceEstimation);
// 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() >= params.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 (!params.ransac_fuseByCorrsMatch)
{
// Find matching by approximate match in the X,Y,PHI means
// -------------------------------------------------------------------
for (size_t i = 0; i < results.transformation.size(); i++)
{
double diffXY =
results.transformation.get(i).mean.distanceTo(
referenceEstimation.mean);
double diffPhi = fabs(math::wrapToPi(
results.transformation.get(i).mean.phi() -
referenceEstimation.mean.phi()));
if (diffXY < params.ransac_fuseMaxDiffXY &&
diffPhi < params.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 (params.ransac_algorithmForLandmarks)
results.transformation.get(indexFound).log_w = log(
1 + exp(results.transformation.get(indexFound).log_w));
else
results.transformation.get(indexFound).log_w =
log(subSet.size() +
exp(results.transformation.get(indexFound).log_w));
}
else
{
// Add a new mode to the SOG:
alreadyAddedSubSets.push_back(subSet);
CPosePDFSOG::TGaussianMode newSOGMode;
if (params.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!
results.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
results.ransac_iters =
log(1 - params.probability_find_good_model) /
log(pNoOutliers);
results.ransac_iters = std::max(
results.ransac_iters, params.ransac_min_nSimulations);
if (params.verbose)
cout << "[tfest::RANSAC] Iter #" << iter_idx
<< ":est. # iters=" << results.ransac_iters
<< " pNoOutliers=" << pNoOutliers
<< " #inliers: " << ninliers << endl;
}
}
// Save the largest subset:
if (subSet.size() >= params.ransac_minSetSize &&
this_subset_RMSE < largestSubSet_RMSE)
{
if (params.verbose)
cout << "[tfest::RANSAC] Iter #" << iter_idx
<< " Better subset: " << subSet.size()
<< " inliers, RMSE=" << this_subset_RMSE << endl;
results.largestSubSet = subSet;
largestSubSet_RMSE = this_subset_RMSE;
}
// Is the found subset good enough?
if (subSet.size() >= params.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 (params.verbose)
cout << "[tfest::RANSAC] Finished after " << iter_idx
<< " iterations.\n";
// Set the weights of the particles to sum the unity:
results.transformation.normalizeWeights();
// 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_results.transformation.txt'...");
results.transformation.saveToTextFile(
"_debug_results.transformation.txt");
printf("Ok\n"););
// ------------------------------------------------------
// TestICP
// ------------------------------------------------------
void TestICP()
{
CSimplePointsMap m1,m2;
float runningTime;
CICP::TReturnInfo info;
CICP ICP;
// Load scans:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.validRange.resize( SCANS_SIZE );
scan1.scan.resize(SCANS_SIZE);
ASSERT_( sizeof(SCAN_RANGES_1) == sizeof(float)*SCANS_SIZE );
memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) );
memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) );
CObservation2DRangeScan scan2 = scan1;
memcpy( &scan2.scan[0], SCAN_RANGES_2, sizeof(SCAN_RANGES_2) );
memcpy( &scan2.validRange[0], SCAN_VALID_2, sizeof(SCAN_VALID_2) );
// Build the points maps from the scans:
m1.insertObservation( &scan1 );
m2.insertObservation( &scan2 );
#if MRPT_HAS_PCL
cout << "Saving map1.pcd and map2.pcd in PCL format...\n";
m1.savePCDFile("map1.pcd", false);
m2.savePCDFile("map2.pcd", false);
#endif
// -----------------------------------------------------
// ICP.options.ICP_algorithm = icpLevenbergMarquardt;
// ICP.options.ICP_algorithm = icpClassic;
ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method;
ICP.options.maxIterations = 100;
ICP.options.thresholdAng = DEG2RAD(10.0f);
ICP.options.thresholdDist = 0.75f;
ICP.options.ALFA = 0.5f;
ICP.options.smallestThresholdDist = 0.05f;
ICP.options.doRANSAC = false;
ICP.options.dumpToConsole();
// -----------------------------------------------------
CPose2D initialPose(0.8f,0.0f,(float)DEG2RAD(0.0f));
CPosePDFPtr pdf = ICP.Align(
&m1,
&m2,
initialPose,
&runningTime,
(void*)&info);
printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ",
runningTime*1000,
info.nIterations,
runningTime*1000.0f/info.nIterations,
info.goodness*100 );
cout << "Mean of estimation: " << pdf->getMeanVal() << endl<< endl;
CPosePDFGaussian gPdf;
gPdf.copyFrom(*pdf);
cout << "Covariance of estimation: " << endl << gPdf.cov << endl;
cout << " std(x): " << sqrt( gPdf.cov(0,0) ) << endl;
cout << " std(y): " << sqrt( gPdf.cov(1,1) ) << endl;
cout << " std(phi): " << RAD2DEG(sqrt( gPdf.cov(2,2) )) << " (deg)" << endl;
//cout << "Covariance of estimation (MATLAB format): " << endl << gPdf.cov.inMatlabFormat() << endl;
cout << "-> Saving reference map as scan1.txt" << endl;
m1.save2D_to_text_file("scan1.txt");
cout << "-> Saving map to align as scan2.txt" << endl;
m2.save2D_to_text_file("scan2.txt");
cout << "-> Saving transformed map to align as scan2_trans.txt" << endl;
CSimplePointsMap m2_trans = m2;
m2_trans.changeCoordinatesReference( gPdf.mean );
m2_trans.save2D_to_text_file("scan2_trans.txt");
cout << "-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m" << endl;
CMatrixFloat COV22 = CMatrixFloat( CMatrixDouble( gPdf.cov ));
COV22.setSize(2,2);
CVectorFloat MEAN2D(2);
MEAN2D[0] = gPdf.mean.x();
MEAN2D[1] = gPdf.mean.y();
{
ofstream f("view_ellip.m");
f << math::MATLAB_plotCovariance2D( COV22, MEAN2D, 3.0f);
}
// If we have 2D windows, use'em:
#if MRPT_HAS_WXWIDGETS
if (!skip_window)
{
gui::CDisplayWindowPlots win("ICP results");
// Reference map:
vector<float> map1_xs, map1_ys, map1_zs;
m1.getAllPoints(map1_xs,map1_ys,map1_zs);
win.plot( map1_xs, map1_ys, "b.3", "map1");
// Translated map:
vector<float> map2_xs, map2_ys, map2_zs;
m2_trans.getAllPoints(map2_xs,map2_ys,map2_zs);
win.plot( map2_xs, map2_ys, "r.3", "map2");
// Uncertainty
win.plotEllipse(MEAN2D[0],MEAN2D[1],COV22,3.0,"b2", "cov");
win.axis(-1,10,-6,6);
win.axis_equal();
cout << "Close the window to exit" << endl;
win.waitForKey();
}
#endif
}
