Refactored how visualization data are transfered between core and gui: using only Signature object instead of separated image,deph,fx,fy...

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1928 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Memory.h

@@ -114,25 +114,7 @@ public:
 	double getDbSavingTime() const;
 	int getMapId(int signatureId) const;
 	std::vector<unsigned char> getImage(int signatureId) const;
-	void getImageDepth(
+	Signature getSignatureData(int locationId, bool uncompressedData = false);
-			int locationId,
-			std::vector<unsigned char> & rgb,
-			std::vector<unsigned char> & depth,
-			std::vector<unsigned char> & depth2d,
-			float & fx,
-			float & fy,
-			float & cx,
-			float & cy,
-			Transform & localTransform);
-	void getImageDepthRaw(
-				int locationId,
-				cv::Mat & rgb,
-				cv::Mat & depth,
-				float & fx,
-				float & fy,
-				float & cx,
-				float & cy,
-				Transform & localTransform);
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -159,7 +141,6 @@ public:
 
 	//keypoint stuff
 	const VWDictionary * getVWDictionary() const;
-	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
 	Feature2D::Type getFeatureType() const {return _featureType;}
 
 	// RGB-D stuff

corelib/include/rtabmap/core/Parameters.h

@@ -131,7 +131,7 @@ class RTABMAP_EXP Parameters
 	// Rtabmap parameters
 	RTABMAP_PARAM(Rtabmap, VhStrategy, 	                 int, 0,     "None 0, Similarity 1, Epipolar 2.");
 	RTABMAP_PARAM(Rtabmap, PublishStats, 	             bool, true, "Publishing statistics.");
-	RTABMAP_PARAM(Rtabmap, PublishImage, 	             bool, true, "Publishing image.");
+	RTABMAP_PARAM(Rtabmap, PublishLastSignature, 	     bool, true, "Publishing last signature.");
 	RTABMAP_PARAM(Rtabmap, PublishPdf, 	                 bool, true, "Publishing pdf.");
 	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true, "Publishing likelihood.");
 	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
@@ -163,7 +163,6 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
 
 	// KeypointMemory (Keypoint-based)
-	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true, 		"Publishing keypoints.");
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1, 	 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf)");

corelib/include/rtabmap/core/Rtabmap.h

@@ -106,14 +106,7 @@ public:
 	void setDatabasePath(const std::string & path);
 	void deleteLocation(int locationId); // Only nodes in STM can be deleted
 	void rejectLoopClosure(int oldId, int newId);
-	void get3DMap(std::map<int, std::vector<unsigned char> > & images,
+	void get3DMap(std::map<int, Signature> & signatures,
-			std::map<int, std::vector<unsigned char> > & depths,
-			std::map<int, std::vector<unsigned char> > & depths2d,
-			std::map<int, float> & depthFxs,
-			std::map<int, float> & depthFys,
-			std::map<int, float> & depthCxs,
-			std::map<int, float> & depthCys,
-			std::map<int, Transform> & localTransforms,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & constraints,
 			std::map<int, int> & mapIds,
@@ -142,10 +135,9 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	bool _publishImage;
+	bool _publishLastSignature;
 	bool _publishPdf;
 	bool _publishLikelihood;
-	bool _publishKeypoints;
 	float _maxTimeAllowed; // in ms
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -152,26 +152,12 @@ public:
 	RtabmapEvent3DMap(int codeError = 0):
 		UEvent(codeError){}
 	RtabmapEvent3DMap(
-			const std::map<int, std::vector<unsigned char> > & images,
+			const std::map<int, Signature> & signatures,
-			const std::map<int, std::vector<unsigned char> > & depths,
-			const std::map<int, std::vector<unsigned char> > & depths2d,
-			const std::map<int, float> & depthFxs,
-			const std::map<int, float> & depthFys,
-			const std::map<int, float> & depthCxs,
-			const std::map<int, float> & depthCys,
-			const std::map<int, Transform> & localTransforms,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & constraints,
 			const std::map<int, int> & mapIds) :
 		UEvent(0),
-		_images(images),
+		_signatures(signatures),
-		_depths(depths),
-		_depths2d(depths2d),
-		_depthFxs(depthFxs),
-		_depthFys(depthFys),
-		_depthCxs(depthCxs),
-		_depthCys(depthCys),
-		_localTransforms(localTransforms),
 		_poses(poses),
 		_constraints(constraints),
 		_mapIds(mapIds)
@@ -179,14 +165,7 @@ public:
 
 	virtual ~RtabmapEvent3DMap() {}
 
-	const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
+	const std::map<int, Signature> & getSignatures() const {return _signatures;}
-	const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
-	const std::map<int, std::vector<unsigned char> > & getDepths2d() const {return _depths2d;}
-	const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
-	const std::map<int, float> & getDepthFys() const {return _depthFys;}
-	const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
-	const std::map<int, float> & getDepthCys() const {return _depthCys;}
-	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
 	const std::map<int, Transform> & getPoses() const {return _poses;}
 	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
 	const std::map<int, int> & getMapIds() const {return _mapIds;}
@@ -194,14 +173,7 @@ public:
 	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
 
 private:
-	std::map<int, std::vector<unsigned char> > _images;
+	std::map<int, Signature> _signatures;
-	std::map<int, std::vector<unsigned char> > _depths;
-	std::map<int, std::vector<unsigned char> > _depths2d;
-	std::map<int, float> _depthFxs;
-	std::map<int, float> _depthFys;
-	std::map<int, float> _depthCxs;
-	std::map<int, float> _depthCys;
-	std::map<int, Transform> _localTransforms;
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
 	std::map<int, int> _mapIds;

corelib/include/rtabmap/core/SensorData.h

@@ -76,7 +76,8 @@ public:
 	RTABMAP_DEPRECATED(bool empty() const, "Use !isValid() instead.");
 
 	const cv::Mat & image() const {return _image;}
-	int id() const {return _id;};
+	int id() const {return _id;}
+	void setId(int id) {_id = id;}
 
 	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
 	void setPose(const Transform & pose) {_pose = pose;}

corelib/include/rtabmap/core/Signature.h

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -40,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <set>
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/SensorData.h>
 
 namespace rtabmap
 {
@@ -50,6 +50,7 @@ class RTABMAP_EXP Signature
 {
 
 public:
+	Signature();
 	Signature(int id,
 			int mapId,
 			const std::multimap<int, cv::KeyPoint> & words,
@@ -134,6 +135,11 @@ public:
 	const Transform & getLocalTransform() const {return _localTransform;}
 	void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
 	const cv::Mat & getDepthRaw() const {return _depthRaw;}
+	void setDepth2DRaw(const cv::Mat & depth2D) {_depth2DRaw = depth2D;}
+	const cv::Mat & getDepth2DRaw() const {return _depth2DRaw;}
+
+	SensorData toSensorData();
+	void uncompressData();
 
 private:
 	int _id;
@@ -166,6 +172,7 @@ private:
 
 	cv::Mat _imageRaw;
 	cv::Mat _depthRaw;
+	cv::Mat _depth2DRaw;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Statistics.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/imgproc/imgproc.hpp>
 #include <list>
 #include <vector>
+#include <rtabmap/core/Signature.h>
 #include <rtabmap/core/Link.h>
 
 namespace rtabmap {
@@ -129,26 +130,16 @@ public:
 	void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
 
 	void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
-	void setImages(const std::map<int, std::vector<unsigned char> > & images) {_images = images;}
+	void setSignature(const Signature & s) {_signature = s;}
-	void setDepths(const std::map<int, std::vector<unsigned char> > & depths) {_depths = depths;}
-	void setDepth2ds(const std::map<int, std::vector<unsigned char> > & depth2ds) {_depth2ds = depth2ds;}
-	void setDepthFxs(const std::map<int, float> & fxs) {_depthFxs = fxs;}
-	void setDepthFys(const std::map<int, float> & fys) {_depthFys = fys;}
-	void setDepthCxs(const std::map<int, float> & cxs) {_depthCxs = cxs;}
-	void setDepthCys(const std::map<int, float> & cys) {_depthCys = cys;}
-	void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}
 
 	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
 	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
-	void setCurrentPose(const Transform & pose) {_currentPose = pose;}
 	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
 	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
 	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
 	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
-	void setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
-	void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
 
 	// getters
 	bool extended() const {return _extended;}
@@ -157,26 +148,16 @@ public:
 	int localLoopClosureId() const {return _localLoopClosureId;}
 
 	const std::map<int, int> & getMapIds() const {return _mapIds;}
-	const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
+	const Signature & getSignature() const {return _signature;}
-	const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
-	const std::map<int, std::vector<unsigned char> > & getDepth2ds() const {return _depth2ds;}
-	const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
-	const std::map<int, float> & getDepthFys() const {return _depthFys;}
-	const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
-	const std::map<int, float> & getDepthCys() const {return _depthCys;}
-	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
 
 	const std::map<int, Transform> & poses() const {return _poses;}
 	const std::multimap<int, Link> & constraints() const {return _constraints;}
-	const Transform & currentPose() const {return _currentPose;}
 	const Transform & mapCorrection() const {return _mapCorrection;}
 	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
 	const std::map<int, int> & weights() const {return _weights;}
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
 	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
-	const std::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
-	const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
 
 	const std::map<std::string, float> & data() const {return _data;}
 
@@ -189,20 +170,12 @@ private:
 
 	// extended data start here...
 	std::map<int, int> _mapIds;
-	std::map<int, std::vector<unsigned char> > _images;
 
-	// Metric data
+	// Signature data
-	std::map<int, std::vector<unsigned char> > _depths;
+	Signature _signature;
-	std::map<int, std::vector<unsigned char> > _depth2ds;
-	std::map<int, float> _depthFxs;
-	std::map<int, float> _depthFys;
-	std::map<int, float> _depthCxs;
-	std::map<int, float> _depthCys;
-	std::map<int, Transform> _localTransforms;
 
 	std::map<int, Transform> _poses;
 	std::multimap<int, Link> _constraints;
-	Transform _currentPose;
 	Transform _mapCorrection;
 	Transform _loopClosureTransform;
 
@@ -211,10 +184,6 @@ private:
 	std::map<int, float> _likelihood;
 	std::map<int, float> _rawLikelihood;
 
-	//keypoint memory
-	std::multimap<int, cv::KeyPoint> _refWords;
-	std::multimap<int, cv::KeyPoint> _loopWords;
-
 	// Format for statistics (Plottable statistics must go in that map) :
 	// {"Group/Name/Unit", value}
 	// Example : {"Timing/Total time/ms", 500.0f}

corelib/include/rtabmap/core/util3d.h

@@ -46,8 +46,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-class Signature;
-
 namespace util3d
 {
 

corelib/src/Memory.cpp

@@ -1821,9 +1821,35 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
 			_dbDriver->loadNodeData(depthToLoad, true);
 		}
 	}
+
 	Transform t;
 	if(oldS && newS)
 	{
+		//make sure data are uncompressed
+		if(icp3D)
+		{
+			if(oldS->getDepthRaw().empty())
+			{
+				oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepth()));
+			}
+			if(newS->getDepthRaw().empty())
+			{
+				newS->setDepthRaw(util3d::uncompressImage(newS->getDepth()));
+			}
+		}
+		else
+		{
+			if(oldS->getDepth2DRaw().empty())
+			{
+				oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2D()));
+			}
+			if(newS->getDepth2DRaw().empty())
+			{
+				newS->setDepth2DRaw(util3d::uncompressData(newS->getDepth2D()));
+			}
+		}
+
+
 		t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg);
 	}
 	else
