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();
}
/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
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
}
void CStream_WriteObject(CArchive& self, CSerializable::Ptr& obj)
{
self.WriteObject(obj.get());
}
// CStream
void CStream_ReadObject(CArchive& self, CSerializable::Ptr& obj)
{
self.ReadObject(obj.get());
}
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
}
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
}
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
}
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
}