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

Reduce graph: added option to remove orphan nodes from WM after reduction (#1735)

Browse Source 
* Reduce graph: remove orphan nodes from WM after reduction

* fixed eror

* ignore ids< 0

* add warning and cleanup only when option is used

* updated feedback

* Abort --sync_wm_and_opt_graph if a lot more WM nodes have to be transferred.

* typo

* refactored... completly

* fixed optimized graph cleared

* updated option description

* updated comment

* Added dummy dictionary function to speedup initialization when we dont need the dictionary fully loaded in memory.

* Fixed nodes weight not modified in db when deleted

* Removed AutoUpdate parameter, not needed

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

* updated log
This commit is contained in:
matlabbe
2026-07-29 08:19:13 -07:00
committed by GitHub
parent 5fad2d29c3
commit 89998284bc
12 changed files with 372 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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
+100 -14
View File
@@ -138,7 +138,8 @@ Memory::Memory(const ParametersMap & parameters) :
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_registrationVis(0)
_registrationVis(0),
_dummyDictionary(false)
{
_feature2D = Feature2D::create(parameters);
_vwd = new VWDictionary(parameters);
@@ -411,31 +412,55 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
{
if(wordIds.size())
{
std::list<VisualWord*> words;
_dbDriver->loadWords(wordIds, words);
for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
if(_dummyDictionary)
{
_vwd->addWord(*iter);
for(std::set<int>::iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter)
{
VisualWord * w = new VisualWord(*iter, cv::Mat());
w->setSaved(true);
_vwd->addWord(w); // placeholder descriptor
}
}
else
{
std::list<VisualWord*> words;
_dbDriver->loadWords(wordIds, words);
for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
{
_vwd->addWord(*iter);
}
}
// Get Last word id
int id = 0;
_dbDriver->getLastWordId(id);
_vwd->setLastWordId(id);
}
else {
_dummyDictionary = false;
}
}
else
{
_dbDriver->load(*_vwd, false);
_dbDriver->load(*_vwd, false, _dummyDictionary);
}
}
else
{
UDEBUG("load words");
// load the last dictionary
_dbDriver->load(*_vwd, _vwd->isIncremental());
_dbDriver->load(*_vwd, _vwd->isIncremental(), _dummyDictionary);
}
UDEBUG("%d words loaded! (type=%s, dim=%d)",
_vwd->getUnusedWordsSize(),
_vwd->getVisualWords().empty()?"NA":_vwd->getVisualWords().begin()->second->getDescriptor().empty()?"dummy":_vwd->getVisualWords().begin()->second->getDescriptor().type() == CV_32FC1?"float":"binary",
_vwd->getVisualWords().empty()?0:_vwd->getVisualWords().begin()->second->getDescriptor().cols);
UDEBUG("Dictionary memory usage: %ld Bytes (%ld MB)", _vwd->getMemoryUsed(), _vwd->getMemoryUsed()/(1024*1024));
if(!_dummyDictionary) {
_vwd->update();
}
else {
UDEBUG("Dictionary update skipped (dummy dictionary is enabled)");
}
UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
_vwd->update();
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
@@ -496,6 +521,24 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
UWARN("%s", msg.c_str());
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg));
if(_dummyDictionary)
{
UWARN("Dummy dictionary cannot be used when repairing the dictionary, disabling dummy dictionary.");
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
Signature * s = this->_getSignature(i->first);
UASSERT(s != 0);
if(!s->isEnabled())
{
break;
}
this->disableWordsRef(s->id());
}
_vwd->deleteUnusedWords();
_vwd->clear();
_dummyDictionary = false;
}
//remove all words ref
const std::map<int, VisualWord *> & addedWords = _vwd->getVisualWords();
@@ -595,6 +638,21 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
UDEBUG("map ids start with %d", _idMapCount);
}
void Memory::setDummyDictionary(bool enabled)
{
if(_dbDriver != 0) {
UERROR("Dummy dictionary can only be set if the memory is not yet initialized. Ignoring.");
return;
}
if(enabled) {
UINFO("Dummy dictionary enabled.");
}
else {
UINFO("Dummy dictionary disabled.");
}
_dummyDictionary = enabled;
}
void Memory::saveFlannIndex(bool postInitClosingEvents)
{
if(!_dbDriver) {
@@ -1324,6 +1382,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())
{
@@ -1340,6 +1403,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);
}
@@ -1347,6 +1415,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)
@@ -1455,10 +1535,10 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
else
{
std::multimap<int, Link> links = s->getLinks();
// Setting true to make sure we save all visual
// Setting keepLinkedInDb=true to make sure we save all visual
// words that could be referenced in a previously
// transferred node in LTM (#979)
reducedId = reduceNode(s->id(), 0, true);
reducedId = reduceNode(s->id(), 0, /*keepLinkedInDb*/ true);
if(reducedToOut) {
*reducedToOut = reducedId;
}
@@ -3132,13 +3212,18 @@ void Memory::convertToIntermediate(int locationId)
}
}
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords, bool keepLinkedInDb)
{
UDEBUG("Deleting location %d", locationId);
UDEBUG("Deleting location %d (keepLinkedInDb=%s)", locationId, keepLinkedInDb?"true":"false");
Signature * location = _getSignature(locationId);
if(location)
{
this->moveToTrash(location, false, deletedWords);
this->moveToTrash(location, keepLinkedInDb, deletedWords);
_memoryChanged = true;
}
else
{
UWARN("Location %d has not been found in STM/WM, cannot delete it.", locationId);
}
}
@@ -5092,6 +5177,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
data.depthOrRightRaw().rows,
data.depthOrRightRaw().type(),
CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str());
UASSERT_MSG(!_dummyDictionary, "Memory::createSignature() cannot be called if the memory has been initialized with a dummy dictionary.");
if(!data.depthOrRightRaw().empty() &&
data.cameraModels().empty() &&
corelib/src/Rtabmap.cpp
+18 -1
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);
}
@@ -6934,6 +6939,18 @@ void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
}
}
void Rtabmap::setDummyDictionary(bool enabled)
{
if(_memory) {
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 {
_dummyDictionary = true;
}
}
void Rtabmap::clearPath(int status)
{
UINFO("status=%d", status);
corelib/src/VWDictionary.cpp
+1 -1
View File
@@ -108,7 +108,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
incrementalDictionary = uStr2Bool((*iter).second.c_str());
}
// Verifying hypotheses strategy
// Verifying NN strategy
bool treeUpdated = false;
if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end())
{