@@ -1860,24 +1886,16 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 	if(icp3D)
 	{
 		UDEBUG("3D ICP");
-		util3d::CompressionThread ctOld(oldS.getDepth(), true);
+		if(!oldS.getDepthRaw().empty() && !newS.getDepthRaw().empty())
-		util3d::CompressionThread ctNew(newS.getDepth(), true);
-		ctOld.start();
-		ctNew.start();
-		ctOld.join();
-		ctNew.join();
-		cv::Mat oldDepth = ctOld.getUncompressedData();
-		cv::Mat newDepth = ctNew.getUncompressedData();
-		if(!oldDepth.empty() && !newDepth.empty())
 		{
-			if(oldDepth.type() == CV_8UC1 || newDepth.type() == CV_8UC1)
+			if(oldS.getDepthRaw().type() == CV_8UC1 || newS.getDepthRaw().type() == CV_8UC1)
 			{
 				UERROR("ICP 3D cannot be done on stereo images!");
 			}
 			else
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
-						oldDepth,
+						oldS.getDepthRaw(),
 						oldS.getDepthFx(),
 						oldS.getDepthFy(),
 						oldS.getDepthCx(),
@@ -1888,7 +1906,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 						_icpSamples,
 						oldS.getLocalTransform());
 				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
-						newDepth,
+						newS.getDepthRaw(),
 						newS.getDepthFx(),
 						newS.getDepthFy(),
 						newS.getDepthCx(),
@@ -1964,19 +1982,11 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 			UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p);
 		}
 
-		util3d::CompressionThread ctOld(oldS.getDepth2D(), false);
+		if(!oldS.getDepth2DRaw().empty() && !newS.getDepth2DRaw().empty())
-		util3d::CompressionThread ctNew(newS.getDepth2D(), false);
-		ctOld.start();
-		ctNew.start();
-		ctOld.join();
-		ctNew.join();
-		cv::Mat oldDepth2D = ctOld.getUncompressedData();
-		cv::Mat newDepth2D = ctNew.getUncompressedData();
-		if(!oldDepth2D.empty() && !newDepth2D.empty())
 		{
 			// 2D
-			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldS.getDepth2DRaw());
-			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, guess);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newS.getDepth2DRaw(), guess);
 
 			//voxelize
 			if(_icp2VoxelSize > 0.0f)
@@ -2545,106 +2555,71 @@ std::vector<unsigned char> Memory::getImage(int signatureId) const
 	return image;
 }
 
-void Memory::getImageDepth(
+Signature Memory::getSignatureData(int locationId, bool uncompressedData)
-		int locationId,
-		std::vector<unsigned char> & rgb,
-		std::vector<unsigned char> & depth,
-		std::vector<unsigned char> & depth2d,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform)
 {
+	Signature r;
 	Signature * s = this->_getSignature(locationId);
+	if(s && s->getImage().size())
+	{
+		r = *s;
+	}
+	else if(_dbDriver)
+	{
+		// load from database
 		if(s)
 		{
-		rgb = s->getImage();
+			std::list<Signature*> signatures;
-		depth = s->getDepth();
+			signatures.push_back(s);
-		depth2d = s->getDepth2D();
+			_dbDriver->loadNodeData(signatures, !s->getPose().isNull());
-		fx = s->getDepthFx();
+			r = *s;
-		fy = s->getDepthFy();
-		cx = s->getDepthCx();
-		cy = s->getDepthCy();
-		localTransform = s->getLocalTransform();
 		}
-	if(rgb.empty() && this->isRawDataKept() && _dbDriver)
+		else
 		{
-		_dbDriver->getNodeData(locationId, rgb, depth, depth2d, fx, fy, cx, cy, localTransform);
+			std::list<int> ids;
-
+			ids.push_back(locationId);
-		if(s)
+			std::list<Signature*> signatures;
+			_dbDriver->loadSignatures(ids, signatures);
+			if(signatures.size())
 			{
-			// keep in cache
+				Signature * sTmp = signatures.front();
-			if(!rgb.empty())
+				if(sTmp->getImage().size() == 0)
 				{
-				s->setImage(rgb);
+					_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
-			}
-			if(!depth.empty())
-			{
-				s->setDepth(depth, fx, fy, cx, cy);
-			}
-			if(!depth2d.empty())
-			{
-				s->setDepth2D(depth2d);
-			}
-			if(!localTransform.isNull())
-			{
-				s->setLocalTransform(localTransform);
 				}
+				r = *sTmp;
+				this->moveToTrash(s);
 			}
 		}
 	}
 
-void Memory::getImageDepthRaw(
+	if(uncompressedData && r.getImageRaw().empty() && r.getImage().size())
-		int locationId,
-		cv::Mat & rgb,
-		cv::Mat & depth,
-		float & fx,
-		float & fy,
-		float & cx,
-		float & cy,
-		Transform & localTransform)
 	{
-	Signature * s = this->_getSignature(locationId);
+		//uncompress data
 		if(s)
 		{
-		rgb = s->getImageRaw();
+			s->uncompressData();
-		depth = s->getDepthRaw();
+			r.setImageRaw(s->getImageRaw());
-		fx = s->getDepthFx();
+			r.setDepthRaw(s->getDepthRaw());
-		fy = s->getDepthFy();
+			r.setDepth2DRaw(s->getDepth2DRaw());
-		cx = s->getDepthCx();
-		cy = s->getDepthCy();
-		localTransform = s->getLocalTransform();
 		}
-	if(rgb.empty())
+		else
 		{
-		std::vector<unsigned char> compressedRgb;
+			util3d::CompressionThread ctImage(r.getImage(), true);
-		std::vector<unsigned char> compressedDepth;
+			util3d::CompressionThread ctDepth(r.getDepth(), true);
-		std::vector<unsigned char> comressedDepth2d;
+			util3d::CompressionThread ctDepth2D(r.getDepth2D(), false);
-		getImageDepth(locationId, compressedRgb, compressedDepth, comressedDepth2d, fx, fy, cx, cy, localTransform);
-
-		//uncomressed data
-		util3d::CompressionThread ctImage(compressedRgb, true);
-		util3d::CompressionThread ctDepth(compressedDepth, true);
 			ctImage.start();
 			ctDepth.start();
+			ctDepth2D.start();
 			ctImage.join();
 			ctDepth.join();
-		rgb = ctImage.getUncompressedData();
+			ctDepth2D.join();
-		depth = ctDepth.getUncompressedData();
+			r.setImageRaw(ctImage.getUncompressedData());
-		if(s)
+			r.setDepthRaw(ctDepth.getUncompressedData());
-		{
+			r.setDepth2DRaw(ctDepth2D.getUncompressedData());
-			//save it uncompressed in the signature
-			if(!rgb.empty())
-			{
-				s->setImageRaw(rgb);
-			}
-			if(!depth.empty())
-			{
-				s->setDepthRaw(depth);
-			}
 		}
 	}
+
+	return r;
 }
 
 void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
@@ -2938,49 +2913,11 @@ void Memory::createGraph(GraphNode * parent, unsigned int maxDepth, const std::s
 	}
 }
 
-// Keypoint stuff
-std::multimap<int, cv::KeyPoint> Memory::getWords(int signatureId) const
-{
-	std::multimap<int, cv::KeyPoint> words;
-	if(signatureId>0)
-	{
-		const Signature * s = this->getSignature(signatureId);
-		if(s)
-		{
-			const Signature * ks = dynamic_cast<const Signature*>(s);
-			if(ks)
-			{
-				words = ks->getWords();
-			}
-		}
-		else if(_dbDriver)
-		{
-			std::list<int> ids;
-			ids.push_back(signatureId);
-			std::list<Signature *> signatures;
-			_dbDriver->loadSignatures(ids, signatures);
-			if(signatures.size())
-			{
-				const Signature * ks = dynamic_cast<const Signature*>(signatures.front());
-				if(ks)
-				{
-					words = ks->getWords();
-				}
-			}
-			for(std::list<Signature *>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
-			{
-				delete *iter;
-			}
-		}
-	}
-	return words;
-}
-
 int Memory::getNi(int signatureId) const
 {
 	int ni = 0;
 	const Signature * s = this->getSignature(signatureId);
-	if(s) // Must be a SurfSignature
+	if(s)
 	{
 		ni = ((Signature *)s)->getWords().size();
 	}
@@ -2994,7 +2931,6 @@ int Memory::getNi(int signatureId) const
 
 void Memory::copyData(const Signature * from, Signature * to)
 {
-	// The signatures must be KeypointSignature
 	UTimer timer;
 	timer.start();
 	if(from && to)
@@ -3415,21 +3351,22 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 		}
 		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
 		util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
+		util3d::CompressionThread ctDepth2d(data.depth2d());
 		ctImage.start();
 		ctDepth.start();
+		ctDepth2d.start();
 		ctImage.join();
 		ctDepth.join();
-		imageBytes = ctImage.getCompressedData();
+		ctDepth2d.join();
-		depthBytes = ctDepth.getCompressedData();
 
 		s = new Signature(id,
 			_idMapCount,
 			words,
 			words3D,
 			data.pose(),
-			util3d::compressData(data.depth2d()),
+			ctDepth2d.getCompressedData(),
-			imageBytes,
+			ctImage.getCompressedData(),
-			depthBytes,
+			ctDepth.getCompressedData(),
 			data.fx(),
 			data.fy()>0.0f?data.fy():data.baseline(),
 			data.cx(),
@@ -3437,6 +3374,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
 			data.localTransform());
 		s->setImageRaw(data.image());
 		s->setDepthRaw(depthOrRightImage);
+		s->setDepth2DRaw(data.depth2d());
 	}
 	else
 	{

corelib/src/Rtabmap.cpp

@@ -74,10 +74,9 @@ namespace rtabmap
 
 Rtabmap::Rtabmap() :
 	_publishStats(Parameters::defaultRtabmapPublishStats()),
-	_publishImage(Parameters::defaultRtabmapPublishImage()),
+	_publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()),
 	_publishPdf(Parameters::defaultRtabmapPublishPdf()),
 	_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
-	_publishKeypoints(Parameters::defaultKpPublishKeypoints()),
 	_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
 	_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
@@ -330,10 +329,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 
 	Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
-	Parameters::parse(parameters, Parameters::kRtabmapPublishImage(), _publishImage);
+	Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
-	Parameters::parse(parameters, Parameters::kKpPublishKeypoints(), _publishKeypoints);
 	Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
@@ -1291,6 +1289,7 @@ bool Rtabmap::process(const SensorData & data)
 				uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
 				uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
 				uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+				uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
 
 				//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
 				//{
@@ -1302,30 +1301,25 @@ bool Rtabmap::process(const SensorData & data)
 				UTimer timeT;
 
 				// Add signatures
-				float fxA, fyA, cxA, cyA;
+				SensorData dataFrom = data;
-				float fxB, fyB, cxB, cyB;
+				dataFrom.setId(signature->id());
-				rtabmap::Transform localTransformA, localTransformB;
+				Signature tmpTo = _memory->getSignatureData(_lcHypothesisId, true);
+				SensorData dataTo = tmpTo.toSensorData();
+				UDEBUG("timeTo = %fs", timeT.ticks());
 
-				cv::Mat imageA, depthA;
+				if(dataFrom.isValid() &&
-				_memory->getImageDepthRaw(signature->id(), imageA, depthA, fxA, fyA, cxA, cyA, localTransformA);
+				   dataFrom.isMetric() &&
-				SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
+				   dataTo.isValid() &&
-
+				   dataTo.isMetric() &&
-				UDEBUG("timeA = %fs", timeT.ticks());
+				   dataFrom.id() != Memory::kIdInvalid &&
-
+				   tmpTo.id() != Memory::kIdInvalid)
-				cv::Mat imageB, depthB;
-				_memory->getImageDepthRaw(_lcHypothesisId, imageB, depthB, fxB, fyB, cxB, cyB, localTransformB);
-				SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
-
-				UDEBUG("timeB = %fs", timeT.ticks());
-
-				if(dataFrom.isValid() && dataFrom.isMetric() && dataTo.isValid() && dataTo.isMetric())
 				{
-					memory.update(dataFrom);
-					UDEBUG("timeUpA = %fs", timeT.ticks());
 					memory.update(dataTo);
-					UDEBUG("timeUpB = %fs", timeT.ticks());
+					UDEBUG("timeUpTo = %fs", timeT.ticks());
+					memory.update(dataFrom);
+					UDEBUG("timeUpFrom = %fs", timeT.ticks());
 
-					transform = memory.computeVisualTransform(2, 1, &rejectedMsg, &loopClosureVisualInliers);
+					transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers);
 					UDEBUG("timeTransform = %fs", timeT.ticks());
 				}
 				else
