Merge branch 'IntermediateNode' of github.com:borongyuan/rtabmap into borongyuan-IntermediateNode

corelib/include/rtabmap/core/Memory.h

@@ -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

@@ -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

@@ -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

@@ -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,24 @@ 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);
+		if(!_saveIntermediateNodeData)
+		{
+			this->disableWordsRef(locationId);
+			location->removeAllWords();
+			location->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+			location->sensorData().clearCompressedData(true, false, false);
+			location->sensorData().clearRawData(true, false, false);
+		}
+	}
+}
+
 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
 	UDEBUG("Deleting location %d", locationId);
@@ -4226,8 +4246,8 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 						_rehearsalMaxDistance, _rehearsalMaxAngle);
 				return false;
 			}
-			fullMerge = !isMoving && newS->hasLink(oldS->id());
+			fullMerge = !isMoving && (newS->hasLink(oldS->id()) && !_createIntermediateNodes);
-			intermediateMerge = !isMoving && !newS->hasLink(oldS->id());
+			intermediateMerge = !isMoving && (!newS->hasLink(oldS->id()) || _createIntermediateNodes);
 		}
 		else
 		{
@@ -4317,7 +4337,14 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				// just update weight
 				int w = oldS->getWeight()>=0?oldS->getWeight():0;
 				newS->setWeight(w + newS->getWeight() + 1);
-				oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
+				if(intermediateMerge)
+				{
+					this->convertToIntermediate(oldS->id());
+				}
+				else
+				{
+					oldS->setWeight(0);
+				}
 
 				if(_lastGlobalLoopClosureId == oldS->id())
 				{
@@ -4328,7 +4355,14 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			{
 				int w = newS->getWeight()>=0?newS->getWeight():0;
 				oldS->setWeight(w + oldS->getWeight() + 1);
-				newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
+				if(intermediateMerge)
+				{
+					this->convertToIntermediate(newS->id());
+				}
+				else
+				{
+					newS->setWeight(0);
+				}
 			}
 		}
 	}

corelib/src/Rtabmap.cpp

@@ -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
+			if(!_createIntermediateNodes)
-			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(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++);
+					if(iter->second.from() == signature->id() || iter->second.to() == signature->id())
-				}
+					{
-				else
+						_odomCacheConstraints.erase(iter++);
-				{
+					}
-					++iter;
+					else
+					{
+						++iter;
+					}
 				}
 			}
+			else if(!lastSignatureWasIntermediateNode)
+			{
+				_memory->convertToIntermediate(signature->id());
+				lastSignatureWasIntermediateNode = true;
+			}
 		}
 		else
 		{

corelib/src/RtabmapThread.cpp

@@ -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;
 		}