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

Avoid removing nodes from STM when using CreateIntermediateNodes

Browse Source
This commit is contained in:
Borong Yuan
2026-05-06 12:16:24 +08:00
parent f04067fa16
commit 58792f7b0f
6 changed files with 43 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Memory.h
+2
View File
@@ -140,6 +140,7 @@ public:
float radius,
const std::map<int, Transform> & optimizedPoses,
int maxGraphDepth) const;
void convertToIntermediate(int locationId);
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void saveLocationData(int locationId);
void removeLink(int idA, int idB);
@@ -338,6 +339,7 @@ private:
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
bool _rehearsalWeightIgnoredWhileMoving;
bool _createIntermediateNodes;
bool _useOdometryFeatures;
bool _useOdometryGravity;
bool _rotateImagesUpsideUp;
corelib/include/rtabmap/core/Rtabmap.h
+2
View File
@@ -125,6 +125,7 @@ public:
void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
const std::string & getWorkingDir() const {return _wDir;}
bool isCreateIntermediateNodes() const { return _createIntermediateNodes; }
bool isRGBDMode() const { return _rgbdSlamMode; }
int getLoopClosureId() const {return _loopClosureHypothesis.first;}
float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
@@ -282,6 +283,7 @@ private:
private:
// Modifiable parameters
bool _createIntermediateNodes;
bool _publishStats;
bool _publishLastSignatureData;
bool _publishPdf;
corelib/include/rtabmap/core/RtabmapThread.h
-3
View File
@@ -66,11 +66,9 @@ public:
void clearBufferedData();
void setDetectorRate(float rate);
void setDataBufferSize(unsigned int bufferSize);
void createIntermediateNodes(bool enabled);
float getDetectorRate() const {return _rate;}
unsigned int getDataBufferSize() const {return _dataBufferMaxSize;}
bool getCreateIntermediateNodes() const {return _createIntermediateNodes;}
/**
* Close rtabmap. This will delete rtabmap object if set.
@@ -105,7 +103,6 @@ private:
USemaphore _dataAdded;
unsigned int _dataBufferMaxSize;
float _rate;
bool _createIntermediateNodes;
double _previousStamp;
Rtabmap * _rtabmap;
corelib/src/Memory.cpp
+15 -3
View File
@@ -117,6 +117,7 @@ Memory::Memory(const ParametersMap & parameters) :
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visSSC(Parameters::defaultVisSSC()),
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
@@ -757,6 +758,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kVisSSC(), _visSSC);
Parameters::parse(params, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
@@ -2413,7 +2415,7 @@ int Memory::cleanup()
int signatureRemoved = 0;
// bad signature
if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !(_incrementalMemory || _createIntermediateNodes)))
{
if(_lastSignature->isBadSignature())
{
@@ -3042,6 +3044,16 @@ bool Memory::setUserData(int id, const cv::Mat & data)
return false;
}
void Memory::convertToIntermediate(int locationId)
{
UDEBUG("Converting location %d to intermediate node", locationId);
Signature * location = _getSignature(locationId);
if(location)
{
location->setWeight(-1);
}
}
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
{
UDEBUG("Deleting location %d", locationId);
@@ -4226,8 +4238,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
_rehearsalMaxDistance, _rehearsalMaxAngle);
return false;
}
fullMerge = !isMoving && newS->hasLink(oldS->id());
intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
fullMerge = !isMoving && (newS->hasLink(oldS->id()) && !_createIntermediateNodes);
intermediateMerge = !isMoving && (!newS->hasLink(oldS->id()) || _createIntermediateNodes);
}
else
{
corelib/src/Rtabmap.cpp
+23 -13
View File
@@ -92,6 +92,7 @@ namespace rtabmap
{
Rtabmap::Rtabmap() :
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_publishStats(Parameters::defaultRtabmapPublishStats()),
_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
_publishPdf(Parameters::defaultRtabmapPublishPdf()),
@@ -566,6 +567,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
this->setWorkingDirectory(iter->second.c_str());
}
Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
@@ -4455,23 +4457,31 @@ bool Rtabmap::process(
// Don't delete the location if a loop closure is detected
UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
// If there is a too small displacement, remove the node
signaturesRemoved.push_back(signature->id());
_memory->deleteLocation(signature->id());
// Update odom cache (if we just switched from mapping mode to localization mode)
_odomCachePoses.erase(signature->id());
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
if(!_createIntermediateNodes)
{
if(iter->second.from() == signature->id() || iter->second.to() == signature->id())
// If there is a too small displacement, remove the node
signaturesRemoved.push_back(signature->id());
_memory->deleteLocation(signature->id());
// Update odom cache (if we just switched from mapping mode to localization mode)
_odomCachePoses.erase(signature->id());
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
{
_odomCacheConstraints.erase(iter++);
}
else
{
++iter;
if(iter->second.from() == signature->id() || iter->second.to() == signature->id())
{
_odomCacheConstraints.erase(iter++);
}
else
{
++iter;
}
}
}
else if(!lastSignatureWasIntermediateNode)
{
_memory->convertToIntermediate(signature->id());
lastSignatureWasIntermediateNode = true;
}
}
else
{
corelib/src/RtabmapThread.cpp
+1 -9
View File
@@ -46,7 +46,6 @@ namespace rtabmap {
RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_rate(Parameters::defaultRtabmapDetectionRate()),
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_previousStamp(-1.0),
_rtabmap(rtabmap),
_paused(false),
@@ -107,11 +106,6 @@ void RtabmapThread::setDataBufferSize(unsigned int size)
_dataBufferMaxSize = size;
}
void RtabmapThread::createIntermediateNodes(bool enabled)
{
_createIntermediateNodes = enabled;
}
void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabasePath)
{
this->join(true);
@@ -207,7 +201,6 @@ void RtabmapThread::mainLoop()
ULOGGER_DEBUG("CMD_INIT");
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
UASSERT(_rate >= 0.0f);
_rtabmap->init(cmdEvent.getParameters(), cmdEvent.value1().toStr());
}
@@ -226,7 +219,6 @@ void RtabmapThread::mainLoop()
ULOGGER_DEBUG("CMD_UPDATE_PARAMS");
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapDetectionRate(), _rate);
Parameters::parse(cmdEvent.getParameters(), Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
UASSERT(_rate >= 0.0f);
_rtabmap->parseParameters(cmdEvent.getParameters());
break;
@@ -532,7 +524,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
}
if(ignoreFrame && !_createIntermediateNodes)
if(ignoreFrame && !_rtabmap->isCreateIntermediateNodes())
{
return;
}