Beispiel #1
0
void mrpt::serialization::OctetVectorToObject(
	const std::vector<uint8_t>& in_data, CSerializable::Ptr& obj)
{
	obj.reset();

	if (in_data.empty()) return;
	auto arch = archiveFrom(in_data);
	obj = arch.ReadObject();
}
Beispiel #2
0
/*---------------------------------------------------------------
				deserializeIntoNewObject
 ---------------------------------------------------------------*/
void CMessage::deserializeIntoNewObject(CSerializable::Ptr& obj)
{
	MRPT_START
	CMemoryStream auxStream;

	// Copy data into the stream:
	if (!content.empty())
	{
		auxStream.WriteBuffer(&content[0], content.size());
		auxStream.Seek(0);

		// Try to parse data into a new object:
		obj = auxStream.ReadObject();
	}
	else
		obj.reset();

	MRPT_END
}
Beispiel #3
0
/*---------------------------------------------------------------
				deserializeIntoNewObject
 ---------------------------------------------------------------*/
void CMessage::deserializeIntoNewObject(CSerializable::Ptr& obj)
{
	MRPT_START
	std::stringstream auxStream;
	auto arch = mrpt::serialization::archiveFrom<std::iostream>(auxStream);

	// Copy data into the stream:
	if (!content.empty())
	{
		arch.WriteBuffer(&content[0], content.size());
		auxStream.seekg(0);

		// Try to parse data into a new object:
		obj = arch.ReadObject();
	}
	else
		obj.reset();

	MRPT_END
}
Beispiel #4
0
void CStream_WriteObject(CArchive& self, CSerializable::Ptr& obj)
{
	self.WriteObject(obj.get());
}
Beispiel #5
0
// CStream
void CStream_ReadObject(CArchive& self, CSerializable::Ptr& obj)
{
	self.ReadObject(obj.get());
}
Beispiel #6
0
void xRawLogViewerFrame::OnMenuConvertObservationOnly(wxCommandEvent& event)
{
	WX_START_TRY

	wxMessageBox(_("Select the rawlog file to convert..."));

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

	wxMessageBox(_("Select the target file where to save the new rawlog."));
	string filToSave;
	if (!AskForSaveRawlog(filToSave)) return;

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

	CFileGZOutputStream f_out(filToSave);

	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;
	bool keepLoading = true;
	string errorMsg;

	CPose2D odometry_accum;

	// We'll save here all the individual observations ordered in time:
	TListTimeAndObservations time_ordered_list_observation;

	mrpt::system::TTimeStamp lastValidObsTime = INVALID_TIMESTAMP;

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

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

			// Check type:
			if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
			{
				CSensoryFrame::Ptr SF(
					std::dynamic_pointer_cast<CSensoryFrame>(newObj));
				for (CSensoryFrame::iterator it = SF->begin(); it != SF->end();
					 ++it)
				{
					time_ordered_list_observation.insert(
						TTimeObservationPair((*it)->timestamp, (*it)));
					lastValidObsTime = (*it)->timestamp;
				}
			}
			else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
			{
				// Replace "odometry action" with "odometry observation":
				CActionCollection::Ptr acts =
					std::dynamic_pointer_cast<CActionCollection>(newObj);
				// Get odometry:
				CActionRobotMovement2D::Ptr actOdom =
					acts->getBestMovementEstimation();
				if (actOdom)
				{
					odometry_accum =
						odometry_accum + actOdom->poseChange->getMeanVal();

					// Generate "odometry obs":
					CObservationOdometry::Ptr newO =
						mrpt::make_aligned_shared<CObservationOdometry>();
					newO->sensorLabel = "odometry";
					newO->timestamp = actOdom->timestamp != INVALID_TIMESTAMP
										  ? actOdom->timestamp
										  : lastValidObsTime;
					newO->odometry = odometry_accum;

					time_ordered_list_observation.insert(
						TTimeObservationPair(newO->timestamp, newO));
				}
			}
			else if (
				newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
			{
				CObservation::Ptr o =
					std::dynamic_pointer_cast<CObservation>(newObj);
				time_ordered_list_observation.insert(
					TTimeObservationPair(o->timestamp, o));
			}

			// Dump to the new file: Only the oldest one:
			// --------------------------------------------------
			if (time_ordered_list_observation.size() > 30)
			{
				// Save a few of the oldest and continue:
				for (unsigned i = 0; i < 15; i++)
				{
					f_out << *(time_ordered_list_observation.begin()->second);
					time_ordered_list_observation.erase(
						time_ordered_list_observation.begin());
				}
			}

			// Free memory:
			newObj.reset();
		}
		catch (exception& e)
		{
			errorMsg = e.what();
			keepLoading = false;
		}
		catch (...)
		{
			keepLoading = false;
		}
	}  // end while keep loading

	// Save the rest to the out file:
	while (!time_ordered_list_observation.empty())
	{
		f_out << *(time_ordered_list_observation.begin()->second);
		time_ordered_list_observation.erase(
			time_ordered_list_observation.begin());
	}

	progDia.Update(filSize);

	// Set error msg:
	wxMessageBox(_("Done"));

	WX_END_TRY
}
Beispiel #7
0
void xRawLogViewerFrame::OnMenuConvertExternallyStored(wxCommandEvent& event)
{
	WX_START_TRY

	wxMessageBox(_("Select the rawlog file with embedded images."));

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

	wxMessageBox(_("Select the target file where to save the new rawlog."));
	string filToSave;
	if (!AskForSaveRawlog(filToSave)) return;

	// Create the default "/Images" directory.
	string outDir = extractFileDirectory(filToSave) + string("/") +
					extractFileName(filToSave) + string("_Images");
	if (fileExists(outDir))
	{
		wxMessageBox(
			_U(format(
				   "*ABORTING*: Output directory for images already exists. "
				   "Select a different output path or remove the "
				   "directory:\n%s",
				   outDir.c_str())
				   .c_str()));
		return;
	}

	createDirectory(outDir);
	if (!fileExists(outDir))
	{
		wxMessageBox(
			_U(format(
				   "*ABORTING*: Cannot create directory:\n%s", outDir.c_str())
				   .c_str()));
		return;
	}

	// Add the final /
	outDir += "/";

	// Let the user choose the image format:
	string imgFileExtension = AskForImageFileFormat();
	if (imgFileExtension.empty()) return;

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

	CFileGZOutputStream f_out(filToSave);

	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 imgSaved = 0;
	bool keepLoading = true;
	string errorMsg;

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

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

			// Check type:
			if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
			{
				CSensoryFrame::Ptr SF(
					std::dynamic_pointer_cast<CSensoryFrame>(newObj));

				for (unsigned k = 0; k < SF->size(); k++)
				{
					if (SF->getObservationByIndex(k)->GetRuntimeClass() ==
						CLASS_ID(CObservationStereoImages))
					{
						CObservationStereoImages::Ptr obsSt =
							SF->getObservationByIndexAs<
								CObservationStereoImages::Ptr>(k);

						// save image to file & convert into external storage:
						string fileName = format(
							"img_stereo_%u_left_%05u.%s", k, imgSaved,
							imgFileExtension.c_str());
						obsSt->imageLeft.saveToFile(outDir + fileName);
						obsSt->imageLeft.setExternalStorage(fileName);

						imgSaved++;

						// save image to file & convert into external storage:
						fileName = format(
							"img_stereo_%u_right_%05u.%s", k, imgSaved,
							imgFileExtension.c_str());
						obsSt->imageRight.saveToFile(outDir + fileName);
						obsSt->imageRight.setExternalStorage(fileName);

						imgSaved++;
					}
					if (SF->getObservationByIndex(k)->GetRuntimeClass() ==
						CLASS_ID(CObservationImage))
					{
						CObservationImage::Ptr obsIm =
							SF->getObservationByIndexAs<CObservationImage::Ptr>(
								k);

						// save image to file & convert into external storage:
						string fileName = format(
							"img_monocular_%u_%05u.%s", k, imgSaved,
							imgFileExtension.c_str());
						obsIm->image.saveToFile(outDir + fileName);
						obsIm->image.setExternalStorage(fileName);

						imgSaved++;
					}
				}
			}
			else if (
				newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) ||
				newObj->GetRuntimeClass() == CLASS_ID(CPose2D))
			{
			}
			else
			{
				// Unknown class:
				THROW_EXCEPTION("Unknown class found in the file!");
			}

			// Dump to the new file:
			f_out << *newObj;

			// Free memory:
			newObj.reset();
		}
		catch (exception& e)
		{
			errorMsg = e.what();
			keepLoading = false;
		}
		catch (...)
		{
			keepLoading = false;
		}
	}  // end while keep loading

	progDia.Update(filSize);

	// Set error msg:
	wxMessageBox(
		_U(format("Images saved: %i", imgSaved).c_str()), _("Done"), wxOK,
		this);

	WX_END_TRY
}
Beispiel #8
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
}
Beispiel #9
0
void xRawLogViewerFrame::OnMenuLossLessDecimate(wxCommandEvent& event)
{
	WX_START_TRY

	CRawlog newRawLog;
	newRawLog.setCommentText(rawlog.getCommentText());

	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)

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

	size_t i, N = rawlog.size();

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

	// For each entry:
	for (i = 0; i < N; i++)
	{
		CSerializable::Ptr obj = rawlog.getAsGeneric(i);

		if (rawlog.getType(i) == CRawlog::etActionCollection)
		{
			// Accumulate Actions
			// ----------------------
			CActionCollection::Ptr curActs =
				std::dynamic_pointer_cast<CActionCollection>(obj);
			CActionRobotMovement2D::Ptr mov =
				curActs->getBestMovementEstimation();
			if (mov)
			{
				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 if (rawlog.getType(i) == CRawlog::etSensoryFrame)
		{
			// 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>(obj));

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

				// INSERT OBSERVATIONS:
				newRawLog.addObservationsMemoryReference(accum_sf);
				accum_sf.reset();

				// INSERT ACTIONS:
				CActionCollection actsCol;
				if (cummMovementInit)
				{
					CActionRobotMovement2D cummMovement;
					cummMovement.computeFromOdometry(
						accumMovement, odometryOptions);
					actsCol.insert(cummMovement);
					// Reset odometry accumulation:
					accumMovement = CPose2D(0, 0, 0);
				}
				newRawLog.addActions(actsCol);
			}
		}
		else
			THROW_EXCEPTION(
				"This operation implemented for SF-based rawlogs only.");

		// Delete object
		obj.reset();

	}  // end for i each entry

	// Clear the list only (objects already deleted)
	rawlog.clear();

	// Copy as new log:
	rawlog = newRawLog;

	rebuildTreeView();

	WX_END_TRY
}