// ------------------------------------------------------
// TestMapping
// ------------------------------------------------------
void Run_HMT_SLAM()
{
CHMTSLAM mapping;
CConfigFile cfgFile( configFile );
std::string rawlogFileName;
// wait for threads init. (Just to do not mix debug strings on console)
sleep(100);
// The rawlog file:
// ----------------------------------------
rawlogFileName = cfgFile.read_string("HMT-SLAM","rawlog_file",std::string("log.rawlog"));
unsigned int rawlog_offset = cfgFile.read_int("HMT-SLAM","rawlog_offset",0);
mapping.printf_debug("RAWLOG FILE: \n%s\n",rawlogFileName.c_str());
const std::string OUT_DIR = cfgFile.read_string("HMT-SLAM","LOG_OUTPUT_DIR", "HMT_SLAM_OUTPUT");
ASSERT_FILE_EXISTS_( rawlogFileName )
CFileGZInputStream rawlogFile( rawlogFileName);
mapping.printf_debug("---------------------------------------------------\n\n");
// Set relative path for externally-stored images in rawlogs:
string rawlog_images_path = extractFileDirectory( rawlogFileName );
rawlog_images_path+="/Images";
CImage::IMAGES_PATH_BASE = rawlog_images_path; // Set it.
// Load the config options for mapping:
// ----------------------------------------
mapping.loadOptions( configFile );
// INITIALIZATION
// ----------------------------------------
//utils::CRandomGenerator::Randomize( ); // Not necesary (called inside the classes)
mapping.initializeEmptyMap();
// The main loop:
// ---------------------------------------
unsigned int rawlogEntry = 0, step = 0;
bool finish=false;
while (!finish && !mapping.abortedDueToErrors() )
{
if (os::kbhit())
{
char pushKey = os::getch();
finish = 27 == pushKey;
}
// Load next object from the rawlog:
// ----------------------------------------
CSerializablePtr objFromRawlog;
try
{
rawlogFile >> objFromRawlog;
rawlogEntry++;
}
catch(std::exception &) { break; }
catch(...) { printf("Untyped exception reading rawlog file!!\n");break;}
if (rawlogEntry>=rawlog_offset)
{
// Process the action and observations:
// --------------------------------------------
if (IS_CLASS(objFromRawlog,CActionCollection))
{
mapping.pushAction( CActionCollectionPtr( objFromRawlog) ); // Memory will be freed in mapping class
}
else if (IS_CLASS(objFromRawlog,CSensoryFrame))
{
mapping.pushObservations( CSensoryFramePtr( objFromRawlog) ); // Memory will be freed in mapping class
}
else if (IS_CLASS(objFromRawlog,CObservation))
{
mapping.pushObservation( CObservationPtr( objFromRawlog) ); // Memory will be freed in mapping class
}
else THROW_EXCEPTION("Invalid object class from rawlog!!")
// Wait for the mapping framework processed the data
// ---------------------------------------------------
if ((rawlogEntry % STEPS_BETWEEN_WAITING_FOR_QUEUE_EMPTY)==0)
while (!mapping.isInputQueueEmpty() && !os::kbhit() && !mapping.abortedDueToErrors() )
{
sleep(2);
}
} // (rawlogEntry>=rawlog_offset)
mapping.printf_debug("======== Rawlog entries processed: %i ========\n", rawlogEntry);
step++;
}; // end "while(1)"
mapping.printf_debug("********* Application finished!! 3 seconds to exit... **********\n");
sleep(1000);
mapping.printf_debug("********* Application finished!! 2 seconds to exit... **********\n");
sleep(1000);
mapping.printf_debug("********* Application finished!! 1 second to exit... **********\n");
sleep(1000);
{
string final_file = OUT_DIR+string("/final_map.hmtslam");
mapping.printf_debug("\n Saving FINAL HMT-MAP to file: %s\n",final_file.c_str());
CFileGZOutputStream fil(final_file);
mapping.saveState( fil );
}
}
/*---------------------------------------------------------------
CHMTSLAM_TBI
Topological Bayesian Inference (TBI) process within HMT-SLAM
---------------------------------------------------------------*/
void CHMTSLAM::thread_TBI()
{
CHMTSLAM *obj = this;
CTicTac tictac;
// Seems that must be called in each thread??
if (obj->m_options.random_seed)
randomGenerator.randomize( obj->m_options.random_seed );
else randomGenerator.randomize();
try
{
// Start thread:
// -------------------------
obj->printf_debug("[thread_TBI] Thread started (ID=0x%08lX)\n", mrpt::system::getCurrentThreadId() );
// --------------------------------------------
// The main loop
// Executes until termination is signaled
// --------------------------------------------
while ( !obj->m_terminateThreads )
{
mrpt::system::sleep(100);
}; // end while execute thread
// Finish thread:
// -------------------------
time_t timCreat,timExit; double timCPU=0;
try { mrpt::system::getCurrentThreadTimes( timCreat,timExit,timCPU); } catch(...) {};
obj->printf_debug("[thread_TBI] Thread finished. CPU time used:%.06f secs \n",timCPU);
obj->m_terminationFlag_TBI = true;
}
catch(std::exception &e)
{
obj->m_terminationFlag_TBI = true;
// Release semaphores:
obj->printf_debug( (char*)e.what() );
// DEBUG: Terminate application:
obj->m_terminateThreads = true;
}
catch(...)
{
obj->m_terminationFlag_TBI = true;
obj->printf_debug("\n---------------------- EXCEPTION CAUGHT! ---------------------\n");
obj->printf_debug(" In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime error!!\n");
// Release semaphores:
// DEBUG: Terminate application:
obj->m_terminateThreads = true;
}
}
/*---------------------------------------------------------------
areaAbstraction
Area Abstraction (AA) process within HMT-SLAM
---------------------------------------------------------------*/
CHMTSLAM::TMessageLSLAMfromAAPtr CHMTSLAM::areaAbstraction(
CLocalMetricHypothesis *LMH,
const TPoseIDList &newPoseIDs )
{
MRPT_START
ASSERT_( !newPoseIDs.empty() );
ASSERT_(LMH);
CHMTSLAM *obj = LMH->m_parent.get();
ASSERT_(obj);
// The output results:
TMessageLSLAMfromAAPtr resMsg = TMessageLSLAMfromAAPtr(new TMessageLSLAMfromAA());
// Process msg:
THypothesisID LMH_ID = LMH->m_ID;
resMsg->hypothesisID = LMH_ID;
for (TPoseIDList::const_iterator newID=newPoseIDs.begin();newID!=newPoseIDs.end();++newID)
{
// Add a new node to the graph:
obj->printf_debug("[thread_AA] Processing new pose ID: %u\n", static_cast<unsigned>( *newID ) );
// Get SF & pose pdf for the new pose.
const CSensoryFrame *sf;
CPose3DPDFParticlesPtr posePDF = CPose3DPDFParticles::Create();
{
// CCriticalSectionLocker lock( & LMH->m_lock ); // We are already within the LMH's lock!
// SF:
std::map<TPoseID,CSensoryFrame>::const_iterator itSFs = LMH->m_SFs.find( *newID );
ASSERT_(itSFs != LMH->m_SFs.end() );
sf = &itSFs->second;
// Pose PDF:
LMH->getPoseParticles( *newID, *posePDF );
} // end of LMH's critical section lock
{
synch::CCriticalSectionLocker locker ( &LMH->m_robotPosesGraph.lock );
// Add to the graph partitioner:
LMH->m_robotPosesGraph.partitioner.options = obj->m_options.AA_options;
unsigned int newIdx = LMH->m_robotPosesGraph.partitioner.addMapFrame(
CSensoryFramePtr(new CSensoryFrame(*sf)),
posePDF );
LMH->m_robotPosesGraph.idx2pose[ newIdx ] = *newID;
} // end of critical section lock on "m_robotPosesGraph.lock"
} // end for each new ID
vector< vector_uint > partitions;
{
synch::CCriticalSectionLocker locker ( &LMH->m_robotPosesGraph.lock );
// Recompute partitions:
LMH->m_robotPosesGraph.partitioner.markAllNodesForReconsideration(); // We will have always small local maps.
LMH->m_robotPosesGraph.partitioner.updatePartitions( partitions );
}
// Send result to LSLAM
resMsg->partitions.resize( partitions.size() );
vector< TPoseIDList >::iterator itDest;
vector< vector_uint >::const_iterator itSrc;
for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin(); itSrc!=partitions.end(); itSrc++, itDest++)
{
itDest->resize( itSrc->size() );
vector_uint::const_iterator it1;
TPoseIDList::iterator it2;
for (it1=itSrc->begin(), it2=itDest->begin(); it1!=itSrc->end(); it1++,it2++)
*it2 = LMH->m_robotPosesGraph.idx2pose[ *it1 ];
}
resMsg->dumpToConsole( );
return resMsg;
MRPT_END
}