Пример #1
0
/*---------------------------------------------------------------
				dumpAsText
  ---------------------------------------------------------------*/
void  CLocalMetricHypothesis::dumpAsText(utils::CStringList &st) const
{
	st.clear();
	st << "LIST OF POSES IN LMH";
	st << "====================";

	string s;
	s = "Neighbors: ";
	for (TNodeIDSet::const_iterator it=m_neighbors.begin();it!=m_neighbors.end();++it)
		s+=format("%i ",(int)*it);
	st << s;

	TMapPoseID2Pose3D lst;
	getMeans( lst );

	for (TMapPoseID2Pose3D::const_iterator it=lst.begin();it!=lst.end();++it)
	{
		map<TPoseID,CHMHMapNode::TNodeID>::const_iterator	area = m_nodeIDmemberships.find(it->first);

		string s = format("  ID: %i \t AREA: %i \t %.03f,%.03f,%.03fdeg",
			(int)it->first,
			(int)area->second,
			it->second.x(),it->second.y(),RAD2DEG(it->second.yaw()) );
		st << s;
	}
}
Пример #2
0
/*---------------------------------------------------------------
					getMeans
   Returns the mean of each robot pose in this LMH, as
     computed from the set of particles.
  ---------------------------------------------------------------*/
void CLocalMetricHypothesis::getMeans( TMapPoseID2Pose3D &outList ) const
{
	MRPT_START

	outList.clear();

	// Build list of particles pdfs:
	std::map< TPoseID, CPose3DPDFParticles >	parts;
	getPathParticles( parts );

	std::map< TPoseID, CPose3DPDFParticles >::iterator   it;

	for (it=parts.begin();it!=parts.end();it++)
		it->second.getMean( outList[ it->first ] );

	MRPT_END
}
/*---------------------------------------------------------------

					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:
            TMapPoseID2Pose3D  lstRobotPoses;
            LMH->getMeans( lstRobotPoses );

            CPose3D	  *currentRobotPose = & lstRobotPoses[currentPoseID];
            float		minDistLin    = 1e6f;
            float		minDistAng    = 1e6f;

            //printf("Poses in graph:\n");
            for (TMapPoseID2Pose3D::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
}