@@ -1580,74 +1574,9 @@ bool Rtabmap::process(const SensorData & data)
 			//Epipolar geometry constraint
 			statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 
-			if(_publishImage)
+			if(_publishLastSignature)
 			{
-				std::map<int, std::vector<unsigned char> > images;
+				statistics_.setSignature(*signature);
-				std::map<int, std::vector<unsigned char> > depths;
-				std::map<int, std::vector<unsigned char> > depth2ds;
-				std::map<int, float> depthFxs;
-				std::map<int, float> depthFys;
-				std::map<int, float> depthCxs;
-				std::map<int, float> depthCys;
-				std::map<int, Transform> localTransforms;
-
-				std::vector<int> ids(signaturesRetrieved.begin(), signaturesRetrieved.end());
-				ids.push_back(signature->id());
-				if(sLoop)
-				{
-					ids.push_back(sLoop->id());
-				}
-
-				UTimer tmpTimer;
-				for(unsigned int i=0; i<ids.size(); ++i)
-				{
-					// Add data
-					std::vector<unsigned char> im;
-					if(_rgbdSlamMode && _memory->isIncremental())
-					{
-						std::vector<unsigned char> depth, depth2d;
-						float fx, fy, cx, cy;
-						Transform localTransform;
-						_memory->getImageDepth(ids[i], im, depth, depth2d, fx, fy, cx, cy, localTransform);
-
-						if(!depth.empty())
-						{
-							depths.insert(std::make_pair(ids[i], depth));
-							depthFxs.insert(std::make_pair(ids[i], fx));
-							depthFys.insert(std::make_pair(ids[i], fy));
-							depthCxs.insert(std::make_pair(ids[i], cx));
-							depthCys.insert(std::make_pair(ids[i], cy));
-							localTransforms.insert(std::make_pair(ids[i], localTransform));
-						}
-						if(!depth2d.empty())
-						{
-							depth2ds.insert(std::make_pair(ids[i], depth2d));
-						}
-					}
-					else
-					{
-						im = _memory->getImage(ids[i]);
-					}
-					UASSERT(_memory->getSignature(ids[i]) != 0);
-					if(!im.empty())
-					{
-						images.insert(std::make_pair(ids[i], im));
-					}
-				}
-
-				if(tmpTimer.elapsed() > 0.03)
-				{
-					UWARN("getting data[%d] time = %fs", (int)ids.size(), tmpTimer.ticks());
-				}
-
-				statistics_.setImages(images);
-				statistics_.setDepths(depths);
-				statistics_.setDepth2ds(depth2ds);
-				statistics_.setDepthFxs(depthFxs);
-				statistics_.setDepthFys(depthFys);
-				statistics_.setDepthCxs(depthCxs);
-				statistics_.setDepthCys(depthCys);
-				statistics_.setLocalTransforms(localTransforms);
 			}
 
 			if(_publishLikelihood || _publishPdf)
@@ -1664,17 +1593,6 @@ bool Rtabmap::process(const SensorData & data)
 					statistics_.setRawLikelihood(rawLikelihood);
 				}
 			}
-
-			if(_publishKeypoints)
-			{
-				//Copy keypoints
-				statistics_.setRefWords(signature->getWords());
-				if(sLoop)
-				{
-					//Copy keypoints
-					statistics_.setLoopWords(sLoop->getWords());
-				}
-			}
 		}
 
 		timeStatsCreation = timer.ticks();
@@ -1764,7 +1682,6 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
 			statistics_.setMapCorrection(_mapCorrection);
-			statistics_.setCurrentPose(_mapCorrection * currentRawOdomPose);
 			UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
 		}
 	}
@@ -2292,14 +2209,7 @@ void Rtabmap::dumpPrediction() const
 	}
 }
 
-void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
+void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
-		std::map<int, std::vector<unsigned char> > & depths,
-		std::map<int, std::vector<unsigned char> > & depths2d,
-		std::map<int, float> & depthFxs,
-		std::map<int, float> & depthFys,
-		std::map<int, float> & depthCxs,
-		std::map<int, float> & depthCys,
-		std::map<int, Transform> & localTransforms,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		std::map<int, int> & mapIds,
@@ -2328,38 +2238,14 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			std::vector<unsigned char> image, depth, depth2d;
+			Signature data = _memory->getSignatureData(*iter);
-			float fx, fy, cx, cy;
+			if(data.id() != Memory::kIdInvalid)
-			Transform localTransform;
-			_memory->getImageDepth(*iter, image, depth, depth2d, fx, fy, cx, cy, localTransform);
-
-			if(image.size())
 			{
-				images.insert(std::make_pair(*iter, image));
+				signatures.insert(std::make_pair(*iter, data));
-			}
-			if(depth.size())
-			{
-				depths.insert(std::make_pair(*iter, depth));
-			}
-			if(depth2d.size())
-			{
-				depths2d.insert(std::make_pair(*iter, depth2d));
-			}
-			if(fx > 0 && fy > 0)
-			{
-				depthFxs.insert(std::make_pair(*iter, fx));
-				depthFys.insert(std::make_pair(*iter, fy));
-				depthCxs.insert(std::make_pair(*iter, cx));
-				depthCys.insert(std::make_pair(*iter, cy));
-			}
-			if(!localTransform.isNull())
-			{
-				localTransforms.insert(std::make_pair(*iter, localTransform));
-			}
-
 				mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 			}
 		}
+	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))
 	{
 		UERROR("Last working signature is null!?");

corelib/src/RtabmapThread.cpp

@@ -100,40 +100,19 @@ void RtabmapThread::setBufferSize(int bufferSize)
 
 void RtabmapThread::publishMap(bool optimized, bool full) const
 {
-	std::map<int, std::vector<unsigned char> > images;
+	std::map<int, Signature> signatures;
-	std::map<int, std::vector<unsigned char> > depths;
-	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthFxs;
-	std::map<int, float> depthFys;
-	std::map<int, float> depthCxs;
-	std::map<int, float> depthCys;
-	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
 
-	_rtabmap->get3DMap(images,
+	_rtabmap->get3DMap(signatures,
-			depths,
-			depths2d,
-			depthFxs,
-			depthFys,
-			depthCxs,
-			depthCys,
-			localTransforms,
 			poses,
 			constraints,
 			mapIds,
 			optimized,
 			full);
 
-	this->post(new RtabmapEvent3DMap(images,
+	this->post(new RtabmapEvent3DMap(signatures,
-			depths,
-			depths2d,
-			depthFxs,
-			depthFys,
-			depthCxs,
-			depthCys,
-			localTransforms,
 			poses,
 			constraints,
 			mapIds));
@@ -141,14 +120,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 
 void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 {
-	std::map<int, std::vector<unsigned char> > images;
+	std::map<int, Signature> signatures;
-	std::map<int, std::vector<unsigned char> > depths;
-	std::map<int, std::vector<unsigned char> > depths2d;
-	std::map<int, float> depthFxs;
-	std::map<int, float> depthFys;
-	std::map<int, float> depthCxs;
-	std::map<int, float> depthCys;
-	std::map<int, Transform> localTransforms;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
 	std::map<int, int> mapIds;
@@ -159,14 +131,7 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
 			optimized,
 			full);
 
-	this->post(new RtabmapEvent3DMap(images,
+	this->post(new RtabmapEvent3DMap(signatures,
-			depths,
-			depths2d,
-			depthFxs,
-			depthFys,
-			depthCxs,
-			depthCys,
-			localTransforms,
 			poses,
 			constraints,
 			mapIds));

corelib/src/Signature.cpp

@@ -36,9 +36,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
-Signature::~Signature()
+Signature::Signature() :
+	_id(0), // invalid id
+	_mapId(-1),
+	_weight(-1),
+	_saved(false),
+	_modified(true),
+	_neighborsModified(true),
+	_enabled(false),
+	_fx(0.0f),
+	_fy(0.0f),
+	_cx(0.0f),
+	_cy(0.0f)
 {
-	ULOGGER_DEBUG("id=%d", _id);
 }
 
 Signature::Signature(
@@ -76,6 +86,11 @@ Signature::Signature(
 {
 }
 
+Signature::~Signature()
+{
+	ULOGGER_DEBUG("id=%d", _id);
+}
+
 void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
 {
 	for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
@@ -212,4 +227,39 @@ void Signature::setDepth(const std::vector<unsigned char> & depth, float fx, flo
 	_cy=cy;
 }
 
+SensorData Signature::toSensorData()
+{
+	this->uncompressData();
+	return SensorData(_imageRaw,
+			_depthRaw,
+			_depth2DRaw,
+			_fx,
+			_fy,
+			_cx,
+			_cy,
+			_pose,
+			_localTransform,
+			_id);
+}
+
+void Signature::uncompressData()
+{
+	if(_imageRaw.empty() && _image.size())
+	{
+		//uncompress data
+		util3d::CompressionThread ctImage(_image, true);
+		util3d::CompressionThread ctDepth(_depth, true);
+		util3d::CompressionThread ctDepth2D(_depth2D, false);
+		ctImage.start();
+		ctDepth.start();
+		ctDepth2D.start();
+		ctImage.join();
+		ctDepth.join();
+		ctDepth2D.join();
+		_imageRaw = ctImage.getUncompressedData();
+		_depthRaw = ctDepth.getUncompressedData();
+		_depth2DRaw = ctDepth2D.getUncompressedData();
+	}
+}
+
 } //namespace rtabmap

examples/BOWMapping/CMakeLists.txt

@@ -1,10 +1,12 @@
 SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/corelib/include
     ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
 )
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES} 
+	${PCL_LIBRARIES}
 )
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

examples/RGBDMapping/MapBuilder.h

@@ -163,23 +163,16 @@ private slots:
 					cloudViewer_->setCloudVisibility(cloudName, true);
 				}
 				else if(iter->first == stats.refImageId() &&
-						uContains(stats.getImages(), iter->first) &&
+						stats.getSignature().id() == iter->first)
-						uContains(stats.getDepths(), iter->first) &&
-						uContains(stats.getDepthFxs(), iter->first) &&
-						uContains(stats.getDepthFys(), iter->first) &&
-						uContains(stats.getLocalTransforms(), iter->first))
 				{
 					// Add the new cloud
-					cv::Mat rgb = util3d::uncompressImage(stats.getImages().at(iter->first));
-					cv::Mat depth = util3d::uncompressImage(stats.getDepths().at(iter->first));
-					Transform localTransform = stats.getLocalTransforms().at(iter->first);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
-							rgb,
+							stats.getSignature().getImageRaw(),
-							depth,
+							stats.getSignature().getDepthRaw(),
-							stats.getDepthCxs().at(iter->first),
+							stats.getSignature().getDepthCx(),
-							stats.getDepthCys().at(iter->first),
+							stats.getSignature().getDepthCy(),
-							stats.getDepthFxs().at(iter->first),
+							stats.getSignature().getDepthFx(),
-							stats.getDepthFys().at(iter->first),
+							stats.getSignature().getDepthFy(),
 						   8); // decimation
 
 					if(cloud->size())
@@ -187,7 +180,7 @@ private slots:
 						cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, 4.0f);
 						if(cloud->size())
 						{
-							cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
+							cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, stats.getSignature().getLocalTransform());
 						}
 					}
 					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))

