/*---------------------------------------------------------------
getCurrentEntropyOfPaths
---------------------------------------------------------------*/
double CMultiMetricMapPDF::getCurrentEntropyOfPaths()
{
size_t i;
size_t N=m_particles[0].d->robotPath.size(); // The poses count along the paths
// Compute paths entropy:
// ---------------------------
double H_paths = 0;
if (N)
{
// For each pose along the path:
for (i=0;i<N;i++)
{
// Get pose est. as m_particles:
CPose3DPDFParticles posePDFParts;
getEstimatedPosePDFAtTime(i,posePDFParts);
// Approximate to gaussian and compute entropy of covariance:
H_paths+= posePDFParts.getCovarianceEntropy();
}
H_paths /= N;
}
return H_paths;
}
/*---------------------------------------------------------------
getRelativePose
---------------------------------------------------------------*/
void CLocalMetricHypothesis::getRelativePose(
const TPoseID &reference,
const TPoseID &pose,
CPose3DPDFParticles &outPDF ) const
{
MRPT_START
// Resize output:
outPDF.resetDeterministic( CPose3D(), m_particles.size() );
CParticleList::const_iterator it;
CPose3DPDFParticles::CParticleList::iterator itP;
for ( it = m_particles.begin(), itP = outPDF.m_particles.begin(); it!=m_particles.end(); it++, itP++ )
{
itP->log_w = it->log_w;
TMapPoseID2Pose3D::const_iterator srcPose = it->d->robotPoses.find( reference );
TMapPoseID2Pose3D::const_iterator trgPose = it->d->robotPoses.find( pose );
ASSERT_( srcPose != it->d->robotPoses.end() )
ASSERT_( trgPose != it->d->robotPoses.end() )
*itP->d = trgPose->second - srcPose->second;
}
MRPT_END
}
/*---------------------------------------------------------------
inverse
---------------------------------------------------------------*/
void CPose3DPDFParticles::inverse(CPose3DPDF& o) const
{
MRPT_START
ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles));
CPose3DPDFParticles* out = static_cast<CPose3DPDFParticles*>(&o);
// Prepare the output:
out->copyFrom(*this);
CPose3DPDFParticles::CParticleList::iterator it;
CPose3D zero(0, 0, 0);
for (it = out->m_particles.begin(); it != out->m_particles.end(); ++it)
*it->d = zero - *it->d;
MRPT_END
}
/*---------------------------------------------------------------
getEstimatedPosePDFAtTime
---------------------------------------------------------------*/
void CMultiMetricMapPDF::getEstimatedPosePDFAtTime(
size_t timeStep,
CPose3DPDFParticles &out_estimation ) const
{
//CPose3D p;
size_t i,n = m_particles.size();
// Delete current content of "out_estimation":
out_estimation.clearParticles();
// Create new m_particles:
out_estimation.m_particles.resize(n);
for (i=0;i<n;i++)
{
out_estimation.m_particles[i].d = new CPose3D( m_particles[i].d->robotPath[ timeStep ] );
out_estimation.m_particles[i].log_w = m_particles[i].log_w;
}
}
/*---------------------------------------------------------------
getPoseParticles
---------------------------------------------------------------*/
void CLocalMetricHypothesis::getPoseParticles( const TPoseID &poseID, CPose3DPDFParticles &outPDF ) const
{
MRPT_START
ASSERT_(!m_particles.empty());
CParticleList::const_iterator it;
outPDF.resetDeterministic( CPose3D(), m_particles.size() );
CPose3DPDFParticles::CParticleList::iterator itP;
for ( it = m_particles.begin(), itP = outPDF.m_particles.begin(); it!=m_particles.end(); it++, itP++ )
{
itP->log_w = it->log_w;
TMapPoseID2Pose3D::const_iterator itPose = it->d->robotPoses.find(poseID);
ASSERT_( itPose!=it->d->robotPoses.end() );
*itP->d = itPose->second;
}
MRPT_END
}
/*---------------------------------------------------------------
processActionObservation
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::processActionObservation(
CActionCollection &action,
CSensoryFrame &observations )
{
MRPT_START
// Enter critical section (updating map)
enterCriticalSection();
// Update the traveled distance estimations:
{
CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
if (act3D)
{
odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
odoIncrementSinceLastLocalization += act3D->poseChange;
}
else if (act2D)
{
odoIncrementSinceLastMapUpdate += act2D->poseChange->getMeanVal();
odoIncrementSinceLastLocalization.mean += act2D->poseChange->getMeanVal();
}
else
{
std::cerr << "[CMetricMapBuilderRBPF] WARNING: action contains no odometry." << std::endl;
}
}
// Execute particle filter:
// But only if the traveled distance since the last update is long enough,
// or this is the first observation, etc...
// ----------------------------------------------------------------------------
bool do_localization = (
!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastLocalization.mean.norm()>localizeLinDistance ||
std::abs(odoIncrementSinceLastLocalization.mean.yaw())>localizeAngDistance);
bool do_map_update = (
!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastMapUpdate.norm()>insertionLinDistance ||
std::abs(odoIncrementSinceLastMapUpdate.yaw())>insertionAngDistance);
// Used any "options.alwaysInsertByClass" ??
for (CListOfClasses::const_iterator itCl=options.alwaysInsertByClass.begin();!do_map_update && itCl!=options.alwaysInsertByClass.end();++itCl)
for ( CSensoryFrame::iterator it=observations.begin();it!=observations.end();++it)
if ((*it)->GetRuntimeClass()==*itCl)
{
do_map_update = true;
do_localization = true;
break;
}
if (do_map_update)
do_localization = true;
if (do_localization)
{
// Create an artificial action object, since
// we've been collecting them until a threshold:
// ------------------------------------------------
CActionCollection fakeActs;
{
CActionRobotMovement3DPtr act3D = action.getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
CActionRobotMovement3D newAct;
newAct.estimationMethod = act3D->estimationMethod;
newAct.poseChange = odoIncrementSinceLastLocalization;
newAct.timestamp = act3D->timestamp;
fakeActs.insert(newAct);
}
else
{
// It must be 2D odometry:
CActionRobotMovement2DPtr act2D = action.getActionByClass<CActionRobotMovement2D>();
ASSERT_(act2D)
CActionRobotMovement2D newAct;
newAct.computeFromOdometry( CPose2D(odoIncrementSinceLastLocalization.mean), act2D->motionModelConfiguration );
newAct.timestamp = act2D->timestamp;
fakeActs.insert(newAct);
}
}
// Reset distance counters:
odoIncrementSinceLastLocalization.mean.setFromValues(0,0,0,0,0,0);
odoIncrementSinceLastLocalization.cov.zeros();
CParticleFilter pf;
pf.m_options = m_PF_options;
pf.executeOn( mapPDF, &fakeActs, &observations );
if (options.verbose)
{
// Get current pose estimation:
CPose3DPDFParticles poseEstimation;
CPose3D meanPose;
mapPDF.getEstimatedPosePDF(poseEstimation);
poseEstimation.getMean(meanPose);
CPose3D estPos;
CMatrixDouble66 cov;
poseEstimation.getCovarianceAndMean(cov,estPos);
std::cout << " New pose=" << estPos << std::endl << "New ESS:"<< mapPDF.ESS() << std::endl;
std::cout << format(" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n", sqrt(cov(0,0)),sqrt(cov(1,1)),sqrt(cov(2,2)),RAD2DEG(sqrt(cov(3,3))));
}
}
if (do_map_update)
{
odoIncrementSinceLastMapUpdate.setFromValues(0,0,0,0,0,0);
// Update the particles' maps:
// -------------------------------------------------
if (options.verbose)
printf(" 3) New observation inserted into the map!\n");
// Add current observation to the map:
mapPDF.insertObservation(observations);
m_statsLastIteration.observationsInserted = true;
}
else
{
m_statsLastIteration.observationsInserted = false;
}
// Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux. variables
// (if any) since one PF cycle is over:
for (CMultiMetricMapPDF::CParticleList::iterator it = mapPDF.m_particles.begin(); it!=mapPDF.m_particles.end();++it)
it->d->mapTillNow.auxParticleFilterCleanUp();
leaveCriticalSection(); /* Leaving critical section (updating map) */
MRPT_END_WITH_CLEAN_UP( leaveCriticalSection(); /* Leaving critical section (updating map) */ );
/*---------------------------------------------------------------
processActionObservation
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::processActionObservation(
CActionCollection& action, CSensoryFrame& observations)
{
MRPT_START
std::lock_guard<std::mutex> csl(
critZoneChangingMap); // Enter critical section (updating map)
// Update the traveled distance estimations:
{
CActionRobotMovement3D::Ptr act3D =
action.getActionByClass<CActionRobotMovement3D>();
CActionRobotMovement2D::Ptr act2D =
action.getActionByClass<CActionRobotMovement2D>();
if (act3D)
{
MRPT_LOG_DEBUG_STREAM(
"processActionObservation(): Input action is "
"CActionRobotMovement3D="
<< act3D->poseChange.getMeanVal().asString());
odoIncrementSinceLastMapUpdate += act3D->poseChange.getMeanVal();
odoIncrementSinceLastLocalization += act3D->poseChange;
}
else if (act2D)
{
MRPT_LOG_DEBUG_STREAM(
"processActionObservation(): Input action is "
"CActionRobotMovement2D="
<< act2D->poseChange->getMeanVal().asString());
odoIncrementSinceLastMapUpdate +=
mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
odoIncrementSinceLastLocalization.mean +=
mrpt::poses::CPose3D(act2D->poseChange->getMeanVal());
}
else
{
MRPT_LOG_WARN("Action contains no odometry.\n");
}
}
// Execute particle filter:
// But only if the traveled distance since the last update is long enough,
// or this is the first observation, etc...
// ----------------------------------------------------------------------------
bool do_localization =
(!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastLocalization.mean.norm() > localizeLinDistance ||
std::abs(odoIncrementSinceLastLocalization.mean.yaw()) >
localizeAngDistance);
bool do_map_update =
(!mapPDF.SFs.size() || // This is the first observation!
options.debugForceInsertion ||
odoIncrementSinceLastMapUpdate.norm() > insertionLinDistance ||
std::abs(odoIncrementSinceLastMapUpdate.yaw()) > insertionAngDistance);
// Used any "options.alwaysInsertByClass" ??
for (auto itCl = options.alwaysInsertByClass.data.begin();
!do_map_update && itCl != options.alwaysInsertByClass.data.end();
++itCl)
for (auto& o : observations)
if (o->GetRuntimeClass() == *itCl)
{
do_map_update = true;
do_localization = true;
break;
}
if (do_map_update) do_localization = true;
MRPT_LOG_DEBUG(mrpt::format(
"do_map_update=%s do_localization=%s", do_map_update ? "YES" : "NO",
do_localization ? "YES" : "NO"));
if (do_localization)
{
// Create an artificial action object, since
// we've been collecting them until a threshold:
// ------------------------------------------------
CActionCollection fakeActs;
{
CActionRobotMovement3D::Ptr act3D =
action.getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
CActionRobotMovement3D newAct;
newAct.estimationMethod = act3D->estimationMethod;
newAct.poseChange = odoIncrementSinceLastLocalization;
newAct.timestamp = act3D->timestamp;
fakeActs.insert(newAct);
}
else
{
// It must be 2D odometry:
CActionRobotMovement2D::Ptr act2D =
action.getActionByClass<CActionRobotMovement2D>();
ASSERT_(act2D);
CActionRobotMovement2D newAct;
newAct.computeFromOdometry(
CPose2D(odoIncrementSinceLastLocalization.mean),
act2D->motionModelConfiguration);
newAct.timestamp = act2D->timestamp;
fakeActs.insert(newAct);
}
}
MRPT_LOG_DEBUG_STREAM(
"odoIncrementSinceLastLocalization before resetting = "
<< odoIncrementSinceLastLocalization.mean);
// Reset distance counters:
odoIncrementSinceLastLocalization.mean.setFromValues(0, 0, 0, 0, 0, 0);
odoIncrementSinceLastLocalization.cov.zeros();
CParticleFilter pf;
pf.m_options = m_PF_options;
pf.setVerbosityLevel(this->getMinLoggingLevel());
pf.executeOn(mapPDF, &fakeActs, &observations);
if (isLoggingLevelVisible(mrpt::system::LVL_INFO))
{
// Get current pose estimation:
CPose3DPDFParticles poseEstimation;
CPose3D meanPose;
mapPDF.getEstimatedPosePDF(poseEstimation);
poseEstimation.getMean(meanPose);
CPose3D estPos;
CMatrixDouble66 cov;
poseEstimation.getCovarianceAndMean(cov, estPos);
MRPT_LOG_INFO_STREAM(
"New pose=" << estPos << std::endl
<< "New ESS:" << mapPDF.ESS() << std::endl);
MRPT_LOG_INFO(format(
" STDs: x=%2.3f y=%2.3f z=%.03f yaw=%2.3fdeg\n",
sqrt(cov(0, 0)), sqrt(cov(1, 1)), sqrt(cov(2, 2)),
RAD2DEG(sqrt(cov(3, 3)))));
}
}
if (do_map_update)
{
odoIncrementSinceLastMapUpdate.setFromValues(0, 0, 0, 0, 0, 0);
// Update the particles' maps:
// -------------------------------------------------
MRPT_LOG_INFO("New observation inserted into the map.");
// Add current observation to the map:
const bool anymap_update = mapPDF.insertObservation(observations);
if (!anymap_update)
MRPT_LOG_WARN_STREAM(
"**No map was updated** after inserting a CSensoryFrame with "
<< observations.size());
m_statsLastIteration.observationsInserted = true;
}
else
{
m_statsLastIteration.observationsInserted = false;
}
// Added 29/JUN/2007 JLBC: Tell all maps that they can now free aux.
// variables
// (if any) since one PF cycle is over:
for (auto& m_particle : mapPDF.m_particles)
m_particle.d->mapTillNow.auxParticleFilterCleanUp();
MRPT_END;
}
/*---------------------------------------------------------------
perform_TLC
Topological Loop Closure: Performs all the required operations
to close a loop between two areas which have been determined
to be the same.
---------------------------------------------------------------*/
void CHMTSLAM::perform_TLC(
CLocalMetricHypothesis &LMH,
const CHMHMapNode::TNodeID Ai, // areaInLMH,
const CHMHMapNode::TNodeID Ae, // areaLoopClosure,
const mrpt::poses::CPose3DPDFGaussian &pose_i_wrt_e
)
{
MRPT_START
ASSERT_(Ai!=Ae)
synch::CCriticalSectionLocker locker ( &LMH.m_robotPosesGraph.lock );
printf_debug("[perform_TLC] TLC of areas: %u <-> %u \n",(unsigned)Ai, (unsigned)Ae );
// * Verify a1 \in LMH & a2 \notin LMH
// ----------------------------------------------------------------------
ASSERT_( LMH.m_neighbors.count(Ai) == 1);
ASSERT_( LMH.m_neighbors.count(Ae) == 0);
// * Get the old pose reference of the area which will desapear:
// ----------------------------------------------------------------------
TPoseID poseID_Ai_origin;
m_map.getNodeByID(Ai)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_Ai_origin, LMH.m_ID, true);
// TPoseID poseID_new_origin;
// m_map.getNodeByID(Ae)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);
// * Make a list Li of pose IDs in Ai, which need a change in coordinates:
// ----------------------------------------------------------------------------------------------
//TPoseIDSet Li;
//for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
// if(it->second==Ai)
// Li.insert(it->first);
// * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"
// ----------------------------------------------------------------------------------------------
// For all the particles
//for ( CLocalMetricHypothesis::CParticleList::const_iterator it = LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
//{
// // Touch only the affected poseIDs:
// for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
// {
// map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
// ASSERT_(itPos!=it->d->robotPoses.end());
// itPos->second = Apose_ei + (itPos->second - oldRef);
// }
//}
// Change coords in incr. partitioning as well:
//CSimpleMap *SFseq = LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
//for (std::map<uint32_t,TPoseID>::const_iterator it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
//{
// // Only if the pose has been affected:
// if (Li.count(it->second)==0) continue;
// CPose3DPDFPtr pdf;
// CSensoryFramePtr sf;
// SFseq->get( it->first, pdf, sf);
// // Copy from particles:
// CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
// LMH.getPoseParticles( it->second, *pdfParts);
//}
// * Mark all poses in LMH \in Ai as being of "Ae":
// ----------------------------------------------------------------------------------------------
for (map<TPoseID,CHMHMapNode::TNodeID>::iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
if(it->second==Ai)
it->second=Ae;
// * Replace "Ai" by "Ae" in the list of neighbors areas:
// ----------------------------------------------------------------------------------------------
LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
LMH.m_neighbors.insert(Ae);
// * Reflect the change of coordinates in all existing arcs from/to "Ai", which are not part of the LMH (list of neighbors)
// "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
// NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
// ----------------------------------------------------------------------------------------------
{
TArcList lstArcs;
m_map.getNodeByID(Ai)->getArcs(lstArcs,LMH.m_ID);
for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
{
// TODO ...
}
}
// * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
// ----------------------------------------------------------------------------------------------
{
TArcList lstArcs;
m_map.getNodeByID(Ai)->getArcs(lstArcs);
for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
arc->clear(); // The "delete" will automatically remove the entry in "m_map". Other smrtpnts will be cleared too.
m_map.getNodeByID(Ai).clear(); // The "delete" will automatically remove the entry in "m_map"
}
// * Merge Ae into the LMH: This includes the change of coordinates to those used now in "Ai" and the rest of neighbors!
// ----------------------------------------------------------------------------------------------
CPose3DPDFParticles pdfAiRef;
LMH.getPoseParticles(poseID_Ai_origin,pdfAiRef);
CPose3D AiRef;
pdfAiRef.getMean(AiRef);
const CPose3D &Apose_ie = pose_i_wrt_e.mean;
//const CPose3D Apose_ei = -Apose_ie;
const CPose3D AeRefInLMH = AiRef + Apose_ie;
CRobotPosesGraphPtr AePG = m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH, LMH.m_ID);
// For each pose in Ae:
for (CRobotPosesGraph::const_iterator it=AePG->begin();it!=AePG->end();++it)
{
const TPoseID poseId = it->first;
const TPoseInfo &pi = it->second;
// Insert the observations into the metric maps:
ASSERT_(LMH.m_particles.size() == pi.pdf.size() );
for (size_t i=0;i<pi.pdf.size();i++)
{
// Transport coordinates:
CPose3D &p = *pi.pdf.m_particles[i].d;
LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
//pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps, pi.pdf.m_particles[i].d );
}
// Add node membership:
LMH.m_nodeIDmemberships[poseId] = Ae;
// Now, insert the SF
LMH.m_SFs[poseId] = pi.sf;
// Also add to the list of poses pending to be evaluated by the AA:
LMH.m_posesPendingAddPartitioner.push_back(poseId);
}
// * The current pose of the robot should be in the new coordinate system:
// ----------------------------------------------------------------------------------------------
// This is already done since the current pose is just another poseID and it's threaded like the others.
// * Rebuild metric maps for consistency:
// ----------------------------------------------------------------------------------------------
LMH.rebuildMetricMaps();
// * Update all the data in the node "Ae":
// ----------------------------------------------------------------------------------------------
LMH.updateAreaFromLMH(Ae);
MRPT_END
}
/*---------------------------------------------------------------
updateAreaFromLMH
// The corresponding node in the HMT map is updated with the
// robot poses & SFs in the LMH.
---------------------------------------------------------------*/
void CLocalMetricHypothesis::updateAreaFromLMH(
const CHMHMapNode::TNodeID areaID,
bool eraseSFsFromLMH )
{
// Build the list with the poses belonging to that area from LMH:
// ----------------------------------------------------------------------
TNodeIDList lstPoseIDs;
for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator it=m_nodeIDmemberships.begin();it!=m_nodeIDmemberships.end();++it)
if ( it->second==areaID )
lstPoseIDs.insert(it->first);
ASSERT_( !lstPoseIDs.empty() );
CHMHMapNodePtr node;
{
synch::CCriticalSectionLocker ( &m_parent->m_map_cs );
node = m_parent->m_map.getNodeByID( areaID );
ASSERT_(node);
ASSERT_(node->m_hypotheses.has( m_ID ));
} // end of HMT map cs
// The pose to become the origin:
TPoseID poseID_origin;
node->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_origin, m_ID, true);
// 1) The set of robot poses and SFs
// In annotation: NODE_ANNOTATION_POSES_GRAPH
// ---------------------------------------------------------------------
CRobotPosesGraphPtr posesGraph;
{
CSerializablePtr annot = node->m_annotations.get(NODE_ANNOTATION_POSES_GRAPH,m_ID);
if (!annot)
{
// Add it now:
posesGraph = CRobotPosesGraph::Create();
node->m_annotations.setMemoryReference(NODE_ANNOTATION_POSES_GRAPH, posesGraph, m_ID);
}
else
{
posesGraph = CRobotPosesGraphPtr(annot);
posesGraph->clear();
}
}
// For each pose in the area:
CPose3DPDFParticles pdfOrigin;
bool pdfOrigin_ok=false;
for (TNodeIDList::const_iterator it=lstPoseIDs.begin();it!=lstPoseIDs.end();++it)
{
TPoseInfo &poseInfo = (*posesGraph)[*it];
getPoseParticles( *it, poseInfo.pdf ); // Save pose particles
// Save the PDF of the origin:
if (*it==poseID_origin)
{
pdfOrigin.copyFrom( poseInfo.pdf );
pdfOrigin_ok = true;
}
if ( *it != m_currentRobotPose ) // The current robot pose has no SF
{
std::map<TPoseID,CSensoryFrame>::iterator itSF = m_SFs.find(*it);
ASSERT_(itSF!=m_SFs.end());
if (eraseSFsFromLMH)
{
poseInfo.sf.moveFrom( itSF->second ); // This leaves m_SFs[*it] without observations, but it is being erased just now:
m_SFs.erase( itSF );
}
else
{
poseInfo.sf = itSF->second; // Copy observations
}
}
}
// Readjust to set the origin pose ID:
ASSERT_(pdfOrigin_ok);
CPose3DPDFParticles pdfOriginInv;
pdfOrigin.inverse(pdfOriginInv);
for (CRobotPosesGraph::iterator it=posesGraph->begin();it!=posesGraph->end();++it)
{
CPose3DPDFParticles::CParticleList::iterator orgIt,pdfIt;
ASSERT_( it->second.pdf.size() == pdfOriginInv.size() );
for ( pdfIt=it->second.pdf.m_particles.begin(), orgIt= pdfOriginInv.m_particles.begin();orgIt!=pdfOriginInv.m_particles.end();orgIt++,pdfIt++)
*pdfIt->d = *orgIt->d + *pdfIt->d;
}
// 2) One single metric map built from the most likelily robot poses
// In annotation: NODE_ANNOTATION_METRIC_MAPS
// ---------------------------------------------------------------------
CMultiMetricMapPtr metricMap = node->m_annotations.getAs<CMultiMetricMap>(NODE_ANNOTATION_METRIC_MAPS,m_ID, false);
metricMap->clear();
posesGraph->insertIntoMetricMap( *metricMap );
}
