/*---------------------------------------------------------------
pushObservation
---------------------------------------------------------------*/
void CHMTSLAM::pushObservation( const CObservationPtr &obs )
{
if (m_terminateThreads)
{ // Discard it:
//delete obs;
return;
}
// Add a CSensoryFrame with the obs:
CSensoryFramePtr sf = CSensoryFrame::Create();
sf->insert(obs); // memory will be freed when deleting the SF in other thread
{ // Wait for critical section
CCriticalSectionLocker locker( &m_inputQueue_cs);
m_inputQueue.push( sf );
}
}
// Get the rawlog entry (from cur. loaded rawlog), build and displays its map:
void CScanAnimation::RebuildMaps()
{
WX_START_TRY
int idx = slPos->GetValue();
if ( idx>=0 && idx<(int)rawlog.size() )
{
if (rawlog.getType(idx)==CRawlog::etSensoryFrame)
{
CSensoryFramePtr sf = rawlog.getAsObservations(idx);
BuildMapAndRefresh(sf.pointer());
}
else
if (rawlog.getType(idx)==CRawlog::etObservation)
{
CSensoryFramePtr sf = CSensoryFrame::Create();
sf->insert( rawlog.getAsObservation(idx) );
BuildMapAndRefresh(sf.pointer());
}
}
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
CActionRobotMovement2DPtr lastAct;
CSensoryFramePtr lastSF; // = NULL;
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:
CActionRobotMovement2DPtr act = CActionCollectionPtr(*it)->getMovementEstimationByType( CActionRobotMovement2D::emOdometry );
ASSERT_(act);
lastAct->computeFromOdometry( lastAct->rawOdometryIncrementReading + act->rawOdometryIncrementReading, lastAct->motionModelConfiguration);
deleteThis=true;
nActionsDel++;
}
else
{
// This is the first one:
lastAct = CActionCollectionPtr(*it)->getMovementEstimationByType( CActionRobotMovement2D::emOdometry );
ASSERT_(lastAct);
// Before leaving the last SF, leave only one observation for each sensorLabel:
if (onlyOnePerLabel && lastSF)
{
CSensoryFramePtr newSF = CSensoryFrame::Create();
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.clear_unique();
}
}
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( * CSensoryFramePtr(*it) );
deleteThis = true;
nEmptySFDel++;
}
else
{
// This is the first SF:
CSensoryFramePtr sf = CSensoryFramePtr(*it);
// Only take into account if not empty!
if (sf->size())
{
lastSF = sf;
ASSERT_(lastSF);
lastAct.clear_unique();
}
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
}