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

Fixed dummy dictionary usage for detectMoreLoopClosures. Added checks to disable graph reduction when intermediate nodes are detected.

Browse Source
This commit is contained in:
matlabbe
2026-07-17 17:06:30 -07:00
parent bc90058612
commit 17441f9347
9 changed files with 73 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -162,7 +162,7 @@ public:
void executeNoResult(const std::string & sql) const;
// Load objects
void load(VWDictionary & dictionary, bool lastStateOnly = true) const;
void load(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const;
void loadLastNodes(std::list<Signature *> & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage
Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage
@@ -279,7 +279,7 @@ protected:
virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const = 0;
// Load objects
virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0;
virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const = 0;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
corelib/include/rtabmap/core/DBDriverSqlite3.h
+1 -1
View File
@@ -138,7 +138,7 @@ protected:
virtual void saveFlannIndexQuery(const std::vector<unsigned char> & indexData) const;
// Load objects
virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const;
virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool loadWordIdsOnly) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
corelib/include/rtabmap/core/Rtabmap.h
+2
View File
@@ -403,6 +403,8 @@ private:
int _pathStuckCount;
float _pathStuckDistance;
bool _dummyDictionary;
#ifdef RTABMAP_PYTHON
PythonInterface * _python;
#endif
corelib/include/rtabmap/core/VWDictionary.h
-3
View File
@@ -108,8 +108,6 @@ public:
void setIncrementalDictionary();
void setFixedDictionary(const std::string & dictionaryPath);
bool isModified() const;
void setAutoUpdate(bool enabled) {_autoUpdate = enabled;} // Enable/Disable internal update when parameters change, update() has to be externally triggered if disabled.
bool isAutoUpdateEnabled() const {return _autoUpdate;}
std::vector<unsigned char> serializeIndex() const;
void deserializeIndex(const std::vector<unsigned char> & data);
@@ -150,7 +148,6 @@ private:
cv::Mat _dataTree;
bool _modified;
NNStrategy _strategy;
bool _autoUpdate;
std::map<int ,int> _mapIndexId;
std::map<int ,int> _mapIdIndex;
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
corelib/src/DBDriver.cpp
+2 -2
View File
@@ -539,10 +539,10 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
_dbSafeAccessMutex.unlock();
}
void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const
void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly, bool idsOnly) const
{
_dbSafeAccessMutex.lock();
this->loadQuery(dictionary, lastStateOnly);
this->loadQuery(dictionary, lastStateOnly, idsOnly);
_dbSafeAccessMutex.unlock();
}
corelib/src/DBDriverSqlite3.cpp
+29 -22
View File
@@ -3538,7 +3538,7 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool lo
}
}
void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const
void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly, bool idsOnly) const
{
ULOGGER_DEBUG("");
if(_ppDb)
@@ -3552,7 +3552,12 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
std::list<VisualWord *> visualWords;
// Get the visual words
query << "SELECT id, descriptor_size, descriptor FROM Word ";
query << "SELECT id";
if(!idsOnly)
{
query << ", descriptor_size, descriptor";
}
query << " FROM Word ";
if(lastStateOnly)
{
if(uStrNumCmp(_version, "0.11.11") >= 0)
@@ -3581,27 +3586,29 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
int index=0;
id = sqlite3_column_int(ppStmt, index++); // VisualWord Id
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
cv::Mat d;
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
if(!idsOnly) {
descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
memcpy(d.data, descriptor, dRealSize);
if(dRealSize == descriptorSize)
{
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
}
VisualWord * vw = new VisualWord(id, d);
vw->setSaved(true);
dictionary.addWord(vw);
@@ -3621,7 +3628,7 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c
getLastWordId(id);
dictionary.setLastWordId(id);
if(uStrNumCmp(_version, "0.23.0") >= 0) {
if(!idsOnly && uStrNumCmp(_version, "0.23.0") >= 0) {
// load dictionary index
std::stringstream query3;
query3 << "SELECT dictionary_index "
corelib/src/Memory.cpp
+24 -8
View File
@@ -441,20 +441,14 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
}
else
{
_dbDriver->load(*_vwd, false);
_dummyDictionary = false;
_dbDriver->load(*_vwd, false, _dummyDictionary);
}
}
else
{
if(_dummyDictionary)
{
UWARN("A dummy dictionary is requested but %s is false, loading the dictionary as usual.", Parameters::kMemInitWMWithAllNodes().c_str());
_dummyDictionary = false;
}
UDEBUG("load words");
// load the last dictionary
_dbDriver->load(*_vwd, _vwd->isIncremental());
_dbDriver->load(*_vwd, _vwd->isIncremental(), _dummyDictionary);
}
UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
UDEBUG("Dictionary memory usage: %ld Bytes", _vwd->getMemoryUsed());
@@ -1385,6 +1379,11 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc
UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
return 0;
}
else if(s->getWeight() == -1)
{
UWARN("Cannot reduce intermediate node %d (not supported).", id);
return 0;
}
if(!s->getLabel().empty())
{
@@ -1401,6 +1400,11 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc
{
float distance = iter->second.transform().getNorm();
reducedTo = iter->second.to();
if(this->_getSignature(reducedTo) == 0)
{
UWARN("Node %d is not in WM/STM, cannot reduce %d to it.", reducedTo, id);
return 0;
}
UDEBUG("Reduce %d to %d (distance=%f)",
s->id(), iter->second.to(), distance);
}
@@ -1408,6 +1412,18 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc
if(iter->second.type() == Link::kNeighbor)
{
neighbors.insert(*iter);
// neighbors should not be intermediate nodes
Signature * sTo = this->_getSignature(iter->first);
if(sTo == 0)
{
UWARN("Neighbor node %d is not in WM/STM, cannot reduce %d.", iter->first, id);
return 0;
}
else if(sTo->getWeight() == -1)
{
UWARN("Neighbor node %d is an intermediate node (not supported), cannot reduce %d.", iter->first, id);
return 0;
}
}
}
if(reducedTo>0)
corelib/src/Rtabmap.cpp
+10 -3
View File
@@ -180,7 +180,8 @@ Rtabmap::Rtabmap() :
_pathGoalIndex(0),
_pathTransformToGoal(Transform::getIdentity()),
_pathStuckCount(0),
_pathStuckDistance(0.0f)
_pathStuckDistance(0.0f),
_dummyDictionary(false)
#ifdef RTABMAP_PYTHON
,_python(new PythonInterface())
#endif
@@ -360,6 +361,10 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
if(!_memory)
{
_memory = new Memory(allParameters);
if(_dummyDictionary)
{
_memory->setDummyDictionary(true);
}
_memory->init(_databasePath, false, allParameters, true);
}
@@ -6937,10 +6942,12 @@ void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
void Rtabmap::setDummyDictionary(bool enabled)
{
if(_memory) {
_memory->setDummyDictionary(enabled);
UERROR("Memory is already initialized, cannot set dummy dictionary. This "
"function can only be called after Rtabmap object is created, but "
"before init() is called.");
}
else {
UERROR("Memory is null, cannot set dummy dictionary.");
_dummyDictionary = true;
}
}