guilib/include/rtabmap/gui/LoopClosureViewer.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Signature.h>
 #include <opencv2/opencv.hpp>
 #include <QtGui/QWidget>
 
@@ -39,8 +40,6 @@ class Ui_loopClosureViewer;
 
 namespace rtabmap {
 
-class Signature;
-
 class RTABMAPGUI_EXP LoopClosureViewer : public QWidget {
 
 	Q_OBJECT
@@ -49,11 +48,10 @@ public:
 	LoopClosureViewer(QWidget * parent);
 	virtual ~LoopClosureViewer();
 
-	// take ownership
+	void setData(const Signature & sA, const Signature & sB); // sB contains loop transform as pose() from sA
-	void setData(Signature * sA, Signature * sB); // sB contains loop transform as pose() from sA
 
-	const Signature * sA() const {return sA_;}
+	const Signature & sA() const {return sA_;}
-	const Signature * sB() const {return sB_;}
+	const Signature & sB() const {return sB_;}
 
 public slots:
 	void setDecimation(int decimation) {decimation_ = decimation;}
@@ -67,8 +65,8 @@ protected:
 private:
 	Ui_loopClosureViewer * ui_;
 
-	Signature * sA_;
+	Signature sA_;
-	Signature * sB_;
+	Signature sB_;
 	Transform transform_;
 
 	int decimation_;

guilib/include/rtabmap/gui/MainWindow.h

@@ -248,15 +248,8 @@ private:
 	bool _processingStatistics;
 	bool _odometryReceived;
 
-	QMap<int, std::vector<unsigned char> > _imagesMap;
+	QMap<int, Signature> _cachedSignatures;
-	QMap<int, std::vector<unsigned char> > _depthsMap;
-	QMap<int, std::vector<unsigned char> > _depths2DMap;
-	QMap<int, float> _depthFxsMap;
-	QMap<int, float> _depthFysMap;
-	QMap<int, float> _depthCxsMap;
-	QMap<int, float> _depthCysMap;
 	QMap<int, int> _mapIds;
-	QMap<int, Transform> _localTransformsMap;
 	std::map<int, Transform> _currentPosesMap;
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;

guilib/src/DatabaseViewer.cpp

@@ -304,40 +304,10 @@ void DatabaseViewer::exportDatabase()
 				for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 				{
 					int id = ids_.at(i);
-					std::vector<unsigned char> compressedRgb, compressedDepth, compressedDepth2d;
-					float tmpFx, tmpFy, tmpCx, tmpCy;
-					rtabmap::Transform tmpLocalTransform, pose;
 
-					memory_->getImageDepth(id, compressedRgb, compressedDepth, compressedDepth2d, tmpFx, tmpFy, tmpCx, tmpCy, tmpLocalTransform);
+					Signature data = memory_->getSignatureData(id, true);
-					if(dialog.isOdomExported())
+					rtabmap::SensorData sensorData = data.toSensorData();
-					{
+					recorder.addData(sensorData);
-						memory_->getPose(id, pose, true);
-					}
-
-					cv::Mat rgb, depth, depth2d;
-					float fx = 0, fy = 0, cx = 0, cy = 0;
-					rtabmap::Transform localTransform;
-
-					if(dialog.isRgbExported())
-					{
-						rgb = rtabmap::util3d::uncompressImage(compressedRgb);
-					}
-					if(dialog.isDepthExported())
-					{
-						depth = rtabmap::util3d::uncompressImage(compressedDepth);
-						fx = tmpFx;
-						fy = tmpFy;
-						cx = tmpCx;
-						cy = tmpCy;
-						localTransform = tmpLocalTransform;
-					}
-					if(dialog.isDepth2dExported())
-					{
-						depth2d = rtabmap::util3d::uncompressData(compressedDepth2d);
-					}
-
-					rtabmap::SensorData data(rgb, depth, depth2d, fx, fy, cx, cy, pose, localTransform, id);
-					recorder.addData(data);
 
 					progressDialog.appendText(tr("Exported node %1").arg(id));
 					progressDialog.incrementStep();
@@ -764,40 +734,35 @@ void DatabaseViewer::view3DMap()
 						rtabmap::Transform pose = iter->second;
 						if(!pose.isNull())
 						{
-							std::vector<unsigned char> image, depth, depth2d;
+							Signature data = memory_->getSignatureData(iter->first, true);
-							float fx, fy, cx, cy;
-							rtabmap::Transform localTransform;
-							memory_->getImageDepth(iter->first, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-							cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
+							UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
-							cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+							UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
-							UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
+							if(data.getDepthRaw().type() == CV_8UC1)
-							UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
-							if(depthMat.type() == CV_8UC1)
 							{
 								cv::Mat leftImg;
-								if(imageMat.channels() == 3)
+								if(data.getImageRaw().channels() == 3)
 								{
-									cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
+									cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
 								}
 								else
 								{
-									leftImg = imageMat;
+									leftImg = data.getImageRaw();
 								}
 								cloud = rtabmap::util3d::cloudFromDisparityRGB(
-									imageMat,
+										data.getImageRaw(),
-									util3d::disparityFromStereoImages(leftImg, depthMat),
+									util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
-									cx, cy,
+									data.getDepthCx(), data.getDepthCy(),
-									fx, fy,
+									data.getDepthFx(), data.getDepthFy(),
 									decimation);
 							}
 							else
 							{
 								cloud = rtabmap::util3d::cloudFromDepthRGB(
-										imageMat,
+										data.getImageRaw(),
-										depthMat,
+										data.getDepthRaw(),
-										cx, cy,
+										data.getDepthCx(), data.getDepthCy(),
-										fx, fy,
+										data.getDepthFx(), data.getDepthFy(),
 										decimation);
 							}
 
@@ -806,7 +771,7 @@ void DatabaseViewer::view3DMap()
 								cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
 							}
 
-							cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
+							cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
 
 							QColor color = Qt::red;
 							int mapId = memory_->getMapId(iter->first);
@@ -890,40 +855,35 @@ void DatabaseViewer::generate3DMap()
 								rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
 								if(!pose.isNull())
 								{
-									std::vector<unsigned char> image, depth, depth2d;
+									Signature data = memory_->getSignatureData(iter->first, true);
-									float fx, fy, cx, cy;
-									rtabmap::Transform localTransform;
-									memory_->getImageDepth(iter->first, image, depth, depth2d, fx, fy, cx, cy, localTransform);
 									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-									cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
+									UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
-									cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+									UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
-									UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
+									if(data.getDepthRaw().type() == CV_8UC1)
-									UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
-									if(depthMat.type() == CV_8UC1)
 									{
 										cv::Mat leftImg;
-										if(imageMat.channels() == 3)
+										if(data.getImageRaw().channels() == 3)
 										{
-											cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
+											cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
 										}
 										else
 										{
-											leftImg = imageMat;
+											leftImg = data.getImageRaw();
 										}
 										cloud = rtabmap::util3d::cloudFromDisparityRGB(
-											imageMat,
+												data.getImageRaw(),
-											util3d::disparityFromStereoImages(leftImg, depthMat),
+											util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
-											cx, cy,
+											data.getDepthCx(), data.getDepthCy(),
-											fx, fy,
+											data.getDepthFx(), data.getDepthFy(),
 											decimation);
 									}
 									else
 									{
 										cloud = rtabmap::util3d::cloudFromDepthRGB(
-												imageMat,
+												data.getImageRaw(),
-												depthMat,
+												data.getDepthRaw(),
-												cx, cy,
+												data.getDepthCx(), data.getDepthCy(),
-												fx, fy,
+												data.getDepthFx(), data.getDepthFy(),
 												decimation);
 									}
 
@@ -932,7 +892,7 @@ void DatabaseViewer::generate3DMap()
 										cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
 									}
 
-									cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*localTransform);
+									cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*data.getLocalTransform());
 									std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
 									pcl::io::savePCDFile(name, *cloud);
 									UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
@@ -1077,25 +1037,19 @@ void DatabaseViewer::update(int value,
 			QImage imgDepth;
 			if(memory_)
 			{
-				std::vector<unsigned char> image, depth, depth2d;
+				Signature data = memory_->getSignatureData(id, true);
-				float fx, fy, cx, cy;
+				if(!data.getImageRaw().empty())
-				rtabmap::Transform localTransform;
-				memory_->getImageDepth(id, image, depth, depth2d, fx, fy, cx, cy, localTransform);
-				cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
-				cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
-				if(!image.empty())
 				{
-					img = uCvMat2QImage(imageMat);
+					img = uCvMat2QImage(data.getImageRaw());
 				}
-				if(!depth.empty())
+				if(!data.getDepthRaw().empty())
 				{
-					imgDepth = uCvMat2QImage(depthMat);
+					imgDepth = uCvMat2QImage(data.getDepthRaw());
 				}
 
-				std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
+				if(data.getWords().size())
-				if(words.size())
 				{
-					view->setFeatures(words);
+					view->setFeatures(data.getWords());
 				}
 
 				mapId = memory_->getMapId(id);
@@ -1388,84 +1342,77 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 
 	if(cloudFrom->size() == 0 && cloudTo->size() == 0)
 	{
-		float fxA, fyA, cxA, cyA;
+		Signature dataFrom, dataTo;
-		float fxB, fyB, cxB, cyB;
-		rtabmap::Transform localTransformA, localTransformB;
 
-		std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
+		dataFrom = memory_->getSignatureData(link.from(), true);
-		memory_->getImageDepth(link.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
+		UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
-		cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
+		UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
-		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
+
-		cv::Mat depth2dA = rtabmap::util3d::uncompressData(depth2dBytesA);
+		dataTo = memory_->getSignatureData(link.to(), true);
-		UASSERT(imageA.empty() || imageA.type()==CV_8UC3 || imageA.type() == CV_8UC1);
+		UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
-		UASSERT(depthA.empty() || depthA.type()==CV_8UC1 || depthA.type() == CV_16UC1 || depthA.type() == CV_32FC1);
+		UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
 
-		std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
-		memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
-		cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
-		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
-		cv::Mat depth2dB = rtabmap::util3d::uncompressData(depth2dBytesB);
 
 		//cloud 3d
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
-		if(depthA.type() == CV_8UC1)
+		if(dataFrom.getDepthRaw().type() == CV_8UC1)
 		{
-			cloudA = rtabmap::util3d::cloudFromStereoImages(
+			cloudFrom = rtabmap::util3d::cloudFromStereoImages(
-					imageA,
+					dataFrom.getImageRaw(),
-					depthA,
+					dataFrom.getDepthRaw(),
-					cxA, cyA,
+					dataFrom.getDepthCx(), dataFrom.getDepthCy(),
-					fxA, fyA,
+					dataFrom.getDepthFx(), dataFrom.getDepthFy(),
 					1);
 		}
 		else
 		{
-			cloudA = rtabmap::util3d::cloudFromDepthRGB(
+			cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
-					imageA,
+					dataFrom.getImageRaw(),
-					depthA,
+					dataFrom.getDepthRaw(),
-					cxA, cyA,
+					dataFrom.getDepthCx(), dataFrom.getDepthCy(),
-					fxA, fyA,
+					dataFrom.getDepthFx(), dataFrom.getDepthFy(),
 					1);
 		}
 
-		cloudA = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudA);
+		cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
-		cloudA = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, localTransformA);
+		cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
 
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
-		if(depthB.type() == CV_8UC1)
+		if(dataTo.getDepthRaw().type() == CV_8UC1)
 		{
-			cloudB = rtabmap::util3d::cloudFromStereoImages(
+			cloudTo = rtabmap::util3d::cloudFromStereoImages(
-					imageB,
+					dataTo.getImageRaw(),
-					depthB,
+					dataTo.getDepthRaw(),
-					cxB, cyB,
+					dataTo.getDepthCx(), dataTo.getDepthCy(),
-					fxB, fyB,
+					dataTo.getDepthFx(), dataTo.getDepthFy(),
 					1);
 		}
 		else
 		{
-			cloudB = rtabmap::util3d::cloudFromDepthRGB(
+			cloudTo = rtabmap::util3d::cloudFromDepthRGB(
-					imageB,
+					dataTo.getImageRaw(),
-					depthB,
+					dataTo.getDepthRaw(),
-					cxB, cyB,
+					dataTo.getDepthCx(), dataTo.getDepthCy(),
-					fxB, fyB,
+					dataTo.getDepthFx(), dataTo.getDepthFy(),
 					1);
 		}
 
-		cloudB = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudB);
+		cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
-		cloudB = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*localTransformB);
+		cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
 
 		//cloud 2d
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-		scanA = rtabmap::util3d::depth2DToPointCloud(depth2dA);
+		scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
-		scanB = rtabmap::util3d::depth2DToPointCloud(depth2dB);
+		scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
 		scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
 
-		if(cloudA->size())
+		if(cloudFrom->size())
 		{
-			ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudA);
+			ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
 		}
-		if(cloudB->size())
+		if(cloudTo->size())
 		{
-			ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudB);
+			ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
 		}
 		if(scanA->size())
 		{
@@ -1548,14 +1495,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("Update scans list...");
 			for(int i=0; i<ids_.size(); ++i)
 			{
-				std::vector<unsigned char> imageBytes, depthBytes, depth2dBytes;
+				Signature data = memory_->getSignatureData(ids_.at(i), false);
-				float fx, fy, cx, cy;
+				if(data.getDepth2D().size())
-				rtabmap::Transform localTransform;
-				memory_->getImageDepth(ids_.at(i), imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
-				if(depth2dBytes.size())
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-					cv::Mat depth2d = rtabmap::util3d::uncompressData(depth2dBytes);
+					cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2D());
 					cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
 					scans_.insert(std::make_pair(ids_.at(i), cloud));
 				}
@@ -1719,23 +1663,17 @@ void DatabaseViewer::refineConstraint(int from, int to)
 	double fitness = 0.0f;
 	Transform transform;
 
-	float fxA, fyA, cxA, cyA;
+	Signature dataFrom, dataTo;
-	float fxB, fyB, cxB, cyB;
+	dataFrom = memory_->getSignatureData(currentLink.from(), false);
-	rtabmap::Transform localTransformA, localTransformB;
+	dataTo = memory_->getSignatureData(currentLink.to(), false);
-
-	std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
-	memory_->getImageDepth(currentLink.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
-
-	std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
-	memory_->getImageDepth(currentLink.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
 	if(ui_->checkBox_icp_2d->isChecked())
 	{
 		//2D
-		cv::Mat oldDepth2D = util3d::uncompressData(depth2dBytesA);
+		cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2D());
-		cv::Mat newDepth2D = util3d::uncompressData(depth2dBytesB);
+		cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2D());
 
 		if(!oldDepth2D.empty() && !newDepth2D.empty())
 		{
@@ -1764,8 +1702,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
 	else
 	{
 		//3D
-		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
+		cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepth());
-		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
+		cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepth());
 
 		if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
 		{
@@ -1775,19 +1713,19 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		}
 
 		cloudA = util3d::getICPReadyCloud(depthA,
-						fxA, fyA, cxA, cyA,
+				dataFrom.getDepthFx(), dataFrom.getDepthFy(), dataFrom.getDepthCx(), dataFrom.getDepthCy(),
 				ui_->spinBox_icp_decimation->value(),
 				ui_->doubleSpinBox_icp_maxDepth->value(),
 				ui_->doubleSpinBox_icp_voxel->value(),
 				0, // no sampling
-						localTransformA);
+				dataFrom.getLocalTransform());
 		cloudB = util3d::getICPReadyCloud(depthB,
-						fxB, fyB, cxB, cyB,
+				dataTo.getDepthFx(), dataTo.getDepthFy(), dataTo.getDepthCx(), dataTo.getDepthCy(),
 				ui_->spinBox_icp_decimation->value(),
 				ui_->doubleSpinBox_icp_maxDepth->value(),
 				ui_->doubleSpinBox_icp_voxel->value(),
 				0, // no sampling
-						currentLink.transform() * localTransformB);
+				currentLink.transform() * dataTo.getLocalTransform());
 
 		if(ui_->checkBox_icp_p2plane->isChecked())
 		{
@@ -1905,26 +1843,22 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			Memory tmpMemory(parameters);
 
 			// Add signatures
-			float fxA, fyA, cxA, cyA;
+			SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
-			float fxB, fyB, cxB, cyB;
+			SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
-			rtabmap::Transform localTransformA, localTransformB;
-
-			std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
-			memory_->getImageDepth(from, imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
-			cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
-			cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
-			SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
-
-			std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
-			memory_->getImageDepth(to, imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
-			cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
-			cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
-			SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
 
+			if(from > to)
+			{
+				tmpMemory.update(dataTo);
+				tmpMemory.update(dataFrom);
+			}
+			else
+			{
 				tmpMemory.update(dataFrom);
 				tmpMemory.update(dataTo);
+			}
 
-			t = tmpMemory.computeVisualTransform(2, 1, &rejectedMsg);
+
+			t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg);
 		}
 		else
 		{

guilib/src/LoopClosureViewer.cpp

@@ -40,8 +40,6 @@ namespace rtabmap {
 
 LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
 	QWidget(parent),
-	sA_(0),
-	sB_(0),
 	decimation_(1),
 	maxDepth_(0),
 	samples_(0)
@@ -55,37 +53,21 @@ LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
 
 LoopClosureViewer::~LoopClosureViewer() {
 	delete ui_;
-	if(sA_)
-	{
-		delete sA_;
-	}
-	if(sB_)
-	{
-		delete sB_;
-	}
 }
 
-void LoopClosureViewer::setData(Signature * sA, Signature * sB)
+void LoopClosureViewer::setData(const Signature & sA, const Signature & sB)
 {
-	if(sA_)
-	{
-		delete sA_;
-	}
-	if(sB_)
-	{
-		delete sB_;
-	}
 	sA_ = sA;
 	sB_ = sB;
-	if(sA_ && sB_)
+	if(sA_.id()>0 && sB_.id()>0)
 	{
-		ui_->label_idA->setText(QString("[%1-%2]").arg(sA->id()).arg(sB->id()));
+		ui_->label_idA->setText(QString("[%1-%2]").arg(sA.id()).arg(sB.id()));
 	}
 }
 
 void LoopClosureViewer::updateView(const Transform & transform)
 {
-	if(sA_ && sB_)
+	if(sA_.id()>0 && sB_.id()>0)
 	{
 		int decimation = 1;
 		float maxDepth = 0;
@@ -113,56 +95,31 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			t = transform_;
 		}
 		else
-		{
+		{
 			t = sB_.getPose();
 		}
 
 		UDEBUG("t= %s", t.prettyPrint().c_str());
 		ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
 		if(!t.isNull())
-		{
-			util3d::CompressionThread ctiA(sA_->getImage(), true);
-			util3d::CompressionThread ctdA(sA_->getDepth(), true);
-			util3d::CompressionThread ctiB(sB_->getImage(), true);
-			util3d::CompressionThread ctdB(sB_->getDepth(), true);
-			util3d::CompressionThread ct2dA(sA_->getDepth2D(), false);
-			util3d::CompressionThread ct2dB(sB_->getDepth2D(), false);
-			ctiA.start();
-			ctdA.start();
-			ctiB.start();
-			ctdB.start();
-			ct2dA.start();
-			ct2dB.start();
-			ctiA.join();
-			ctdA.join();
-			ctiB.join();
-			ctdB.join();
-			ct2dA.join();
-			ct2dB.join();
-			cv::Mat imageA = ctiA.getUncompressedData();
-			cv::Mat depthA = ctdA.getUncompressedData();
-			cv::Mat imageB = ctiB.getUncompressedData();
-			cv::Mat depthB = ctdB.getUncompressedData();
-			cv::Mat depth2dA = ct2dA.getUncompressedData();
-			cv::Mat depth2dB = ct2dB.getUncompressedData();
 		{
 			//cloud 3d
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
 			if(sA_.getDepthRaw().type() == CV_8UC1)
 			{
-				cloudA = util3d::cloudFromStereoImages(
+				cloudA = util3d::cloudFromStereoImages(
-						imageA,
+						sA_.getImageRaw(),
-						depthA,
+						sA_.getDepthRaw(),
-						sA_->getDepthCx(), sA_->getDepthCy(),
+						sA_.getDepthCx(), sA_.getDepthCy(),
 						sA_.getDepthFx(), sA_.getDepthFy(),
 						decimation);
 			}
 			else
 			{
-				cloudA = util3d::cloudFromDepthRGB(
+				cloudA = util3d::cloudFromDepthRGB(
-						imageA,
+						sA_.getImageRaw(),
-						depthA,
+						sA_.getDepthRaw(),
-						sA_->getDepthCx(), sA_->getDepthCy(),
+						sA_.getDepthCx(), sA_.getDepthCy(),
 						sA_.getDepthFx(), sA_.getDepthFy(),
 						decimation);
 			}
@@ -176,25 +133,25 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			if(samples>0 && (int)cloudA->size() > samples)
 			{
 				cloudA = util3d::sampling<pcl::PointXYZRGB>(cloudA, samples);
-			}
+			}
 			cloudA = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, sA_.getLocalTransform());
 
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
 			if(sB_.getDepthRaw().type() == CV_8UC1)
 			{
-				cloudB = util3d::cloudFromStereoImages(
+				cloudB = util3d::cloudFromStereoImages(
-						imageB,
+						sB_.getImageRaw(),
-						depthB,
+						sB_.getDepthRaw(),
-						sB_->getDepthCx(), sB_->getDepthCy(),
+						sB_.getDepthCx(), sB_.getDepthCy(),
 						sB_.getDepthFx(), sB_.getDepthFy(),
 						decimation);
 			}
 			else
 			{
-				cloudB = util3d::cloudFromDepthRGB(
+				cloudB = util3d::cloudFromDepthRGB(
-						imageB,
+						sB_.getImageRaw(),
-						depthB,
+						sB_.getDepthRaw(),
-						sB_->getDepthCx(), sB_->getDepthCy(),
+						sB_.getDepthCx(), sB_.getDepthCy(),
 						sB_.getDepthFx(), sB_.getDepthFy(),
 						decimation);
 			}
@@ -208,15 +165,15 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			if(samples>0 && (int)cloudB->size() > samples)
 			{
 				cloudB = util3d::sampling<pcl::PointXYZRGB>(cloudB, samples);
-			}
+			}
 			cloudB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*sB_.getLocalTransform());
 
 			//cloud 2d
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-			scanA = util3d::depth2DToPointCloud(depth2dA);
+			scanA = util3d::depth2DToPointCloud(sA_.getDepth2DRaw());
 			scanB = util3d::depth2DToPointCloud(sB_.getDepth2DRaw());
 			scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
-
+
 			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));
 
 			if(cloudA->size())

guilib/src/MainWindow.cpp

@@ -189,7 +189,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
 
-	_posteriorCurve = new PdfPlotCurve("Posterior", &_imagesMap, this);
+	_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
 	_ui->posteriorPlot->addCurve(_posteriorCurve, false);
 	_ui->posteriorPlot->showLegend(false);
 	_ui->posteriorPlot->setFixedYAxis(0,1);
@@ -197,11 +197,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
 	connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
 
-	_likelihoodCurve = new PdfPlotCurve("Likelihood", &_imagesMap, this);
+	_likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
 	_ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
 	_ui->likelihoodPlot->showLegend(false);
 
-	_rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_imagesMap, this);
+	_rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
 	_ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
 	_ui->rawLikelihoodPlot->showLegend(false);
 
@@ -698,55 +698,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
 
 		// update cache
-		if(_preferencesDialog->isImagesKept())
+		Signature & signature = *_cachedSignatures.insert(stat.getSignature().id(), stat.getSignature());
-		{
+		signature.uncompressData(); // make sure data are already uncompressed
-			// images
+		UDEBUG("");
-			for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getImages().begin();
-				iter != stat.getImages().end();
-				++iter)
-			{
-				if(!iter->second.empty() && !_imagesMap.contains(iter->first))
-				{
-					_imagesMap.insert(iter->first, iter->second);
-				}
-			}
-			// depths
-			for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getDepths().begin();
-				iter != stat.getDepths().end();
-				++iter)
-			{
-				if(!iter->second.empty() && !_depthsMap.contains(iter->first))
-				{
-					float fx = uValue(stat.getDepthFxs(), iter->first, 0.0f);
-					float fy = uValue(stat.getDepthFys(), iter->first, 0.0f);
-					float cx = uValue(stat.getDepthCxs(), iter->first, 0.0f);
-					float cy = uValue(stat.getDepthCys(), iter->first, 0.0f);
-					Transform transform = uValue(stat.getLocalTransforms(), iter->first, Transform());
-					if(fx > 0.0f && fy > 0.0f && !transform.isNull())
-					{
-						_depthsMap.insert(iter->first, iter->second);
-						_depthFxsMap.insert(iter->first, fx);
-						_depthFysMap.insert(iter->first, fy);
-						_depthCxsMap.insert(iter->first, cx);
-						_depthCysMap.insert(iter->first, cy);
-						_localTransformsMap.insert(iter->first, transform);
-					}
-					else
-					{
-						UERROR("Invalid depth data for id=%d", iter->first);
-					}
-				}
-			}
-			// depths2d
-			for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getDepth2ds().begin();
-				iter != stat.getDepth2ds().end();
-				++iter)
-			{
-				if(!iter->second.empty())
-				{
-					_depths2DMap.insert(iter->first, iter->second);
-				}
-			}
 
 		// map ids
 		for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
@@ -755,7 +709,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		{
 			_mapIds.insert(iter->first, iter->second);
 		}
-		}
 
 		int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
 		int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
@@ -778,16 +731,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 		UDEBUG("time= %d ms", time.restart());
 
-		std::vector<unsigned char> refImage = uValue(stat.getImages(), stat.refImageId(), std::vector<unsigned char>());
-		std::vector<unsigned char> refDepth = uValue(stat.getDepths(), stat.refImageId(), std::vector<unsigned char>());
-		std::vector<unsigned char> refDepth2D = uValue(stat.getDepth2ds(), stat.refImageId(), std::vector<unsigned char>());
-		std::vector<unsigned char> loopImage = uValue(stat.getImages(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
-		std::vector<unsigned char> loopDepth = uValue(stat.getDepths(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
-		std::vector<unsigned char> loopDepth2D = uValue(stat.getDepth2ds(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
-
 		int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
 		float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
 		int matchId = 0;
+		cv::Mat loopImage;
+		cv::Mat loopDepth;
+		int shownLoopId = 0;
 		if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
 		{
 			bool show = true;
@@ -829,23 +778,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			if(show)
 			{
-				if(loopImage.empty())
+				shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
+				QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
+				if(iter != _cachedSignatures.end())
 				{
-					int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
+					loopImage = iter.value().getImageRaw();
-					QMap<int, std::vector<unsigned char> >::iterator iter = _imagesMap.find(id);
+					loopDepth = iter.value().getDepthRaw();
-					if(iter != _imagesMap.end())
-					{
-						loopImage = iter.value();
-					}
-				}
-				if(loopDepth.empty())
-				{
-					int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
-					QMap<int, std::vector<unsigned char> >::iterator iter = _depthsMap.find(id);
-					if(iter != _depthsMap.end())
-					{
-						loopDepth = iter.value();
-					}
 				}
 			}
 		}
@@ -854,24 +792,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 		//update image views
 		{
-			util3d::CompressionThread imageThread(refImage, true);
+			UCvMat2QImageThread qimageThread(signature.getImageRaw());
-			util3d::CompressionThread imageLoopThread(loopImage, true);
+			UCvMat2QImageThread qimageLoopThread(loopImage);
-			util3d::CompressionThread depthThread(refDepth, true);
+			UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
-			util3d::CompressionThread depthLoopThread(loopDepth, true);
+			UCvMat2QImageThread qdepthLoopThread(loopDepth);
-			imageThread.start();
-			depthThread.start();
-			imageLoopThread.start();
-			depthLoopThread.start();
-			imageThread.join();
-			depthThread.join();
-			imageLoopThread.join();
-			depthLoopThread.join();
-			UDEBUG("time= %d ms", time.restart());
-
-			UCvMat2QImageThread qimageThread(imageThread.getUncompressedData());
-			UCvMat2QImageThread qimageLoopThread(imageLoopThread.getUncompressedData());
-			UCvMat2QImageThread qdepthThread(depthThread.getUncompressedData());
-			UCvMat2QImageThread qdepthLoopThread(depthLoopThread.getUncompressedData());
 			qimageThread.start();
 			qdepthThread.start();
 			qimageLoopThread.start();
@@ -912,7 +836,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		// We use the reference image to resize the 2 views
 		_ui->imageView_source->resetZoom();
 		_ui->imageView_loopClosure->resetZoom();
-		if(refImage.empty())
+		if(signature.getImageRaw().empty())
 		{
 			_ui->imageView_source->setSceneRect(_ui->imageView_source->scene()->itemsBoundingRect());
 			_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->scene()->itemsBoundingRect());
@@ -921,14 +845,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
 
 		// do it after scaling
-		if(_ui->imageView_loopClosure->items().size() || stat.loopClosureId()>0)
+		std::multimap<int, cv::KeyPoint> loopWords;
+		if(shownLoopId)
 		{
-			this->drawKeypoints(stat.refWords(), stat.loopWords());
+			QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
-		}
+			if(iter!=_cachedSignatures.end())
-		else
 			{
-			this->drawKeypoints(stat.refWords(), std::multimap<int, cv::KeyPoint>()); //empty loop keypoints...
+				loopWords = iter->getWords();
 			}
+		}
+		this->drawKeypoints(signature.getWords(), loopWords);
 
 		if(_preferencesDialog->isImageFlipped())
 		{
@@ -938,9 +864,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 		UDEBUG("time= %d ms", time.restart());
 
-		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), stat.refWords().size());
+		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), signature.getWords().size());
-		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), stat.loopWords().size());
+		_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), loopWords.size());
-		ULOGGER_DEBUG("");
 
 		// PDF AND LIKELIHOOD
 		if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -986,7 +911,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		if(stat.poses().size())
 		{
 			// update pose only if a odometry is not received
-			updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose(), stat.constraints());
+			updateMapCloud(stat.poses(),
+					_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
+					stat.constraints());
 
 			_odometryReceived = false;
 
@@ -1008,38 +935,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				}
 				int loopNewId = stat.refImageId();
 
-				// Add to loop closure viewer if all data is saved
+				QMap<int, Signature>::iterator newIter = _cachedSignatures.find(loopNewId);
-				Signature * loopOld = new Signature(
+				QMap<int, Signature>::iterator oldIter = _cachedSignatures.find(loopOldId);
-						loopOldId,
-						loopMapId,
-						std::multimap<int, cv::KeyPoint>(),
-						std::multimap<int, pcl::PointXYZ>(),
-						Transform(),
-						_depths2DMap.value(loopOldId, std::vector<unsigned char>()),
-						_imagesMap.value(loopOldId, std::vector<unsigned char>()),
-						_depthsMap.value(loopOldId, std::vector<unsigned char>()),
-						_depthFxsMap.value(loopOldId, 0.0f),
-						_depthFysMap.value(loopOldId, 0.0f),
-						_depthCxsMap.value(loopOldId, 0.0f),
-						_depthCysMap.value(loopOldId, 0.0f),
-						_localTransformsMap.value(loopOldId, Transform()));
 
-				Signature * loopNew = new Signature(
+				if(newIter!=_cachedSignatures.end() && oldIter!=_cachedSignatures.end())
-						loopNewId,
+				{
-						refMapId,
+					_ui->widget_loopClosureViewer->setData(*oldIter, *newIter);
-						std::multimap<int, cv::KeyPoint>(),
-						std::multimap<int, pcl::PointXYZ>(),
-						loopClosureTransform,
-						_depths2DMap.value(loopNewId, std::vector<unsigned char>()),
-						_imagesMap.value(loopNewId, std::vector<unsigned char>()),
-						_depthsMap.value(loopNewId, std::vector<unsigned char>()),
-						_depthFxsMap.value(loopNewId, 0.0f),
-						_depthFysMap.value(loopNewId, 0.0f),
-						_depthCxsMap.value(loopNewId, 0.0f),
-						_depthCysMap.value(loopNewId, 0.0f),
-						_localTransformsMap.value(loopNewId, Transform()));
-
-				_ui->widget_loopClosureViewer->setData(loopOld, loopNew);
 					if(_ui->dockWidget_loopClosureViewer->isVisible())
 					{
 						UTimer loopTimer;
@@ -1047,6 +948,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 						UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
 						_ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
 					}
+				}
 
 				UDEBUG("time= %d ms", time.restart());
 			}
@@ -1078,14 +980,18 @@ void MainWindow::updateMapCloud(
 		_currentPosesMap = posesIn;
 		if(_currentPosesMap.size())
 		{
-			if(_depthsMap.size())
+			if(!_ui->actionSave_point_cloud->isEnabled() &&
+				_cachedSignatures.size() &&
+				(--_cachedSignatures.end())->getDepth().size())
 			{
 				//enable save cloud action
 				_ui->actionSave_point_cloud->setEnabled(true);
 				_ui->actionView_high_res_point_cloud->setEnabled(true);
 			}
 
-			if(_depths2DMap.size())
+			if(!_ui->actionView_scans->isEnabled() &&
+				_cachedSignatures.size() &&
+				(--_cachedSignatures.end())->getDepth2D().size())
 			{
 				_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1147,11 +1053,15 @@ void MainWindow::updateMapCloud(
 					_ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 					_ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 				}
-				else if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
+				else if(_cachedSignatures.contains(iter->first))
+				{
+					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
+					if(!jter->getImageRaw().empty() && !jter->getDepthRaw().empty())
 					{
 						this->createAndAddCloudToMap(iter->first, iter->second);
 					}
 				}
+			}
 			else if(viewerClouds.contains(cloudName))
 			{
 				UDEBUG("Hide cloud %s", cloudName.c_str());
@@ -1178,10 +1088,14 @@ void MainWindow::updateMapCloud(
 					_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 					_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
-				else if(_depths2DMap.contains(iter->first))
+				else if(_cachedSignatures.contains(iter->first))
+				{
+					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
+					if(!jter->getDepth2DRaw().empty())
 					{
 						this->createAndAddScanToMap(iter->first, iter->second);
 					}
+				}
 				if(!_preferencesDialog->isScansShown(0))
 				{
 					UDEBUG("Hide scan %s", scanName.c_str());
@@ -1302,15 +1216,23 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 		UERROR("Cloud %d already added to map.", nodeId);
 		return;
 	}
+
+	QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
+	if(iter == _cachedSignatures.end())
+	{
+		UERROR("Node %d is not in the cache.", nodeId);
+		return;
+	}
+
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 	cloud = createCloud(nodeId,
-			util3d::uncompressImage(_imagesMap.value(nodeId)),
+			iter->getImageRaw(),
-			util3d::uncompressImage(_depthsMap.value(nodeId)),
+			iter->getDepthRaw(),
-			_depthFxsMap.value(nodeId),
+			iter->getDepthFx(),
-			_depthFysMap.value(nodeId),
+			iter->getDepthFy(),
-			_depthCxsMap.value(nodeId),
+			iter->getDepthCx(),
-			_depthCysMap.value(nodeId),
+			iter->getDepthCy(),
-			_localTransformsMap.value(nodeId),
+			iter->getLocalTransform(),
 			Transform::getIdentity(),
 			_preferencesDialog->getCloudVoxelSize(0),
 			_preferencesDialog->getCloudDecimation(0),
@@ -1318,6 +1240,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 
 	if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
 	{
+		UTimer timer;
 		float cellSize = _preferencesDialog->getGridMapResolution();
 		float groundNormalMaxAngle = M_PI_4;
 		int minClusterSize = 20;
@@ -1326,6 +1249,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 		{
 			_occupancyLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 		}
+		UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
 	}
 
 	if(_preferencesDialog->isCloudMeshing())
@@ -1396,9 +1320,16 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
 		UERROR("Scan %d already added to map.", nodeId);
 		return;
 	}
+
+	QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
+	if(iter == _cachedSignatures.end())
+	{
+		UERROR("Node %d is not in the cache.", nodeId);
+		return;
+	}
+
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-	cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(nodeId));
+	cloud = util3d::depth2DToPointCloud(iter->getDepth2DRaw());
-	cloud = util3d::depth2DToPointCloud(depth2d);
 	QColor color = Qt::red;
 	int mapId = _mapIds.value(nodeId, -1);
 	if(mapId >= 0)
@@ -1422,7 +1353,7 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 	if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
 	{
 		QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
-		if(_preferencesDialog->isCloudsShown(0) && _depthsMap.contains(nodeId))
+		if(_preferencesDialog->isCloudsShown(0) && _cachedSignatures.contains(nodeId))
 		{
 			std::string cloudName = uFormat("cloud%d", nodeId);
 			if(visible && !viewerClouds.contains(cloudName))
@@ -1440,7 +1371,7 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 			}
 		}
 
-		if(_preferencesDialog->isScansShown(0) && _depths2DMap.contains(nodeId))
+		if(_preferencesDialog->isScansShown(0) && _cachedSignatures.contains(nodeId))
 		{
 			std::string scanName = uFormat("scan%d", nodeId);
 			if(visible && !viewerClouds.contains(scanName))
@@ -1501,72 +1432,31 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 	{
 
 		UINFO("Received map!");
-		UINFO(" images = %d", event.getImages().size());
+		UINFO(" signatures = %d", event.getSignatures().size());
-		UINFO(" depths = %d", event.getDepths().size());
-		UINFO(" depths2d = %d", event.getDepths2d().size());
-		UINFO(" depthFxs = %d", event.getDepthFxs().size());
-		UINFO(" depthFys = %d", event.getDepthFys().size());
-		UINFO(" depthCxs = %d", event.getDepthCxs().size());
-		UINFO(" depthCys = %d", event.getDepthCys().size());
 		UINFO(" map ids = %d", event.getMapIds().size());
-		UINFO(" localTransforms = %d", event.getLocalTransforms().size());
 		UINFO(" poses = %d", event.getPoses().size());
 		UINFO(" constraints = %d", event.getConstraints().size());
 
-		_initProgressDialog->appendText("Inserting data in the cache...");
+		_initProgressDialog->setMaximumSteps(event.getSignatures().size());
+		_initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
 
-		for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getImages().begin();
+		int addedSignatures = 0;
-			iter!=event.getImages().end();
+		for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
+			iter!=event.getSignatures().end();
 			++iter)
 		{
-			_imagesMap.insert(iter->first, iter->second);
+			if(!_cachedSignatures.contains(iter->first))
-		}
-		_initProgressDialog->appendText(tr("Inserted %1 images.").arg(_imagesMap.size()));
-		_initProgressDialog->incrementStep();
-
-		for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths().begin();
-			iter!=event.getDepths().end();
-			++iter)
 			{
-			_depthsMap.insert(iter->first, iter->second);
+				QMap<int, Signature>::iterator inserted = _cachedSignatures.insert(iter->first, iter->second);
-		}
+				//uncompress data if required
-		_initProgressDialog->appendText(tr("Inserted %1 depth images.").arg(_depthsMap.size()));
+				if(inserted->getImageRaw().empty() && inserted->getImage().size())
-		_initProgressDialog->incrementStep();
-
-		for(std::map<int, float>::const_iterator iter = event.getDepthFxs().begin();
-			iter!=event.getDepthFxs().end();
-			++iter)
 				{
-			_depthFxsMap.insert(iter->first, iter->second);
+					inserted->uncompressData();
+					++addedSignatures;
 				}
-		_initProgressDialog->appendText(tr("Inserted %1 depth fx parameters.").arg(_depthFxsMap.size()));
-		_initProgressDialog->incrementStep();
-
-		for(std::map<int, float>::const_iterator iter = event.getDepthFys().begin();
-			iter!=event.getDepthFys().end();
-			++iter)
-		{
-			_depthFysMap.insert(iter->first, iter->second);
 			}
-		_initProgressDialog->appendText(tr("Inserted %1 depth fy parameters.").arg(_depthFysMap.size()));
-		_initProgressDialog->incrementStep();
-
-		for(std::map<int, float>::const_iterator iter = event.getDepthCxs().begin();
-			iter!=event.getDepthCxs().end();
-			++iter)
-		{
-			_depthCxsMap.insert(iter->first, iter->second);
 		}
-		_initProgressDialog->appendText(tr("Inserted %1 depth cx parameters.").arg(_depthCxsMap.size()));
+		_initProgressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
-		_initProgressDialog->incrementStep();
-
-		for(std::map<int, float>::const_iterator iter = event.getDepthCys().begin();
-			iter!=event.getDepthCys().end();
-			++iter)
-		{
-			_depthCysMap.insert(iter->first, iter->second);
-		}
-		_initProgressDialog->appendText(tr("Inserted %1 depth cy parameters.").arg(_depthCysMap.size()));
 		_initProgressDialog->incrementStep();
 
 		for(std::map<int, int>::const_iterator iter = event.getMapIds().begin();
@@ -1578,24 +1468,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		_initProgressDialog->appendText(tr("Inserted %1 map ids").arg(_mapIds.size()));
 		_initProgressDialog->incrementStep();
 
-		for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths2d().begin();
-			iter!=event.getDepths2d().end();
-			++iter)
-		{
-			_depths2DMap.insert(iter->first, iter->second);
-		}
-		_initProgressDialog->appendText(tr("Inserted %1 laser scans.").arg(_depths2DMap.size()));
-		_initProgressDialog->incrementStep();
-
-		for(std::map<int, Transform>::const_iterator iter = event.getLocalTransforms().begin();
-			iter!=event.getLocalTransforms().end();
-			++iter)
-		{
-			_localTransformsMap.insert(iter->first, iter->second);
-		}
-		_initProgressDialog->appendText(tr("Inserted %1 local transforms.").arg(_localTransformsMap.size()));
-		_initProgressDialog->incrementStep();
-
 		_initProgressDialog->appendText("Inserting data in the cache... done.");
 
 		if(event.getPoses().size())
@@ -2779,15 +2651,8 @@ void MainWindow::downloadPoseGraph()
 
 void MainWindow::clearTheCache()
 {
-	_imagesMap.clear();
+	_cachedSignatures.clear();
-	_depthsMap.clear();
-	_depths2DMap.clear();
-	_depthFxsMap.clear();
-	_depthFysMap.clear();
-	_depthCxsMap.clear();
-	_depthCysMap.clear();
 	_mapIds.clear();
-	_localTransformsMap.clear();
 	_createdClouds.clear();
 	_createdScans.clear();
 	_occupancyLocalMaps.clear();
@@ -3854,19 +3719,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 		bool inserted = false;
 		if(!iter->second.isNull())
 		{
-			if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
+			if(_cachedSignatures.contains(iter->first))
 			{
+				QMap<int, Signature>::const_iterator jter = _cachedSignatures.find(iter->first);
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 				if(regenerateClouds)
 				{
 					cloud = createCloud(iter->first,
-							util3d::uncompressImage(_imagesMap.value(iter->first)),
+							jter->getImageRaw(),
-							util3d::uncompressImage(_depthsMap.value(iter->first)),
+							jter->getDepthRaw(),
-							_depthFxsMap.value(iter->first),
+							jter->getDepthFx(),
-							_depthFysMap.value(iter->first),
+							jter->getDepthFy(),
-							_depthCxsMap.value(iter->first),
+							jter->getDepthCx(),
-							_depthCysMap.value(iter->first),
+							jter->getDepthCy(),
-							_localTransformsMap.value(iter->first),
+							jter->getLocalTransform(),
 							iter->second,
 							regenerateVoxelSize,
 							regenerateDecimation,
@@ -3937,19 +3803,20 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
 		bool inserted = false;
 		if(!iter->second.isNull())
 		{
-			if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
+			if(_cachedSignatures.contains(iter->first))
 			{
+				QMap<int, Signature>::const_iterator jter = _cachedSignatures.find(iter->first);
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 				if(regenerateClouds)
 				{
 					cloud = createCloud(iter->first,
-							util3d::uncompressImage(_imagesMap.value(iter->first)),
+							jter->getImageRaw(),
-							util3d::uncompressImage(_depthsMap.value(iter->first)),
+							jter->getDepthRaw(),
-							_depthFxsMap.value(iter->first),
+							jter->getDepthFx(),
-							_depthFysMap.value(iter->first),
+							jter->getDepthFy(),
-							_depthCxsMap.value(iter->first),
+							jter->getDepthCx(),
-							_depthCysMap.value(iter->first),
+							jter->getDepthCy(),
-							_localTransformsMap.value(iter->first),
+							jter->getLocalTransform(),
 							Transform::getIdentity(),
 							regenerateVoxelSize,
 							regenerateDecimation,

guilib/src/PdfPlot.cpp

@@ -35,7 +35,7 @@ namespace rtabmap {
 PdfPlotItem::PdfPlotItem(float dataX, float dataY, float width, int childCount) :
 	UPlotItem(dataX, dataY, width),
 	_img(0),
-	_imagesRef(0),
+	_signaturesRef(0),
 	_text(0)
 {
 	setLikelihood(dataX, dataY, childCount);
@@ -66,13 +66,13 @@ void PdfPlotItem::showDescription(bool shown)
 	}
 	if(shown)
 	{
-		if(!_img && _imagesRef)
+		if(!_img && _signaturesRef)
 		{
 			QImage img;
-			QMap<int, std::vector<unsigned char> >::const_iterator iter = _imagesRef->find(int(this->data().x()));
+			QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
-			if(iter != _imagesRef->constEnd())
+			if(iter != _signaturesRef->constEnd() && !iter.value().getImageRaw().empty())
 			{
-				img = uCvMat2QImage(util3d::uncompressImage(iter.value()));
+				img = uCvMat2QImage(iter.value().getImageRaw());
 				QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
 				_img = new QGraphicsPixmapItem(scaled, this);
 				_img->setVisible(false);
@@ -111,9 +111,9 @@ void PdfPlotItem::showDescription(bool shown)
 
 
 
-PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef = 0, QObject * parent) :
+PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef = 0, QObject * parent) :
 	UPlotCurve(name, parent),
-	_imagesMapRef(imagesMapRef)
+	_signaturesMapRef(signaturesMapRef)
 {
 
 }
@@ -139,7 +139,7 @@ void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int
 		while(margin < 0)
 		{
 			PdfPlotItem * newItem = new PdfPlotItem(0, 0, 2, 0);
-			newItem->setImagesRef(_imagesMapRef);
+			newItem->setSignaturesRef(_signaturesMapRef);
 			this->_addValue(newItem);
 			++margin;
 		}

guilib/src/PdfPlot.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <utilite/UPlot.h>
 #include "opencv2/opencv.hpp"
+#include "rtabmap/core/Signature.h"
 
 namespace rtabmap {
 
@@ -40,7 +41,7 @@ public:
 	virtual ~PdfPlotItem();
 
 	void setLikelihood(int id, float value, int childCount);
-	void setImagesRef(const QMap<int, std::vector<unsigned char> > * imagesRef) {_imagesRef = imagesRef;}
+	void setSignaturesRef(const QMap<int, Signature> * signaturesRef) {_signaturesRef = signaturesRef;}
 
 	float value() const {return this->data().y();}
 	int id() const {return this->data().x();}
@@ -51,7 +52,7 @@ protected:
 private:
 	QGraphicsPixmapItem * _img;
 	int _childCount;
-	const QMap<int, std::vector<unsigned char> > * _imagesRef;
+	const QMap<int, Signature> * _signaturesRef;
 	QGraphicsTextItem * _text;
 
 };
@@ -61,14 +62,14 @@ class PdfPlotCurve : public UPlotCurve
 	Q_OBJECT
 
 public:
-	PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef, QObject * parent = 0);
+	PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef, QObject * parent = 0);
 	virtual ~PdfPlotCurve();
 
 	virtual void clear();
 	void setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap);
 
 private:
-	const QMap<int, std::vector<unsigned char> > * _imagesMapRef;
+	const QMap<int, Signature> * _signaturesMapRef;
 };
 
 }

guilib/src/PreferencesDialog.cpp

@@ -203,6 +203,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_map_fillEmptySpace, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->spinbox_map_fillEmptyRadius, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_map_occupancyFrom3DCloud, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	//Logging panel
 	connect(_ui->comboBox_loggerLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
@@ -287,7 +289,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	// Map objects name with the corresponding parameter key, needed for the addParameter() slots
 	//Rtabmap
 	_ui->groupBox_publishing->setObjectName(Parameters::kRtabmapPublishStats().c_str());
-	_ui->general_checkBox_publishRawData->setObjectName(Parameters::kRtabmapPublishImage().c_str());
+	_ui->general_checkBox_publishRawData->setObjectName(Parameters::kRtabmapPublishLastSignature().c_str());
 	_ui->general_checkBox_publishPdf->setObjectName(Parameters::kRtabmapPublishPdf().c_str());
 	_ui->general_checkBox_publishLikelihood->setObjectName(Parameters::kRtabmapPublishLikelihood().c_str());
 	_ui->general_checkBox_statisticLogsBufferedInRAM->setObjectName(Parameters::kRtabmapStatisticLogsBufferedInRAM().c_str());
@@ -334,7 +336,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_bayes_predictionLC, SIGNAL(textChanged(const QString &)), this, SLOT(updatePredictionPlot()));
 
 	//Keypoint-based
-	_ui->checkBox_kp_publishKeypoints->setObjectName(Parameters::kKpPublishKeypoints().c_str());
 	_ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str());
 	_ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str());
 	_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
@@ -819,7 +820,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
 		_ui->checkBox_map_fillEmptySpace->setChecked(true);
 		_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
-		_ui->checkBox_map_fillEmptyRadius->setValue(0);
+		_ui->spinbox_map_fillEmptyRadius->setValue(0);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1056,7 +1057,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
 	_ui->checkBox_map_fillEmptySpace->setChecked(settings.value("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked()).toBool());
 	_ui->checkBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked()).toBool());
-	_ui->checkBox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->checkBox_map_fillEmptyRadius->value()).toInt());
+	_ui->spinbox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value()).toInt());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
 
 	settings.endGroup(); // General
@@ -1298,7 +1299,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
 	settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
 	settings.setValue("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked());
 	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked());
-	settings.setValue("gridMapFillEmptyRadius", _ui->checkBox_map_fillEmptyRadius->value());
+	settings.setValue("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value());
 	settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
 	settings.endGroup(); // General
 
@@ -2665,7 +2666,7 @@ bool PreferencesDialog::isGridMapFrom3DCloud() const
 }
 int PreferencesDialog::getGridMapFillEmptyRadius() const
 {
-	return _ui->checkBox_map_fillEmptyRadius->value();
+	return _ui->spinbox_map_fillEmptyRadius->value();
 }
 double PreferencesDialog::getGridMapOpacity() const
 {

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>744</width>
-            <height>978</height>
+            <height>1047</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -558,7 +558,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                           </widget>
                          </item>
                          <item row="5" column="0">
-                          <widget class="QSpinBox" name="checkBox_map_fillEmptyRadius">
+                          <widget class="QSpinBox" name="spinbox_map_fillEmptyRadius">
                            <property name="suffix">
                             <string> cells</string>
                            </property>
@@ -2422,7 +2422,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="0" column="1">
                         <widget class="QLabel" name="label_91">
                          <property name="text">
-                          <string>Publish raw sensor data.</string>
+                          <string>Publish signature data.</string>
                          </property>
                         </widget>
                        </item>
@@ -2460,23 +2460,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
-                        <widget class="QLabel" name="label_144">
-                         <property name="text">
-                          <string>Publish visual words.</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="3" column="0">
-                        <widget class="QCheckBox" name="checkBox_kp_publishKeypoints">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>