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