i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Making Rtabmap interface easier to use from c++ (Update c++ example usage)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@695 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-01-10 20:29:01 +00:00
parent 0f4e24233c
commit b032699860
8 changed files with 205 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -77,8 +77,6 @@ public:
double * dbAccessTime = 0) const;
//getters
unsigned int getWorkingMemSize() const {return _workingMem.size();}
unsigned int getStMemSize() const {return _stMem.size();};
const std::set<int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}

29
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -62,45 +62,40 @@ public:
enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
static const char * kDefaultIniFileName;
static const char * kDefaultIniFilePath;
static const char * kDefaultDatabaseName;
public:
static std::string getVersion();
static std::string getIniFilePath();
static void readParameters(const char * configFile, ParametersMap & parameters);
static void writeParameters(const char * configFile, const ParametersMap & parameters);
public:
Rtabmap();
Rtabmap(const std::string & workingDirectory = ".", bool deleteMemory = true);
virtual ~Rtabmap();
void process(const cv::Mat & image); // for convenience
void process(const Image & image); // for convenience
void dumpData();
void generateLocalGraph(const std::string & path, int id, int margin);
void init(const ParametersMap & param);
void init(const char * configFile = 0);
void clearBufferedSensors();
void close();
const std::string & getWorkingDir() const {return _wDir;}
int getLoopClosureId() const;
int getReactivatedId() const;
int getLastSignatureId() const;
int getRetrievedId() const;
int getLastLocationId() const;
float getLcHypValue() const {return _lastLcHypothesisValue;}
std::list<int> getWorkingMem() const;
std::set<int> getStMem() const;
std::list<int> getWM() const; // working memory
std::set<int> getSTM() const; // short-term memory
int getWMSize() const; // working memory size
int getSTMSize() const; // short-term memory size
std::map<int, int> getWeights() const;
int getTotalMemSize() const;
double getLastProcessTime() const {return _lastProcessTime;};
void setMaxTimeAllowed(float maxTimeAllowed); // in ms
void setDataBufferSize(int size);
void setWorkingDirectory(std::string path);
void deleteMemory();
void setTimeThreshold(float maxTimeAllowed); // in ms
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
@@ -122,7 +117,11 @@ private:
void releaseAllStrategies();
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void dumpPrediction() const;
void dumpData();
void generateLocalGraph(const std::string & path, int id, int margin);
void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path);
void setDataBufferSize(int size);
private:
// Modifiable parameters

2
corelib/src/Memory.cpp
View File

