C++ CActionCollectionPtr::pointer方法代码示例

void PFLocalizationCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
    if(state_ == INIT) initializeFilter();
    tictac_.Tic();
    pf_.executeOn( pdf_, _action.pointer(), _sf.pointer(), &pf_stats_ );
    timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
    update_counter_++;
}

/*---------------------------------------------------------------

					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
//.........这里部分代码省略.........

/** This is the common function for all operations over a rawlog file ("filter" a rawlog file into a new one) or over the loaded rawlog (depending on the user selection in the GUI).
  */
void CFormChangeSensorPositions::executeOperationOnRawlog( TRawlogFilter operation, const char *endMsg )
{
    WX_START_TRY

    int   			processMax;
    bool			isInMemory;
    CStream 		*in_fil=NULL,*out_fil=NULL;


	sensorPoseReadOK = false;
	camReadIsOk = false;

    if (rbLoaded->GetValue())
    {
        // APPLY TO rawlog in memory:
        isInMemory = true;

        processMax = (int)rawlog.size();
    }
    else
    {
        // APPLY TO rawlog files:
        isInMemory = false;

        if ( !txtInputFile->GetValue().size() )
            THROW_EXCEPTION("An input rawlog file must be selected")
            if ( !txtOutputFile->GetValue().size() )
                THROW_EXCEPTION("An output rawlog file must be selected")

                string   fileName_IN( txtInputFile->GetValue().mbc_str() );
        if (!mrpt::system::fileExists(fileName_IN) )
            THROW_EXCEPTION("Input file does not exist!")

            string   fileName_OUT( txtOutputFile->GetValue().mbc_str() );

        if (!fileName_OUT.compare(fileName_IN))
            THROW_EXCEPTION("Input and output files must be different!")

		in_fil = new CFileGZInputStream(fileName_IN);
		out_fil = new CFileGZOutputStream(fileName_OUT);

        processMax = (int)in_fil->getTotalBytesCount();
    }

    wxProgressDialog    progDia(
        wxT("Modifying rawlog"),
        wxT("Processing..."),
        processMax, // range
        this, // parent
        wxPD_CAN_ABORT |
        wxPD_APP_MODAL |
        wxPD_SMOOTH |
        wxPD_AUTO_HIDE |
        wxPD_ELAPSED_TIME |
        wxPD_ESTIMATED_TIME |
        wxPD_REMAINING_TIME);

    wxTheApp->Yield();  // Let the app. process messages

    unsigned int		countLoop = 0;
    bool                keepLoading=true;
    string              errorMsg;
    wxString			auxStr;

    // Apply changes:
    int 	changes = 0;
    wxBusyCursor    cursor;

    while ((( !isInMemory && keepLoading ) ||
            (  isInMemory && countLoop < rawlog.size() ))&& !sensorPoseReadOK && !camReadIsOk )
    {
        CSerializablePtr newObj;
        try
        {
            if (isInMemory)
            {
                newObj = rawlog.getAsGeneric(countLoop);
            }
            else
            {
                (*in_fil) >> newObj;
            }

            // Check type:
            if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
            {
                // A sensory frame:
                CSensoryFramePtr sf(newObj);

                // Process & save:
				operation(NULL,sf.pointer(),changes );

				if (!isInMemory)  (*out_fil) << *sf.pointer();
            }
            else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
            {
                // This is an action:
                CActionCollectionPtr acts =CActionCollectionPtr( newObj );
//.........这里部分代码省略.........