Пример #1
0
void CRawlog::addAction(CAction& action)
{
	CActionCollection::Ptr temp =
		mrpt::make_aligned_shared<CActionCollection>();
	temp->insert(action);
	m_seqOfActObs.push_back(temp);
}
Пример #2
0
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;
						cummMovement.computeFromOdometry(
							accumMovement, odometryOptions);
						cummMovement.timestamp = timestamp_lastAction;
						actsCol->insert(cummMovement);
						// Reset odometry accumulation:
						accumMovement = CPose2D(0, 0, 0);
					}
					f_out << *actsCol;
					actsCol.reset();
				}
			}
			else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
			{
				// Accumulate Actions
				// ----------------------
				CActionCollection::Ptr curActs =
					std::dynamic_pointer_cast<CActionCollection>(newObj);
				CActionRobotMovement2D::Ptr mov =
					curActs->getBestMovementEstimation();
				if (mov)
				{
					timestamp_lastAction = mov->timestamp;
					CPose2D incrPose = mov->poseChange->getMeanVal();

					// If we have previous observations, shift them according to
					// this new increment:
					if (accum_sf)
					{
						CPose3D inv_incrPose3D(
							CPose3D(0, 0, 0) - CPose3D(incrPose));

						for (CSensoryFrame::iterator it = accum_sf->begin();
							 it != accum_sf->end(); ++it)
						{
							CPose3D tmpPose;

							(*it)->getSensorPose(tmpPose);
							tmpPose = inv_incrPose3D + tmpPose;
							(*it)->setSensorPose(tmpPose);
						}
					}

					// Accumulate from odometry:
					accumMovement = accumMovement + incrPose;

					// Copy the probabilistic options from the first entry we
					// find:
					if (!cummMovementInit)
					{
						odometryOptions = mov->motionModelConfiguration;
						cummMovementInit = true;
					}
				}
			}
			else
			{
				// Unknown class:
				THROW_EXCEPTION("Unknown class found in the file!");
			}
		}
		catch (exception& e)
		{
			errorMsg = e.what();
			keepLoading = false;
		}
		catch (...)
		{
			keepLoading = false;
		}
	}  // end while keep loading

	progDia.Update(filSize);

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

	WX_END_TRY
}