void CRawlog::addAction(CAction& action)
{
CActionCollection::Ptr temp =
mrpt::make_aligned_shared<CActionCollection>();
temp->insert(action);
m_seqOfActObs.push_back(temp);
}
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
}