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