void CFormMotionModel::applyToLoadedRawlog()
{
WX_START_TRY
// Apply changes:
size_t changes = 0;
{
wxBusyCursor cursor;
size_t first = 0;
size_t last = rawlog.size() - 1;
if (!cbAll->GetValue())
{
first = atoi(string(edRangeFrom->GetValue().mb_str()).c_str());
last = atoi(string(edRangeTo->GetValue().mb_str()).c_str());
}
for (size_t i = first; i <= last; i++)
{
// Check type:
if (rawlog.getType(i) == CRawlog::etActionCollection)
{
// This is an action:
CActionCollection::Ptr acts = rawlog.getAsAction(i);
CActionRobotMovement2D::Ptr firstActionMov =
acts->getActionByClass<CActionRobotMovement2D>();
if (firstActionMov)
{
if (firstActionMov->estimationMethod ==
CActionRobotMovement2D::emOdometry)
{
// Use the kinematics motion model to estimate a pose
// change gaussian approximation:
firstActionMov->computeFromOdometry(
firstActionMov->rawOdometryIncrementReading,
options);
}
else
{
// Take the mean value
firstActionMov->computeFromOdometry(
firstActionMov->poseChange->getMeanVal(), options);
}
changes++;
}
} // end is action
} // end for i
}
wxString str;
str.sprintf(_("Objects changed: %d"), changes);
wxMessageBox(str, _("Result:"), wxOK, this);
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuCompactRawlog(wxCommandEvent& event)
{
WX_START_TRY
bool onlyOnePerLabel =
(wxYES == wxMessageBox(
_("Keep only one observation of each label within each "
"sensoryframe?"),
_("Compact rawlog"), wxYES_NO, this));
int progress_N = static_cast<int>(rawlog.size());
int progress_i = progress_N;
wxProgressDialog progDia(
wxT("Compacting rawlog"), wxT("Processing..."),
progress_N, // 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
CActionRobotMovement2D::Ptr lastAct;
CSensoryFrame::Ptr lastSF; // = nullptr;
unsigned counter_loops = 0;
unsigned nActionsDel = 0;
unsigned nEmptySFDel = 0;
CRawlog::iterator it = rawlog.begin();
for (progress_i = 0; it != rawlog.end(); progress_i--)
{
if (counter_loops++ % 50 == 0)
{
if (!progDia.Update(progress_N - progress_i)) break;
wxTheApp->Yield(); // Let the app. process messages
}
bool deleteThis = false;
if (it.getType() == CRawlog::etActionCollection)
{
// Is this a part of multiple actions?
if (lastAct)
{
// Remove this one and add it to the first in the series:
CActionRobotMovement2D::Ptr act =
std::dynamic_pointer_cast<CActionCollection>(*it)
->getMovementEstimationByType(
CActionRobotMovement2D::emOdometry);
ASSERT_(act);
lastAct->computeFromOdometry(
lastAct->rawOdometryIncrementReading +
act->rawOdometryIncrementReading,
lastAct->motionModelConfiguration);
deleteThis = true;
nActionsDel++;
}
else
{
// This is the first one:
lastAct = std::dynamic_pointer_cast<CActionCollection>(*it)
->getMovementEstimationByType(
CActionRobotMovement2D::emOdometry);
ASSERT_(lastAct);
// Before leaving the last SF, leave only one observation for
// each sensorLabel:
if (onlyOnePerLabel && lastSF)
{
CSensoryFrame::Ptr newSF =
mrpt::make_aligned_shared<CSensoryFrame>();
set<string> knownLabels;
for (CSensoryFrame::const_iterator o = lastSF->begin();
o != lastSF->end(); ++o)
{
if (knownLabels.find((*o)->sensorLabel) ==
knownLabels.end())
newSF->insert(*o);
knownLabels.insert((*o)->sensorLabel);
}
*lastSF = *newSF;
}
// Ok, done:
lastSF.reset();
}
}
else if (it.getType() == CRawlog::etSensoryFrame)
{
// Is this a part of a series?
if (lastSF)
{
// remove this one and accumulate in the first in the serie:
lastSF->moveFrom(
*std::dynamic_pointer_cast<CSensoryFrame>(*it));
deleteThis = true;
nEmptySFDel++;
}
else
{
// This is the first SF:
CSensoryFrame::Ptr sf =
std::dynamic_pointer_cast<CSensoryFrame>(*it);
// Only take into account if not empty!
if (sf->size())
{
lastSF = sf;
ASSERT_(lastSF);
lastAct.reset();
}
else
{
deleteThis = true;
nEmptySFDel++;
}
}
}
else
THROW_EXCEPTION("Unexpected class found!")
if (deleteThis)
{
it = rawlog.erase(it);
progress_i--; // Extra count
}
else
it++;
}
progDia.Update(progress_N);
string str = format(
"%u actions deleted\n%u sensory frames deleted", nActionsDel,
nEmptySFDel);
::wxMessageBox(_U(str.c_str()));
rebuildTreeView();
WX_END_TRY
}