Added Rtabmap::getSignatureCopy()

corelib/include/rtabmap/core/Memory.h

@@ -197,15 +197,15 @@ public:
 			EnvSensors & sensors,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
-	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
-	void getNodeWords(int nodeId,
+	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	void getNodeWordsAndGlobalDescriptors(int nodeId,
 			std::multimap<int, cv::KeyPoint> & words,
 			std::multimap<int, cv::Point3f> & words3,
-			std::multimap<int, cv::Mat> & wordsDescriptors);
+			std::multimap<int, cv::Mat> & wordsDescriptors,
+			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
-			StereoCameraModel & stereoModel);
-	SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+			StereoCameraModel & stereoModel) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}

corelib/include/rtabmap/core/Rtabmap.h

@@ -156,6 +156,7 @@ public:
 	void rejectLastLoopClosure();
 	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const;
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,

corelib/src/DBDriver.cpp

@@ -683,11 +683,11 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->sensorData().imageCompressed().empty() ||
-			!s->sensorData().laserScanCompressed().isEmpty() ||
-			!s->sensorData().userDataCompressed().empty() ||
-			s->sensorData().gridCellSize() != 0.0f ||
-			!s->isSaved())
+		if((!s->isSaved() ||
+			((!images || !s->sensorData().imageCompressed().empty()) &&
+			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
+			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 		{
 			data = (SensorData)s->sensorData();
 			found = true;

corelib/src/Memory.cpp

@@ -2698,13 +2698,13 @@ Transform Memory::computeTransform(
 	   (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
 	{
-		fromS.sensorData() = getNodeData(fromS.id());
+		fromS.sensorData() = getNodeData(fromS.id(), true, true, true, true);
 	}
 	if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
 	   (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().isEmpty()) ||
 	   (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
 	{
-		toS.sensorData() = getNodeData(toS.id());
+		toS.sensorData() = getNodeData(toS.id(), true, true, true, true);
 	}
 	// uncompress only what we need
 	cv::Mat imgBuf, depthBuf, userBuf;
@@ -3815,33 +3815,33 @@ cv::Mat Memory::getImageCompressed(int signatureId) const
 	return image;
 }
 
-SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const
+SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
-	//UDEBUG("nodeId=%d", nodeId);
+	//UDEBUG("");
 	SensorData r;
-	Signature * s = this->_getSignature(nodeId);
-	if(s && !s->sensorData().imageCompressed().empty())
+	const Signature * s = this->getSignature(locationId);
+	if(s && (!s->isSaved() ||
+			((!images || !s->sensorData().imageCompressed().empty()) &&
+			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
+			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
+			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
 	{
 		r = s->sensorData();
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		_dbDriver->getNodeData(nodeId, r);
-	}
-
-	if(uncompressedData)
-	{
-		r.uncompressData();
+		_dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
 	}
 
 	return r;
 }
 
-void Memory::getNodeWords(int nodeId,
+void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 		std::multimap<int, cv::KeyPoint> & words,
 		std::multimap<int, cv::Point3f> & words3,
-		std::multimap<int, cv::Mat> & wordsDescriptors)
+		std::multimap<int, cv::Mat> & wordsDescriptors,
+		std::vector<GlobalDescriptor> & globalDescriptors) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
@@ -3850,6 +3850,7 @@ void Memory::getNodeWords(int nodeId,
 		words = s->getWords();
 		words3 = s->getWords3();
 		wordsDescriptors = s->getWordsDescriptors();
+		globalDescriptors = s->sensorData().globalDescriptors();
 	}
 	else if(_dbDriver)
 	{
@@ -3864,6 +3865,7 @@ void Memory::getNodeWords(int nodeId,
 			words = signatures.front()->getWords();
 			words3 = signatures.front()->getWords3();
 			wordsDescriptors = signatures.front()->getWordsDescriptors();
+			globalDescriptors = signatures.front()->sensorData().globalDescriptors();
 			if(loadedFromTrash.size())
 			{
 				//put back
@@ -3879,7 +3881,7 @@ void Memory::getNodeWords(int nodeId,
 
 void Memory::getNodeCalibration(int nodeId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel)
+		StereoCameraModel & stereoModel) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
@@ -3895,28 +3897,6 @@ void Memory::getNodeCalibration(int nodeId,
 	}
 }
 
-SensorData Memory::getSignatureDataConst(int locationId,
-		bool images, bool scan, bool userData, bool occupancyGrid) const
-{
-	//UDEBUG("");
-	SensorData r;
-	const Signature * s = this->getSignature(locationId);
-	if(s && (!s->sensorData().imageCompressed().empty() ||
-			!s->sensorData().laserScanCompressed().isEmpty() ||
-			!s->sensorData().userDataCompressed().empty() ||
-			s->sensorData().gridCellSize() != 0.0f))
-	{
-		r = s->sensorData();
-	}
-	else if(_dbDriver)
-	{
-		// load from database
-		_dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
-	}
-
-	return r;
-}
-
 void Memory::generateGraph(const std::string & fileName, const std::set<int> & ids)
 {
 	if(!_dbDriver)

corelib/src/Rtabmap.cpp

@@ -4268,6 +4268,62 @@ void Rtabmap::dumpPrediction() const
 	}
 }
 
+Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid) const
+{
+	Signature s;
+	if(_memory)
+	{
+		Transform odomPoseLocal;
+		int weight = -1;
+		int mapId = -1;
+		std::string label;
+		double stamp = 0;
+		Transform groundTruth;
+		std::vector<float> velocity;
+		GPS gps;
+		EnvSensors sensors;
+		_memory->getNodeInfo(id, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true);
+		SensorData data;
+		data.setId(id);
+		if(images || scan || userData || occupancyGrid)
+		{
+			data = _memory->getNodeData(id, images, scan, userData, occupancyGrid);
+		}
+		if(!images)
+		{
+			std::vector<CameraModel> models;
+			StereoCameraModel stereoModel;
+			_memory->getNodeCalibration(id, models, stereoModel);
+			data.setCameraModels(models);
+			data.setStereoCameraModel(stereoModel);
+		}
+		std::multimap<int, cv::KeyPoint> words;
+		std::multimap<int, cv::Point3f> words3;
+		std::multimap<int, cv::Mat> wordsDescriptors;
+		std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
+		_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
+		s=Signature(id,
+					mapId,
+					weight,
+					stamp,
+					label,
+					odomPoseLocal,
+					groundTruth,
+					data);
+		s.setWords(words);
+		s.setWords3(words3);
+		s.setWordsDescriptors(wordsDescriptors);
+		s.sensorData().setGlobalDescriptors(globalDescriptors);
+		if(velocity.size()==6)
+		{
+			s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
+		}
+		s.sensorData().setGPS(gps);
+		s.sensorData().setEnvSensors(sensors);
+	}
+	return s;
+}
+
 void Rtabmap::get3DMap(
 		std::map<int, Signature> & signatures,
 		std::map<int, Transform> & poses,
@@ -4313,40 +4369,7 @@ void Rtabmap::get3DMap(
 
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			Transform odomPoseLocal;
-			int weight = -1;
-			int mapId = -1;
-			std::string label;
-			double stamp = 0;
-			Transform groundTruth;
-			std::vector<float> velocity;
-			GPS gps;
-			EnvSensors sensors;
-			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, true);
-			SensorData data = _memory->getNodeData(*iter);
-			data.setId(*iter);
-			std::multimap<int, cv::KeyPoint> words;
-			std::multimap<int, cv::Point3f> words3;
-			std::multimap<int, cv::Mat> wordsDescriptors;
-			_memory->getNodeWords(*iter, words, words3, wordsDescriptors);
-			signatures.insert(std::make_pair(*iter,
-					Signature(*iter,
-							mapId,
-							weight,
-							stamp,
-							label,
-							odomPoseLocal,
-							groundTruth,
-							data)));
-			signatures.at(*iter).setWords(words);
-			signatures.at(*iter).setWords3(words3);
-			signatures.at(*iter).setWordsDescriptors(wordsDescriptors);
-			if(velocity.size()==6)
-			{
-				signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
-			}
-			signatures.at(*iter).sensorData().setGPS(gps);
-			signatures.at(*iter).sensorData().setEnvSensors(sensors);
+			signatures.insert(std::make_pair(*iter, getSignatureCopy(*iter, true, true, true, true)));
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -4393,49 +4416,11 @@ void Rtabmap::getGraph(
 		{
 			for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
 			{
-				Transform odomPoseLocal;
-				int weight = -1;
-				int mapId = -1;
-				std::string label;
-				double stamp = 0;
-				Transform groundTruth;
-				std::vector<float> velocity;
-				GPS gps;
-				EnvSensors sensors;
-				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, global);
-				signatures->insert(std::make_pair(iter->first,
-						Signature(iter->first,
-							mapId,
-							weight,
-							stamp,
-							label,
-							odomPoseLocal,
-							groundTruth)));
-
-				std::multimap<int, cv::KeyPoint> words;
-				std::multimap<int, cv::Point3f> words3;
-				std::multimap<int, cv::Mat> wordsDescriptors;
-				_memory->getNodeWords(iter->first, words, words3, wordsDescriptors);
-				signatures->at(iter->first).setWords(words);
-				signatures->at(iter->first).setWords3(words3);
-				signatures->at(iter->first).setWordsDescriptors(wordsDescriptors);
-
-				std::vector<CameraModel> models;
-				StereoCameraModel stereoModel;
-				_memory->getNodeCalibration(iter->first, models, stereoModel);
-				signatures->at(iter->first).sensorData().setCameraModels(models);
-				signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel);
-
-				if(!velocity.empty())
-				{
-					signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
-				}
-				signatures->at(iter->first).sensorData().setGPS(gps);
-				signatures->at(iter->first).sensorData().setEnvSensors(sensors);
+				signatures->insert(std::make_pair(iter->first, getSignatureCopy(iter->first, false, false, false, false)));
 			}
 		}
 	}
-	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
+	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
 	{
 		UERROR("Last working signature is null!?");
 	}

corelib/src/util3d_surface.cpp

@@ -1553,7 +1553,7 @@ cv::Mat mergeTextures(
 					}
 					else if(memory)
 					{
-						SensorData data = memory->getSignatureDataConst(textureId, true, false, false, false);
+						SensorData data = memory->getNodeData(textureId, true, false, false, false);
 						std::vector<CameraModel> models = data.cameraModels();
 						StereoCameraModel stereoModel = data.stereoCameraModel();
 						if(models.size()>=1 &&
@@ -1682,7 +1682,7 @@ cv::Mat mergeTextures(
 								}
 								else if(memory)
 								{
-									SensorData data = memory->getSignatureDataConst(textures[t].first, true, false, false, false);
+									SensorData data = memory->getNodeData(textures[t].first, true, false, false, false);
 									models = data.cameraModels();
 									data.uncompressDataConst(&image, 0);
 								}
@@ -2291,7 +2291,7 @@ bool multiBandTexturing(
 		}
 		else if(memory)
 		{
-			SensorData data = memory->getSignatureDataConst(camId, true, false, false, false);
+			SensorData data = memory->getNodeData(camId, true, false, false, false);
 			models = data.cameraModels();
 			if(models.empty() && data.stereoCameraModel().isValidForProjection())
 			{