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