C++ CActionRobotMovement2D类代码示例

		// return false on any error.
		bool processOneObservation(CObservationPtr  &o)
		{
			if (!IS_CLASS(o, CObservationOdometry ) )
				return true;

			CObservationOdometry* obs = CObservationOdometryPtr(o).pointer();

			if (!obs->hasEncodersInfo)
				throw std::runtime_error("CObservationOdometry entry does not contain encoder info, cannot recalculate odometry!"); 

			CActionRobotMovement2D auxOdoIncr;
			auxOdoIncr.hasEncodersInfo = true;
			auxOdoIncr.encoderLeftTicks = obs->encoderLeftTicks;
			auxOdoIncr.encoderRightTicks = obs->encoderRightTicks;
			auxOdoIncr.computeFromEncoders( KL,KR, D );

			if (!m_odo_accum_valid)
			{
				m_odo_accum_valid=true;
				m_odo_accum = obs->odometry;
				// and don't modify this odo val.
			}
			else
			{
				obs->odometry = m_odo_accum + auxOdoIncr.rawOdometryIncrementReading;
				m_odo_accum = obs->odometry;
			}
			m_entriesSaved++;
			return true; // All ok
		}

//.........这里部分代码省略.........
            {
                CLandmark   LM;
                CPointPDFGaussian   pt3D;

                // Random coordinates:
                pt3D.mean = CPoint3D(
                                randomGenerator.drawUniform(min_x,max_x),
                                randomGenerator.drawUniform(min_y,max_y),
                                randomGenerator.drawUniform(min_z,max_z) );

                // Add:
                LM.createOneFeature();
                LM.features[0]->type = featBeacon;
                LM.ID = uniqueIds++;
                LM.setPose( pt3D );

                landmarkMap.landmarks.push_back(LM);
            }

            if (nLandmarks) cout << nLandmarks << " generated for the 'randomset' " << randomSetCount << endl;

        }
        while (nLandmarks);


        landmarkMap.saveToTextFile( format("%s/%s_ground_truth.txt",outDir.c_str(),outFile.c_str()) );
        printf("Done!\n");

        // ---------------------------------------------
        // Simulate:
        // ---------------------------------------------
        size_t nWarningsNoSight=0;

        CActionRobotMovement2D::TMotionModelOptions   opts;
        opts.modelSelection = CActionRobotMovement2D::mmGaussian;
        opts.gausianModel.a1=0;
        opts.gausianModel.a2=0;
        opts.gausianModel.a3=0;
        opts.gausianModel.a4=0;
        opts.gausianModel.minStdXY = odometryNoiseXY_std;
        opts.gausianModel.minStdPHI = odometryNoisePhi_std;

        // Output rawlog, gz-compressed.
        CFileGZOutputStream  fil( format("%s/%s",outDir.c_str(),outFile.c_str()));
        CPose3D         realPose;

        const size_t N_STEPS_STOP_AT_THE_BEGINNING = 4;

        CMatrixDouble  GT_path;

        for (size_t i=0; i<nSteps; i++)
        {
            cout << "Generating step " << i << "...\n";
            CSensoryFrame           SF;
            CActionCollection         acts;

            CPose3D	incPose3D;
            bool    incPose_is_3D = random6DPath;

            if (i>=N_STEPS_STOP_AT_THE_BEGINNING)
            {
                if (random6DPath)
                {   // 3D path
                    const double Ar = DEG2RAD(3);
                    TPose3D  Ap = TPose3D(0.20*cos(Ar),0.20*sin(Ar),0,Ar,0,0);
                    //Ap.z  += randomGenerator.drawGaussian1D(0,0.05);

/** The PF algorithm implementation for "optimal sampling for non-parametric observation models"
  */
void  CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal(
	CLocalMetricHypothesis	*LMH,
	const mrpt::slam::CActionCollection	* actions,
	const mrpt::slam::CSensoryFrame		* sf,
	const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
{
	MRPT_START

		// Get the current robot pose estimation:
	TPoseID		currentPoseID = LMH->m_currentRobotPose;

	size_t				i,k,N,M = LMH->m_particles.size();

	// ----------------------------------------------------------------------
	//	  We can execute optimal PF only when we have both, an action, and
	//     a valid observation from which to compute the likelihood:
	//   Accumulate odometry/actions until we have a valid observation, then
	//    process them simultaneously.
	// ----------------------------------------------------------------------
//	static CActionRobotMovement2D	accumRobotMovement;
//	static bool						accumRobotMovementIsValid = false;
	bool  SFhasValidObservations = false;
	// A valid action?
	if (actions!=NULL)
	{
		CActionRobotMovement2DPtr act = actions->getBestMovementEstimation();	// Find a robot movement estimation:
		if (!act) THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D derived object!");

		if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
		{
			act->poseChange->getMean( LMH->m_accumRobotMovement.rawOdometryIncrementReading );
			LMH->m_accumRobotMovement.motionModelConfiguration = act->motionModelConfiguration;
		}
		else
			LMH->m_accumRobotMovement.rawOdometryIncrementReading =
				LMH->m_accumRobotMovement.rawOdometryIncrementReading +
				act->poseChange->getMeanVal();

		LMH->m_accumRobotMovementIsValid = true;
	}

	if (sf!=NULL)
	{
		ASSERT_(LMH->m_particles.size()>0);
		SFhasValidObservations = (*LMH->m_particles.begin()).d->metricMaps.canComputeObservationsLikelihood( *sf );
	}

	// All the needed things?
	if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
		return; // Nothing we can do here...

	// OK, we have all we need, let's start!

	// Take the accum. actions as input:
	CActionRobotMovement2D		theResultingRobotMov;

	// Over
	keep_max(
		LMH->m_accumRobotMovement.motionModelConfiguration.gausianModel.minStdXY,
		LMH->m_parent->m_options.MIN_ODOMETRY_STD_XY);
	keep_max(
		LMH->m_accumRobotMovement.motionModelConfiguration.gausianModel.minStdPHI,
		LMH->m_parent->m_options.MIN_ODOMETRY_STD_PHI);

	theResultingRobotMov.computeFromOdometry( LMH->m_accumRobotMovement.rawOdometryIncrementReading, LMH->m_accumRobotMovement.motionModelConfiguration );

	const CActionRobotMovement2D		*robotMovement = &theResultingRobotMov;

	LMH->m_accumRobotMovementIsValid = false; // To reset odometry at next iteration!


	// ----------------------------------------------------------------------
	//		0) Common part:  Prepare m_particles "draw" and compute
	// ----------------------------------------------------------------------
	// Precompute a list of "random" samples from the movement model:
	LMH->m_movementDraws.clear();

	// Fast pseudorandom generator of poses...
	//m_movementDraws.resize( max(2000,(int)(PF_options.pfAuxFilterOptimal_MaximumSearchSamples * 5.6574) ) );
	LMH->m_movementDraws.resize( PF_options.pfAuxFilterOptimal_MaximumSearchSamples * M );
	size_t			size_movementDraws = LMH->m_movementDraws.size();
	LMH->m_movementDrawsIdx = (unsigned int)floor(randomGenerator.drawUniform(0.0f,((float)size_movementDraws)-0.01f));

	robotMovement->prepareFastDrawSingleSamples();
	for (size_t i=0;i<LMH->m_movementDraws.size();i++)
		robotMovement->fastDrawSingleSample( LMH->m_movementDraws[i] );

	LMH->m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
	LMH->m_maxLikelihood.clear();
	LMH->m_maxLikelihood.resize(M,0);
	LMH->m_movementDrawMaximumLikelihood.resize(M);

	// Prepare data for executing "fastDrawSample"
	CTicTac		tictac;
	tictac.Tic();
	LMH->prepareFastDrawSample(
		PF_options,
		particlesEvaluator_AuxPFOptimal,
//.........这里部分代码省略.........

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

//.........这里部分代码省略.........

// Convert from observations only to actions-SF format:
void xRawLogViewerFrame::OnMenuConvertSF(wxCommandEvent& event)
{
	WX_START_TRY

	bool onlyOnePerLabel =
		(wxYES == wxMessageBox(
					  _("Keep only one observation of each label within each "
						"sensoryframe?"),
					  _("Convert to sensoryframe's"), wxYES_NO, this));

	wxString strMaxL = wxGetTextFromUser(
		_("Maximum length of each sensoryframe (seconds):"),
		_("Convert to sensoryframe's"), _("1.0"));
	double maxLengthSF;
	strMaxL.ToDouble(&maxLengthSF);

	// Process:
	CRawlog new_rawlog;
	new_rawlog.setCommentText(rawlog.getCommentText());

	wxBusyCursor waitCursor;
	unsigned int nEntries = (unsigned int)rawlog.size();

	wxProgressDialog progDia(
		wxT("Progress"), wxT("Parsing rawlog..."),
		nEntries,  // 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

	CSensoryFrame SF_new;
	set<string> SF_new_labels;
	TTimeStamp SF_new_first_t = INVALID_TIMESTAMP;
	CObservationOdometry::Ptr last_sf_odo, cur_sf_odo;

	for (unsigned int countLoop = 0; countLoop < nEntries; countLoop++)
	{
		if (countLoop % 20 == 0)
		{
			if (!progDia.Update(
					countLoop,
					wxString::Format(
						wxT("Parsing rawlog... %u objects"), countLoop)))
			{
				return;
			}
			wxTheApp->Yield();  // Let the app. process messages
		}

		switch (rawlog.getType(countLoop))
		{
			case CRawlog::etSensoryFrame:
			case CRawlog::etActionCollection:
			{
				THROW_EXCEPTION(
					"The rawlog already has sensory frames and/or actions!");
			}
			break;

			case CRawlog::etObservation:
			{
				CObservation::Ptr o = rawlog.getAsObservation(countLoop);

				// Update stats:
				bool label_existed =
					SF_new_labels.find(o->sensorLabel) != SF_new_labels.end();
				double SF_len =
					SF_new_first_t == INVALID_TIMESTAMP
						? 0
						: timeDifference(SF_new_first_t, o->timestamp);

				// Decide:
				// End SF and start a new one?
				if (SF_len > maxLengthSF && SF_new.size() != 0)
				{
					new_rawlog.addObservations(SF_new);

					// Odometry increments:
					CActionCollection acts;
					if (last_sf_odo && cur_sf_odo)
					{
						CActionRobotMovement2D act;
						act.timestamp = cur_sf_odo->timestamp;
						CActionRobotMovement2D::TMotionModelOptions opts;
						act.computeFromOdometry(
							cur_sf_odo->odometry - last_sf_odo->odometry, opts);
						acts.insert(act);
					}
					new_rawlog.addActions(acts);

					last_sf_odo = cur_sf_odo;
					cur_sf_odo.reset();

					SF_new.clear();
					SF_new_labels.clear();
					SF_new_first_t = INVALID_TIMESTAMP;
				}
//.........这里部分代码省略.........

void xRawLogViewerFrame::OnMenuLossLessDecFILE(wxCommandEvent& event)
{
	WX_START_TRY

	string filToOpen;
	if (!AskForOpenRawlog(filToOpen)) return;

	wxString strDecimation = wxGetTextFromUser(
		_("The number of observations will be decimated (only 1 out of M will "
		  "be kept). Enter the decimation ratio M:"),
		_("Decimation"), _("1"));
	long DECIMATE_RATIO;
	strDecimation.ToLong(&DECIMATE_RATIO);

	ASSERT_(DECIMATE_RATIO >= 1)

	string filToSave;
	AskForSaveRawlog(filToSave);

	CFileGZInputStream fil(filToOpen);
	CFileGZOutputStream f_out(filToSave);

	wxBusyCursor waitCursor;
	unsigned int filSize = (unsigned int)fil.getTotalBytesCount();

	wxString auxStr;
	wxProgressDialog progDia(
		wxT("Progress"), wxT("Parsing file..."),
		filSize,  // 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;
	int entryIndex = 0;
	bool keepLoading = true;
	string errorMsg;

	// ------------------------------------------------------------------------------
	// METHOD TO BE MEMORY EFFICIENT:
	//  To free the memory of the current rawlog entries as we create the new
	//  one,
	//  then call "clearWithoutDelete" at the end.
	// ------------------------------------------------------------------------------
	CSensoryFrame::Ptr accum_sf;
	CActionRobotMovement2D::TMotionModelOptions odometryOptions;
	bool cummMovementInit = false;
	long SF_counter = 0;

	// Reset cummulative pose change:
	CPose2D accumMovement(0, 0, 0);

	TTimeStamp timestamp_lastAction = INVALID_TIMESTAMP;

	while (keepLoading)
	{
		if (countLoop++ % 100 == 0)
		{
			auxStr.sprintf(wxT("Parsing file... %u objects"), entryIndex);
			if (!progDia.Update((int)fil.getPosition(), auxStr))
				keepLoading = false;
			wxTheApp->Yield();  // Let the app. process messages
		}

		CSerializable::Ptr newObj;
		try
		{
			fil >> newObj;
			entryIndex++;

			// Check type:
			if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
			{
				// Decimate Observations
				// ---------------------------

				// Add observations to the accum. SF:
				if (!accum_sf)
					accum_sf = mrpt::make_aligned_shared<CSensoryFrame>();

				// Copy pointers to observations only (fast):
				accum_sf->moveFrom(
					*std::dynamic_pointer_cast<CSensoryFrame>(newObj));

				if (++SF_counter >= DECIMATE_RATIO)
				{
					SF_counter = 0;

					// INSERT OBSERVATIONS:
					f_out << *accum_sf;
					accum_sf.reset();

					// INSERT ACTIONS:
					CActionCollection::Ptr actsCol =
						mrpt::make_aligned_shared<CActionCollection>();
					if (cummMovementInit)
					{
						CActionRobotMovement2D cummMovement;
//.........这里部分代码省略.........

//.........这里部分代码省略.........

        // ---------------------------------------------
        // Create the point-beacons:
        // ---------------------------------------------
        printf("Creating beacon map...");
        mrpt::slam::CBeaconMap    beaconMap;
        for (i=0;i<nBeacons;i++)
        {
            CBeacon     b;
            CPoint3D    pt3D;

            // Random coordinates:
            pt3D.x( randomGenerator.drawUniform(min_x,max_x) );
            pt3D.y( randomGenerator.drawUniform(min_y,max_y) );
            pt3D.z( randomGenerator.drawUniform(min_z,max_z) );

            // Add:
            b.m_typePDF=CBeacon::pdfMonteCarlo;
            b.m_locationMC.setSize(1,pt3D);
			b.m_ID = i;
            beaconMap.push_back(b);
        }


        os::sprintf(auxStr,sizeof(auxStr),"%s/outSimMap.txt",outDir.c_str());
        beaconMap.saveToTextFile(auxStr);
        printf("Done!\n");

        //beaconMap.simulateBeaconReadings(  );

        // ---------------------------------------------
        // Simulate:
        // ---------------------------------------------
        CActionRobotMovement2D::TMotionModelOptions   opts;
        CPoint3D                null3D(0,0,0);
        opts.modelSelection = CActionRobotMovement2D::mmGaussian;
	opts.gausianModel.a1=0;
	opts.gausianModel.a2=0;
	opts.gausianModel.a3=0;
	opts.gausianModel.a4=0;
	opts.gausianModel.minStdXY = odometryNoiseXY_std;
	opts.gausianModel.minStdPHI = DEG2RAD( 0.002f );

        os::sprintf(auxStr,sizeof(auxStr),"%s/%s",outDir.c_str(),outFile.c_str());
        CFileOutputStream     fil(auxStr);
        CPose2D         realPose;
	CPose2D         incPose;
	int	stopSteps = 4;

        for (i=0;i<nSteps;i++)
        {
            printf("Generating step %i...",i);
            CSensoryFrame           SF;
            CActionCollection         acts;
            CActionRobotMovement2D    act;
            CPose3D                 pose3D( realPose );


			if (i>=stopSteps)
			{
				if (circularPath)
				{
					// Circular path:
					float Ar = DEG2RAD(5);
					incPose = CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar);
				}

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