void PFLocalizationCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
if(state_ == INIT) initializeFilter();
tictac_.Tic();
pf_.executeOn( pdf_, _action.pointer(), _sf.pointer(), &pf_stats_ );
timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
update_counter_++;
}
/*---------------------------------------------------------------
CLSLAM_RBPF_2DLASER
Implements a 2D local SLAM method based on a RBPF
over an occupancy grid map. A part of HMT-SLAM.
\param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
\param act The action to process (or NULL).
\param sf The observations to process (or NULL).
WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
--------------------------------------------------------------- */
void CLSLAM_RBPF_2DLASER::processOneLMH(
CLocalMetricHypothesis *LMH,
const CActionCollectionPtr &actions,
const CSensoryFramePtr &sf )
{
MRPT_START
// Get the current robot pose estimation:
TPoseID currentPoseID = LMH->m_currentRobotPose;
// If this is the first iteration, just create a new robot pose at the origin:
if (currentPoseID == POSEID_INVALID )
{
currentPoseID = CHMTSLAM::generatePoseID();
LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH
// Create a new robot pose:
CPose3D initPose(0,0,0);
ASSERT_( LMH->m_particles.size()>0 );
for (CLocalMetricHypothesis::CParticleList::iterator it=LMH->m_particles.begin();it!=LMH->m_particles.end();++it)
it->d->robotPoses[ currentPoseID ] = initPose;
ASSERT_( m_parent->m_map.nodeCount()==1 );
m_parent->m_map_cs.enter();
CHMHMapNodePtr firstArea = m_parent->m_map.getFirstNode();
ASSERT_(firstArea);
LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();
// Set annotation for the reference pose:
firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, currentPoseID , LMH->m_ID);
m_parent->m_map_cs.leave();
}
bool insertNewRobotPose = false;
if (sf)
{
if ( LMH->m_nodeIDmemberships.size()<2 ) // If there is just 1 node (the current robot pose), then there is no observation in the map yet!
{ // Update map if this is the first observation!
insertNewRobotPose = true;
}
else
{
// Check minimum distance from current robot pose to those in the neighborhood:
map< TPoseID, CPose3D > lstRobotPoses;
LMH->getMeans( lstRobotPoses );
CPose3D *currentRobotPose = & lstRobotPoses[currentPoseID];
float minDistLin = 1e6f;
float minDistAng = 1e6f;
//printf("Poses in graph:\n");
for (map< TPoseID, CPose3D >::iterator it = lstRobotPoses.begin();it!=lstRobotPoses.end();++it)
{
if (it->first != currentPoseID )
{
float linDist = it->second.distanceTo( *currentRobotPose );
float angDist = fabs(math::wrapToPi( it->second.yaw() - currentRobotPose->yaw() ));
minDistLin = min( minDistLin, linDist );
if ( linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS )
minDistAng = min(minDistAng, angDist);
}
}
// time to insert a new node??
insertNewRobotPose = (minDistLin>m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) || ( minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS );
}
} // end if there are SF
// Save data in members so PF callback "prediction_and_update_pfXXXX" have access to them:
m_insertNewRobotPose = insertNewRobotPose;
// ------------------------------------------------
// Execute RBPF method:
// 1) PROCESS ACTION
// 2) PROCESS OBSERVATIONS
// ------------------------------------------------
CParticleFilter pf;
pf.m_options = m_parent->m_options.pf_options;
pf.executeOn( *LMH, actions.pointer(), sf.pointer() );
// 3) The appearance observation: update the log likelihood
// ...
// -----------------------------------------------------------
// 4) UPDATE THE MAP
// -----------------------------------------------------------
if (insertNewRobotPose)
{
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
// Leave the up-to-now "current pose" in the map, insert the SF in it, and...
// ----------------------------------------------------------------------------
TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();
// ...and create a new "current pose" making a copy of the previous one:
// and insert the observations into the metric maps:
// ----------------------------------------------------------------------------
for (CLocalMetricHypothesis::CParticleList::iterator partIt = LMH->m_particles.begin();
partIt!=LMH->m_particles.end();
partIt++)
{
const CPose3D *curRobotPose = &partIt->d->robotPoses[currentPoseID];
partIt->d->robotPoses[newCurrentPoseID]= *curRobotPose;
sf->insertObservationsInto( &partIt->d->metricMaps, curRobotPose );
}
// Add node membership: for now, a copy of the current pose:
LMH->m_nodeIDmemberships[newCurrentPoseID] = LMH->m_nodeIDmemberships[currentPoseID];
// Now, insert the SF in the just added robot pose:
// -----------------------------------------------------
LMH->m_SFs[ currentPoseID ] = *sf;
// Sets the new poseID as current robot pose:
// ----------------------------------------------------
TPoseID newlyAddedPose = currentPoseID;
currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;
// Mark the "newlyAddedPose" as pending to reconsider in the graph-cut method
// (Done in the main loop @ LSLAM thread)
// --------------------------------------------------------------------------------
LMH->m_posesPendingAddPartitioner.push_back( newlyAddedPose );
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Added pose %i.\n", (int)newlyAddedPose);
// Notice LC detectors:
// ------------------------------
{
synch::CCriticalSectionLocker lock( &m_parent->m_topLCdets_cs );
for (std::deque<CTopLCDetectorBase*>::iterator it=m_parent->m_topLCdets.begin();it!=m_parent->m_topLCdets.end();++it)
(*it)->OnNewPose( newlyAddedPose, sf.pointer() );
}
} // end of insertNewRobotPose
MRPT_END
}
/** This is the common function for all operations over a rawlog file ("filter" a rawlog file into a new one) or over the loaded rawlog (depending on the user selection in the GUI).
*/
void CFormChangeSensorPositions::executeOperationOnRawlog( TRawlogFilter operation, const char *endMsg )
{
WX_START_TRY
int processMax;
bool isInMemory;
CStream *in_fil=NULL,*out_fil=NULL;
sensorPoseReadOK = false;
camReadIsOk = false;
if (rbLoaded->GetValue())
{
// APPLY TO rawlog in memory:
isInMemory = true;
processMax = (int)rawlog.size();
}
else
{
// APPLY TO rawlog files:
isInMemory = false;
if ( !txtInputFile->GetValue().size() )
THROW_EXCEPTION("An input rawlog file must be selected")
if ( !txtOutputFile->GetValue().size() )
THROW_EXCEPTION("An output rawlog file must be selected")
string fileName_IN( txtInputFile->GetValue().mbc_str() );
if (!mrpt::system::fileExists(fileName_IN) )
THROW_EXCEPTION("Input file does not exist!")
string fileName_OUT( txtOutputFile->GetValue().mbc_str() );
if (!fileName_OUT.compare(fileName_IN))
THROW_EXCEPTION("Input and output files must be different!")
in_fil = new CFileGZInputStream(fileName_IN);
out_fil = new CFileGZOutputStream(fileName_OUT);
processMax = (int)in_fil->getTotalBytesCount();
}
wxProgressDialog progDia(
wxT("Modifying rawlog"),
wxT("Processing..."),
processMax, // 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;
wxString auxStr;
// Apply changes:
int changes = 0;
wxBusyCursor cursor;
while ((( !isInMemory && keepLoading ) ||
( isInMemory && countLoop < rawlog.size() ))&& !sensorPoseReadOK && !camReadIsOk )
{
CSerializablePtr newObj;
try
{
if (isInMemory)
{
newObj = rawlog.getAsGeneric(countLoop);
}
else
{
(*in_fil) >> newObj;
}
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
{
// A sensory frame:
CSensoryFramePtr sf(newObj);
// Process & save:
operation(NULL,sf.pointer(),changes );
if (!isInMemory) (*out_fil) << *sf.pointer();
}
else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
{
// This is an action:
CActionCollectionPtr acts =CActionCollectionPtr( newObj );
// Process & save:
operation( (CActionCollection*)acts.pointer(),NULL,changes);
if (!isInMemory) (*out_fil) << *acts;
}
else if ( newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
{
// A sensory frame:
CObservationPtr o(newObj);
static CSensoryFrame sf;
sf.clear();
sf.insert(o);
// Process & save:
operation(NULL,&sf,changes );
if (!isInMemory) (*out_fil) << *o;
}
else
{ // Unknown class:
THROW_EXCEPTION(format("Unexpected class found in the file: '%s'",newObj->GetRuntimeClass()->className) );
}
}
catch (exception &e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
// Step counter & update progress dialog
if (countLoop++ % 300 == 0)
{
auxStr.sprintf(wxT("Processing... (%u objects processed)"),countLoop);
int curProgr = isInMemory ? countLoop : (int)in_fil->getPosition();
if (!progDia.Update( curProgr , auxStr ))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
// Delete only if processing file
if (newObj && !isInMemory)
{
newObj.clear();
}
} // end while keep loading
progDia.Update( processMax ); // Close dialog.
if (strlen(endMsg))
{
char tmpStr[1000];
os::sprintf(tmpStr,sizeof(tmpStr),"%s %i\n\nEnd message:\n%s", endMsg, changes, errorMsg.c_str() );
wxMessageBox( _U(tmpStr), _("Result:"), wxOK,this);
}
if (in_fil) delete in_fil;
if (out_fil) delete out_fil;
WX_END_TRY
}