Merged #1697 and added minor fixes

corelib/src/Memory.cpp

@@ -2413,7 +2413,9 @@ int Memory::cleanup()
 	int signatureRemoved = 0;
 
 	// bad signature
-	if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
+	if(_lastSignature && 
+		((_lastSignature->isBadSignature() && _badSignaturesIgnored && _lastSignature->getWeight()!=-1) || 
+		 !_incrementalMemory))
 	{
 		if(_lastSignature->isBadSignature())
 		{
@@ -2744,7 +2746,11 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		}
 
 		// it is a bad signature (not saved), remove links!
-		if(keepLinkedToGraph && (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
+		if(keepLinkedToGraph && 
+		   !s->isSaved() && 
+		   s->isBadSignature() &&
+		   _badSignaturesIgnored && 
+		   s->getWeight()!=-1)
 		{
 			keepLinkedToGraph = false;
 		}
@@ -3042,6 +3048,29 @@ 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);
+		location->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+		this->disableWordsRef(locationId); // won't be used for loop closure detection anymore
+		if(!_saveIntermediateNodeData)
+		{
+			location->removeAllWords();
+			location->sensorData().clearGlobalDescriptors();
+		}
+
+		location->sensorData().clearRawData();
+		if(!_saveIntermediateNodeData || !this->isBinDataKept())
+		{
+			location->sensorData().clearCompressedData();
+		}
+	}
+}
+
 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
 	UDEBUG("Deleting location %d", locationId);
@@ -4349,13 +4378,13 @@ 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)
 				{
-					// We can safely remove the visual words from that signature
-					// because it will be ignored in likelihood and bayes filter (as now
-					// as an intermediate node).
-					this->disableWordsRef(newS->id());
+					this->convertToIntermediate(newS->id());
+				}
+				else
+				{
+					newS->setWeight(0);
 				}
 			}
 		}

corelib/src/Rtabmap.cpp

@@ -1518,31 +1518,33 @@ bool Rtabmap::process(
 		}
 		else
 		{
+			bool linkedToIntermediateNode = false;
+			Transform t;
+			if(_memory->isIncremental())
+			{
+				const std::multimap<int, Link> & links = signature->getLinks();
+				if(links.size() && links.begin()->second.type() == Link::kNeighbor)
+				{
+					const Signature * s = _memory->getSignature(links.begin()->second.to());
+					UASSERT(s!=0);
+					// Check small motion if current node is not an intermediate node already
+					if(signature->getWeight() >= 0)
+					{
+						t = links.begin()->second.transform();
+						linkedToIntermediateNode = s->getWeight() == -1;
+					}
+				}
+			}
+			else if(!_odomCachePoses.empty())
+			{
+				t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
+			}
+			
 			if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f)
 			{
 				//============================================================
 				// Minimum displacement required to add to Memory
 				//============================================================
-				Transform t;
-
-				if(_memory->isIncremental())
-				{
-					const std::multimap<int, Link> & links = signature->getLinks();
-					if(links.size() && links.begin()->second.type() == Link::kNeighbor)
-					{
-						const Signature * s = _memory->getSignature(links.begin()->second.to());
-						UASSERT(s!=0);
-						// Check small motion if previous node is not an intermediate node (consecutive intermediate nodes are merged later)
-						if(s->getWeight() >= 0 && signature->getWeight() >= 0)
-						{
-							t = links.begin()->second.transform();
-						}
-					}
-				}
-				else if(!_odomCachePoses.empty())
-				{
-					t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
-				}
 				if(!t.isNull())
 				{
 					float x,y,z, roll,pitch,yaw;
@@ -1564,7 +1566,7 @@ bool Rtabmap::process(
 					}
 				}
 			}
-			if(odomVelocity.size() == 6)
+			if(odomVelocity.size() == 6 && signature->getWeight() != -1)
 			{
 				// This will disable global loop closure detection, only retrieval will be done.
 				// The location will also be deleted at the end.
@@ -1572,6 +1574,13 @@ bool Rtabmap::process(
 						(_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) ||
 						(_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate);
 			}
+			if(linkedToIntermediateNode && (smallDisplacement || tooFastMovement))
+			{
+				_memory->convertToIntermediate(signature->id());
+				// Intermediate nodes should stay in the graph to properly propagate loop closure hypotheses
+				smallDisplacement = false;
+				tooFastMovement = false;
+			}
 		}
 
 		// Update optimizedPoses with the newly added node

corelib/src/SensorData.cpp

@@ -976,7 +976,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
 }
 
-void SensorData::clearCompressedData(bool images, bool scan, bool userData)
+void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
@@ -992,14 +992,24 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_userDataCompressed=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsCompressed=cv::Mat();
+		_emptyCellsCompressed=cv::Mat();
+		_obstacleCellsCompressed=cv::Mat();
+	}
 }
-void SensorData::clearRawData(bool images, bool scan, bool userData)
+void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
 		_imageRaw=cv::Mat();
 		_depthOrRightRaw=cv::Mat();
 		_depthConfidenceRaw=cv::Mat();
+#ifdef HAVE_OPENCV_CUDEV
+		_imageRawGpu = cv::cuda::GpuMat();
+		_depthOrRightRawGpu = cv::cuda::GpuMat();
+#endif
 	}
 	if(scan)
 	{
@@ -1009,6 +1019,12 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
 	{
 		_userDataRaw=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsRaw=cv::Mat();
+		_emptyCellsRaw=cv::Mat();
+		_obstacleCellsRaw=cv::Mat();
+	}
 }