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