@@ -2103,7 +2103,7 @@ Signature * Memory::createSignature(int id, const Image & image, bool keepRawDat
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
int treeSize= this->getWorkingMemSize() + this->getStMemSize();
int treeSize= _workingMem.size() + _stMem.size();
int nbCommonWords = 0;
if(treeSize > 0)
{

160
corelib/src/Rtabmap.cpp
View File

@@ -54,10 +54,9 @@
namespace rtabmap
{
const char * Rtabmap::kDefaultIniFileName = "rtabmap.ini";
const char * Rtabmap::kDefaultDatabaseName = "LTM.db";
Rtabmap::Rtabmap() :
Rtabmap::Rtabmap(const std::string & workingDirectory, bool deleteMemory) :
_publishStats(Parameters::defaultRtabmapPublishStats()),
_publishImage(Parameters::defaultRtabmapPublishImage()),
_publishPdf(Parameters::defaultRtabmapPublishPdf()),
@@ -82,8 +81,11 @@ Rtabmap::Rtabmap() :
_foutFloat(0),
_foutInt(0)
{
ULOGGER_DEBUG("Working directory=%s", Parameters::defaultRtabmapWorkingDirectory().c_str());
this->setWorkingDirectory(Parameters::defaultRtabmapWorkingDirectory());
this->setWorkingDirectory(workingDirectory);
if(deleteMemory)
{
this->resetMemory(true);
}
}
Rtabmap::~Rtabmap() {
@@ -139,6 +141,9 @@ void Rtabmap::setupLogFiles(bool overwrite)
attributes = "w";
}
bool addLogFHeader = overwrite || !UFile::exists(_wDir+LOG_F);
bool addLogIHeader = overwrite || !UFile::exists(_wDir+LOG_I);
#ifdef _MSC_VER
fopen_s(&_foutFloat, (_wDir+LOG_F).toStdString().c_str(), attributes.c_str());
fopen_s(&_foutInt, (_wDir+LOG_I).toStdString().c_str(), attributes.c_str());
@@ -147,6 +152,55 @@ void Rtabmap::setupLogFiles(bool overwrite)
_foutInt = fopen((_wDir+LOG_I).c_str(), attributes.c_str());
#endif
printf("addLogFHeader=%d\n", addLogFHeader?1:0);
// add header (column identification)
if(addLogFHeader && _foutFloat)
{
fprintf(_foutFloat, "Column headers:\n");
fprintf(_foutFloat, " 1-Total iteration time (s)\n");
fprintf(_foutFloat, " 2-Memory update time (s)\n");
fprintf(_foutFloat, " 3-Retrieval time (s)\n");
fprintf(_foutFloat, " 4-Likelihood time (s)\n");
fprintf(_foutFloat, " 5-Posterior time (s)\n");
fprintf(_foutFloat, " 6-Hypothesis selection time (s)\n");
fprintf(_foutFloat, " 7-Transfer time (s)\n");
fprintf(_foutFloat, " 8-Statistics creation time (s)\n");
fprintf(_foutFloat, " 9-Loop closure hypothesis value\n");
fprintf(_foutFloat, " 10-NAN\n");
fprintf(_foutFloat, " 11-Maximum likelihood\n");
fprintf(_foutFloat, " 12-Sum likelihood\n");
fprintf(_foutFloat, " 13-Mean likelihood\n");
fprintf(_foutFloat, " 14-Std dev likelihood\n");
fprintf(_foutFloat, " 15-Virtual place hypothesis\n");
fprintf(_foutFloat, " 16-Join trash time (s)\n");
fprintf(_foutFloat, " 17-Weight Update (rehearsal) similarity\n");
fprintf(_foutFloat, " 18-Empty trash time (s)\n");
fprintf(_foutFloat, " 19-Retrieval database access time (s)\n");
fprintf(_foutFloat, " 20-Add loop closure link time (s)\n");
}
if(addLogIHeader && _foutInt)
{
fprintf(_foutInt, "Column headers:\n");
fprintf(_foutInt, " 1-Loop closure ID\n");
fprintf(_foutInt, " 2-Highest loop closure hypothesis\n");
fprintf(_foutInt, " 3-Locations transferred\n");
fprintf(_foutInt, " 4-NAN\n");
fprintf(_foutInt, " 5-Words extracted from the last image\n");
fprintf(_foutInt, " 6-Vocabulary size\n");
fprintf(_foutInt, " 7-Working memory size\n");
fprintf(_foutInt, " 8-Is loop closure hypothesis rejected?\n");
fprintf(_foutInt, " 9-NAN\n");
fprintf(_foutInt, " 10-NAN\n");
fprintf(_foutInt, " 11-Locations retrieved\n");
fprintf(_foutInt, " 12-Retrieval location ID\n");
fprintf(_foutInt, " 13-Unique words extraced from last image\n");
fprintf(_foutInt, " 14-Retrieval ID\n");
fprintf(_foutInt, " 15-Non-null likelihood values\n");
fprintf(_foutInt, " 16-Weight Update ID\n");
fprintf(_foutInt, " 17-Is last location merged through Weight Update?\n");
}
ULOGGER_DEBUG("Log file (int)=%s", (_wDir+LOG_I).c_str());
ULOGGER_DEBUG("Log file (float)=%s", (_wDir+LOG_F).c_str());
}
@@ -220,32 +274,42 @@ void Rtabmap::init(const ParametersMap & parameters)
setupLogFiles();
}
std::string Rtabmap::getIniFilePath()
{
std::string privatePath = UDirectory::homeDir() + "/.rtabmap";
if(!UDirectory::exists(privatePath))
{
UDirectory::makeDir(privatePath);
}
return (privatePath + "/") + kDefaultIniFileName;
}
void Rtabmap::init(const char * configFile)
{
std::string config = this->getIniFilePath();
if(configFile)
{
config = configFile;
}
ULOGGER_DEBUG("file path = %s", config.c_str());
// fill ctrl struct with values from the configuration file
ParametersMap param;// = Parameters::defaultParameters;
this->readParameters(config.c_str(), param);
if(configFile)
{
ULOGGER_DEBUG("Read parameters from = %s", configFile);
this->readParameters(configFile, param);
}
this->init(param);
}
void Rtabmap::close()
{
UEventsManager::removeHandler(this);
// Stop the thread first
join(true);
flushStatisticLogs();
if(_foutFloat)
{
fclose(_foutFloat);
_foutFloat = 0;
}
if(_foutInt)
{
fclose(_foutInt);
_foutInt = 0;
}
this->releaseAllStrategies();
}
void Rtabmap::parseParameters(const ParametersMap & parameters)
{
ULOGGER_DEBUG("");
@@ -360,12 +424,12 @@ int Rtabmap::getLoopClosureId() const
return _lcHypothesisId;
}
int Rtabmap::getReactivatedId() const
int Rtabmap::getRetrievedId() const
{
return _retrievedId;
}
int Rtabmap::getLastSignatureId() const
int Rtabmap::getLastLocationId() const
{
int id = 0;
if(_memory && _memory->getLastSignature())
@@ -375,9 +439,8 @@ int Rtabmap::getLastSignatureId() const
return id;
}
std::list<int> Rtabmap::getWorkingMem() const
std::list<int> Rtabmap::getWM() const
{
ULOGGER_DEBUG("");
std::list<int> mem;
if(_memory)
{
@@ -387,9 +450,17 @@ std::list<int> Rtabmap::getWorkingMem() const
return mem;
}
int Rtabmap::getWMSize() const
{
if(_memory)
{
return _memory->getWorkingMem().size()-1; // remove virtual place
}
return 0;
}
std::map<int, int> Rtabmap::getWeights() const
{
ULOGGER_DEBUG("");
std::map<int, int> weights;
if(_memory)
{
@@ -399,9 +470,8 @@ std::map<int, int> Rtabmap::getWeights() const
return weights;
}
std::set<int> Rtabmap::getStMem() const
std::set<int> Rtabmap::getSTM() const
{
ULOGGER_DEBUG("");
std::set<int> mem;
if(_memory)
{
@@ -410,6 +480,15 @@ std::set<int> Rtabmap::getStMem() const
return mem;
}
int Rtabmap::getSTMSize() const
{
if(_memory)
{
return _memory->getStMem().size();
}
return 0;
}
int Rtabmap::getTotalMemSize() const
{
ULOGGER_DEBUG("");
@@ -518,15 +597,6 @@ void Rtabmap::mainLoop()
}
}
void Rtabmap::deleteMemory()
{
// Only if rtabmap's thread is not started...
if(!this->isRunning())
{
this->resetMemory(true);
}
}
void Rtabmap::resetMemory(bool dbOverwritten)
{
if(_memory)
@@ -726,7 +796,7 @@ void Rtabmap::process()
// If the working memory is empty, don't do the detection. It happens when it
// is the first time the detector is started (there needs some images to
// fill the short-time memory before a signature is added to the working memory).
if(_memory->getWorkingMemSize())
if(_memory->getWorkingMem().size())
{
//============================================================
// Likelihood computation
@@ -1058,8 +1128,8 @@ void Rtabmap::process()
stat->addStatistic(Statistics::kLoopReactivateId(), _retrievedId);
stat->addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
stat->addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMemSize());
stat->addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMemSize());
stat->addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
stat->addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
stat->addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
stat->addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
@@ -1151,9 +1221,9 @@ void Rtabmap::process()
ULOGGER_INFO("Total time processing = %fs...", totalTime);
timer.start();
if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
(_maxMemoryAllowed != 0 && _memory->getWorkingMemSize() > _maxMemoryAllowed))
(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
{
ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMemSize(), _maxMemoryAllowed);
ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed);
signaturesRemoved = _memory->forget(immunizedLocations);
}
_lastProcessTime = totalTime;
@@ -1243,7 +1313,7 @@ void Rtabmap::process()
0,
refWordsCount,
dictionarySize,
int(_memory->getWorkingMemSize()),
int(_memory->getWorkingMem().size()),
rejectedHypothesis?1:0,
0,
0,
@@ -1322,7 +1392,7 @@ void Rtabmap::getImage(Image & image)
}
// SETTERS
void Rtabmap::setMaxTimeAllowed(float maxTimeAllowed)
void Rtabmap::setTimeThreshold(float maxTimeAllowed)
{
//must be positive, 0 mean inf time allowed (no time limit)
_maxTimeAllowed = maxTimeAllowed;