// CParticleFilter CParticleFilter::TParticleFilterStats CParticleFilter_executeOn( CParticleFilter& self, CParticleFilterCapable& obj, const mrpt::obs::CActionCollection::Ptr action, const mrpt::obs::CSensoryFrame::Ptr observation) { CParticleFilter::TParticleFilterStats stats; self.executeOn(obj, action.get(), observation.get(), &stats); return stats; }
/*--------------------------------------------------------------- CLSLAM_RBPF_2DLASER Implements a 2D local SLAM method based on a RBPF over an occupancy grid map. A part of HMT-SLAM. \param LMH The local metric hypothesis which must be updated by this SLAM algorithm. \param act The action to process (or NULL). \param sf The observations to process (or NULL). WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs! --------------------------------------------------------------- */ void CLSLAM_RBPF_2DLASER::processOneLMH( CLocalMetricHypothesis *LMH, const CActionCollectionPtr &actions, const CSensoryFramePtr &sf ) { MRPT_START // Get the current robot pose estimation: TPoseID currentPoseID = LMH->m_currentRobotPose; // If this is the first iteration, just create a new robot pose at the origin: if (currentPoseID == POSEID_INVALID ) { currentPoseID = CHMTSLAM::generatePoseID(); LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH // Create a new robot pose: CPose3D initPose(0,0,0); ASSERT_( LMH->m_particles.size()>0 ); for (CLocalMetricHypothesis::CParticleList::iterator it=LMH->m_particles.begin();it!=LMH->m_particles.end();++it) it->d->robotPoses[ currentPoseID ] = initPose; ASSERT_( m_parent->m_map.nodeCount()==1 ); m_parent->m_map_cs.enter(); CHMHMapNodePtr firstArea = m_parent->m_map.getFirstNode(); ASSERT_(firstArea); LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID(); // Set annotation for the reference pose: firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, currentPoseID , LMH->m_ID); m_parent->m_map_cs.leave(); } bool insertNewRobotPose = false; if (sf) { if ( LMH->m_nodeIDmemberships.size()<2 ) // If there is just 1 node (the current robot pose), then there is no observation in the map yet! { // Update map if this is the first observation! insertNewRobotPose = true; } else { // Check minimum distance from current robot pose to those in the neighborhood: map< TPoseID, CPose3D > lstRobotPoses; LMH->getMeans( lstRobotPoses ); CPose3D *currentRobotPose = & lstRobotPoses[currentPoseID]; float minDistLin = 1e6f; float minDistAng = 1e6f; //printf("Poses in graph:\n"); for (map< TPoseID, CPose3D >::iterator it = lstRobotPoses.begin();it!=lstRobotPoses.end();++it) { if (it->first != currentPoseID ) { float linDist = it->second.distanceTo( *currentRobotPose ); float angDist = fabs(math::wrapToPi( it->second.yaw() - currentRobotPose->yaw() )); minDistLin = min( minDistLin, linDist ); if ( linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS ) minDistAng = min(minDistAng, angDist); } } // time to insert a new node?? insertNewRobotPose = (minDistLin>m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) || ( minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS ); } } // end if there are SF // Save data in members so PF callback "prediction_and_update_pfXXXX" have access to them: m_insertNewRobotPose = insertNewRobotPose; // ------------------------------------------------ // Execute RBPF method: // 1) PROCESS ACTION // 2) PROCESS OBSERVATIONS // ------------------------------------------------ CParticleFilter pf; pf.m_options = m_parent->m_options.pf_options; pf.executeOn( *LMH, actions.pointer(), sf.pointer() ); // 3) The appearance observation: update the log likelihood // ... // ----------------------------------------------------------- // 4) UPDATE THE MAP // ----------------------------------------------------------- if (insertNewRobotPose) { m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Adding new pose...\n"); // Leave the up-to-now "current pose" in the map, insert the SF in it, and... // ---------------------------------------------------------------------------- TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID(); // ...and create a new "current pose" making a copy of the previous one: // and insert the observations into the metric maps: // ---------------------------------------------------------------------------- for (CLocalMetricHypothesis::CParticleList::iterator partIt = LMH->m_particles.begin(); partIt!=LMH->m_particles.end(); partIt++) { const CPose3D *curRobotPose = &partIt->d->robotPoses[currentPoseID]; partIt->d->robotPoses[newCurrentPoseID]= *curRobotPose; sf->insertObservationsInto( &partIt->d->metricMaps, curRobotPose ); } // Add node membership: for now, a copy of the current pose: LMH->m_nodeIDmemberships[newCurrentPoseID] = LMH->m_nodeIDmemberships[currentPoseID]; // Now, insert the SF in the just added robot pose: // ----------------------------------------------------- LMH->m_SFs[ currentPoseID ] = *sf; // Sets the new poseID as current robot pose: // ---------------------------------------------------- TPoseID newlyAddedPose = currentPoseID; currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID; // Mark the "newlyAddedPose" as pending to reconsider in the graph-cut method // (Done in the main loop @ LSLAM thread) // -------------------------------------------------------------------------------- LMH->m_posesPendingAddPartitioner.push_back( newlyAddedPose ); m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Added pose %i.\n", (int)newlyAddedPose); // Notice LC detectors: // ------------------------------ { synch::CCriticalSectionLocker lock( &m_parent->m_topLCdets_cs ); for (std::deque<CTopLCDetectorBase*>::iterator it=m_parent->m_topLCdets.begin();it!=m_parent->m_topLCdets.end();++it) (*it)->OnNewPose( newlyAddedPose, sf.pointer() ); } } // end of insertNewRobotPose MRPT_END }
/*--------------------------------------------------------------- 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) */ );
// ------------------------------------------------------ // TestBayesianTracking // ------------------------------------------------------ void TestBayesianTracking() { randomGenerator.randomize(); CDisplayWindowPlots winEKF("Tracking - Extended Kalman Filter",450,400); CDisplayWindowPlots winPF("Tracking - Particle Filter",450,400); winEKF.setPos(10,10); winPF.setPos(480,10); winEKF.axis(-2,20,-10,10); winEKF.axis_equal(); winPF.axis(-2,20,-10,10); winPF.axis_equal(); // Create EKF // ---------------------- CRangeBearing EKF; EKF.KF_options.method = kfEKFNaive; EKF.KF_options.verbose = true; EKF.KF_options.enable_profiler = true; // Create PF // ---------------------- CParticleFilter::TParticleFilterOptions PF_options; PF_options.adaptiveSampleSize = false; PF_options.PF_algorithm = CParticleFilter::pfStandardProposal; PF_options.resamplingMethod = CParticleFilter::prSystematic; CRangeBearingParticleFilter particles; particles.initializeParticles(NUM_PARTICLES); CParticleFilter PF; PF.m_options = PF_options; #ifdef SAVE_GT_LOGS CFileOutputStream fo_log_ekf("log_GT_vs_EKF.txt"); fo_log_ekf.printf("%%%% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y\n"); #endif // Init. simulation: // ------------------------- float x=VEHICLE_INITIAL_X,y=VEHICLE_INITIAL_Y,phi=DEG2RAD(-180),v=VEHICLE_INITIAL_V,w=VEHICLE_INITIAL_W; float t=0; while (winEKF.isOpen() && winPF.isOpen() && !mrpt::system::os::kbhit() ) { // Update vehicle: x+=v*DELTA_TIME*(cos(phi)-sin(phi)); y+=v*DELTA_TIME*(sin(phi)+cos(phi)); phi+=w*DELTA_TIME; v+=1.0f*DELTA_TIME*cos(t); w-=0.1f*DELTA_TIME*sin(t); // Simulate noisy observation: float realBearing = atan2( y,x ); float obsBearing = realBearing + BEARING_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized(); printf("Real/Simulated bearing: %.03f / %.03f deg\n", RAD2DEG(realBearing), RAD2DEG(obsBearing) ); float realRange = sqrt(square(x)+square(y)); float obsRange = max(0.0, realRange + RANGE_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized() ); printf("Real/Simulated range: %.03f / %.03f \n", realRange, obsRange ); // Process with EKF: EKF.doProcess(DELTA_TIME,obsRange, obsBearing); // Process with PF: CSensoryFrame SF; CObservationBearingRangePtr obsRangeBear = CObservationBearingRange::Create(); obsRangeBear->sensedData.resize(1); obsRangeBear->sensedData[0].range = obsRange; obsRangeBear->sensedData[0].yaw = obsBearing; SF.insert( obsRangeBear ); // memory freed by SF. EKF.getProfiler().enter("PF:complete_step"); PF.executeOn(particles, NULL,&SF); // Process in the PF EKF.getProfiler().leave("PF:complete_step"); // Show EKF state: CRangeBearing::KFVector EKF_xkk; CRangeBearing::KFMatrix EKF_pkk; EKF.getState( EKF_xkk, EKF_pkk ); printf("Real: x:%.03f y=%.03f heading=%.03f v=%.03f w=%.03f\n",x,y,phi,v,w); cout << "EKF: " << EKF_xkk << endl; // Show PF state: cout << "Particle filter ESS: " << particles.ESS() << endl; // Draw EKF state: CRangeBearing::KFMatrix COVXY(2,2); COVXY(0,0) = EKF_pkk(0,0); COVXY(1,1) = EKF_pkk(1,1); COVXY(0,1) = COVXY(1,0) = EKF_pkk(0,1); winEKF.plotEllipse( EKF_xkk[0], EKF_xkk[1], COVXY, 3, "b-2", "ellipse_EKF" ); // Save GT vs EKF state: #ifdef SAVE_GT_LOGS // %% GT_X GT_Y EKF_MEAN_X EKF_MEAN_Y EKF_STD_X EKF_STD_Y: fo_log_ekf.printf("%f %f %f %f %f %f\n", x,y, // Real (GT) EKF_xkk[0], EKF_xkk[1], std::sqrt(EKF_pkk(0,0)), std::sqrt(EKF_pkk(1,1)) ); #endif // Draw the velocity vector: vector_float vx(2),vy(2); vx[0] = EKF_xkk[0]; vx[1] = vx[0] + EKF_xkk[2] * 1; vy[0] = EKF_xkk[1]; vy[1] = vy[0] + EKF_xkk[3] * 1; winEKF.plot( vx,vy, "g-4", "velocityEKF" ); // Draw PF state: { size_t i,N = particles.m_particles.size(); vector_float parts_x(N),parts_y(N); for (i=0;i<N;i++) { parts_x[i] = particles.m_particles[i].d->x; parts_y[i] = particles.m_particles[i].d->y; } winPF.plot( parts_x, parts_y, "b.2", "particles" ); // Draw PF velocities: float avrg_x, avrg_y, avrg_vx,avrg_vy; particles.getMean(avrg_x, avrg_y, avrg_vx,avrg_vy); vector_float vx(2),vy(2); vx[0] = avrg_x; vx[1] = vx[0] + avrg_vx * 1; vy[0] = avrg_y; vy[1] = vy[0] + avrg_vy * 1; winPF.plot( vx,vy, "g-4", "velocityPF" ); } // Draw GT: winEKF.plot( vector_float(1,x), vector_float(1,y),"k.8","plot_GT"); winPF.plot( vector_float(1,x), vector_float(1,y),"k.8","plot_GT"); // Draw noisy observations: vector_float obs_x(2),obs_y(2); obs_x[0] = obs_y[0] = 0; obs_x[1] = obsRange * cos( obsBearing ); obs_y[1] = obsRange * sin( obsBearing ); winEKF.plot(obs_x,obs_y,"r", "plot_obs_ray"); winPF.plot(obs_x,obs_y,"r", "plot_obs_ray"); // Delay: mrpt::system::sleep((int)(DELTA_TIME*1000)); t+=DELTA_TIME; } }