Increased version to 0.20.5

Refactored how features are stored in Signature (significative memory optimization, causing major refactoring in Memory, RegistrationVis, OdometryF2M)
FLANN: optimized memory usage when Kp/IncrementalFlann is false
Added memory usage functions
Added statistics Loop/Visual_inliers_ratio/ and Memory/RAM_estimated/MB
EpipolarGeometry: templated findPairs functions
graph::filterLinks: added inverted option
LocalBundleOnLoopClosure: Force to use only neighbor links
MainWindow: fixed max depth filtering for map's features
Rtabmap::getSignatureCopy() fixed links not returned
Added UPlot::getAllCurveDataAsText() function.
DbViewer: fixed features not rendered in right view when failing ro refine a constraint
report: added --export and --export_prefix options (to export figures data)

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 5)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/BayesFilter.h

@@ -61,6 +61,8 @@ public:
 
 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
+	unsigned long getMemoryUsed() const;
+
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,

corelib/include/rtabmap/core/DBDriver.h

@@ -131,7 +131,7 @@ public:
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
-	long getMemoryUsed() const; // In bytes
+	unsigned long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
 	long getNodesMemoryUsed() const;
 	long getLinksMemoryUsed() const;
@@ -188,7 +188,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
-	virtual long getMemoryUsedQuery() const = 0; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
 	virtual long getNodesMemoryUsedQuery() const = 0;
 	virtual long getLinksMemoryUsedQuery() const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -54,7 +54,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
-	virtual long getMemoryUsedQuery() const; // In bytes
+	virtual unsigned long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
 	virtual long getNodesMemoryUsedQuery() const;
 	virtual long getLinksMemoryUsedQuery() const;
@@ -189,7 +189,7 @@ protected:
 	std::string _version;
 
 private:
-	long _memoryUsedEstimate;
+	unsigned long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <pcl/point_cloud.h>
@@ -91,41 +92,133 @@ public:
 	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
 	 * realPairsCount = 4
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::map<int, cv::KeyPoint> & wordsA,
+			const std::map<int, T> & wordsA,
-			const std::map<int, cv::KeyPoint> & wordsB,
+			const std::map<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true);
+			bool ignoreNegativeIds = true)
+	{
+		int realPairsCount = 0;
+		pairs.clear();
+		for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
+			{
+				std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+				if(ptB != wordsB.end())
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairs(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
+			const std::multimap<int, T> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
+			const std::multimap<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true);
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		typename std::multimap<int, T>::const_iterator iterA;
+		typename std::multimap<int, T>::const_iterator iterB;
+		pairs.clear();
+		int realPairsCount = 0;
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
+			{
+				iterA = wordsA.find(*i);
+				iterB = wordsB.find(*i);
+				while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
+					++iterA;
+					++iterB;
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsUnique(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
+			const std::multimap<int, T> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
+			const std::multimap<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true);
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		int realPairsCount = 0;
+		pairs.clear();
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *i);
+				std::list<T> ptsB = uValues(wordsB, *i);
+				if(ptsA.size() == 1 && ptsB.size() == 1)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
+					++realPairsCount;
+				}
+				else if(ptsA.size()>1 && ptsB.size()>1)
+				{
+					// just update the count
+					realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
+	template<typename T>
 	static int findPairsAll(
-			const std::multimap<int, cv::KeyPoint> & wordsA,
+			const std::multimap<int, T> & wordsA,
-			const std::multimap<int, cv::KeyPoint> & wordsB,
+			const std::multimap<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true);
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		pairs.clear();
+		int realPairsCount = 0;;
+		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *iter);
+				std::list<T> ptsB = uValues(wordsB, *iter);
+
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+				for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+				{
+					for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+					{
+						pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
+					}
+				}
+			}
+		}
+		return realPairsCount;
+	}
 
 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)

corelib/include/rtabmap/core/FlannIndex.h

@@ -43,8 +43,8 @@ public:
 	void release();
 	unsigned int indexedFeatures() const;
 
-	// return KB
+	// return Bytes
-	unsigned int memoryUsed() const;
+	unsigned long memoryUsed() const;
 
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
@@ -74,7 +74,7 @@ public:
 	int featuresType() const {return featuresType_;}
 	int featuresDim() const {return featuresDim_;}
 
-	unsigned int addPoints(const cv::Mat & features);
+	std::vector<unsigned int> addPoints(const cv::Mat & features);
 
 	void removePoint(unsigned int index);
 

corelib/include/rtabmap/core/Graph.h

@@ -155,12 +155,20 @@ std::list<Link> RTABMAP_EXP findLinks(
 
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType);
+		Link::Type filteredType,
+		bool inverted = false);
 
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(

corelib/include/rtabmap/core/Memory.h

@@ -199,9 +199,10 @@ public:
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
 	void getNodeWordsAndGlobalDescriptors(int nodeId,
-			std::multimap<int, cv::KeyPoint> & words,
+			std::multimap<int, int> & words,
-			std::multimap<int, cv::Point3f> & words3,
+			std::vector<cv::KeyPoint> & wordsKpts,
-			std::multimap<int, cv::Mat> & wordsDescriptors,
+			std::vector<cv::Point3f> & words3,
+			cv::Mat & wordsDescriptors,
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
@@ -225,6 +226,7 @@ public:
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+	unsigned long getMemoryUsed() const; //Bytes
 
 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
 

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -104,6 +104,8 @@ public:
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
 
+	unsigned long getMemoryUsed() const;
+
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -75,6 +75,7 @@ public:
 
 	// RegistrationVis
 	int inliers;
+	float inliersRatio;
 	float inliersMeanDistance;
 	float inliersDistribution;
 	std::vector<int> inliersIDs;

corelib/include/rtabmap/core/Rtabmap.h

@@ -136,11 +136,9 @@ public:
 	std::map<int, int> getWeights() const;
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
-	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
 	bool isInSTM(int locationId) const;
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
-	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
 	Transform getPose(int locationId) const;

corelib/include/rtabmap/core/SensorData.h

@@ -275,7 +275,7 @@ public:
 	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
 	const Landmarks & landmarks() const {return _landmarks;}
 
-	long getMemoryUsed() const; // Return memory usage in Bytes
+	unsigned long getMemoryUsed() const; // Return memory usage in Bytes
 	/**
 	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
 	 * Raw data are kept is set.

corelib/include/rtabmap/core/Signature.h

@@ -104,19 +104,18 @@ public:
 
 	//visual words stuff
 	void removeAllWords();
-	void removeWord(int wordId);
 	void changeWordsRef(int oldWordId, int activeWordId);
-	void setWords(const std::multimap<int, cv::KeyPoint> & words);
+	void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
 	bool isEnabled() const {return _enabled;}
 	void setEnabled(bool enabled) {_enabled = enabled;}
-	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
+	const std::multimap<int, int> & getWords() const {return _words;}
+	const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
 	int getInvalidWordsCount() const {return _invalidWordsCount;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
+	const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
-	void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
+	void setWordsDescriptors(const cv::Mat & descriptors);
 
 	//metric stuff
-	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
 	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
@@ -129,7 +128,7 @@ public:
 		_velocity[5]=vyaw;
 	}
 
-	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
+	const std::vector<cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
@@ -138,7 +137,7 @@ public:
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
 
-	long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+	unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
 
 private:
 	int _id;
@@ -155,9 +154,10 @@ private:
 	// Contains all words (Some can be duplicates -> if a word appears 2
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
-	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
+	std::multimap<int, int> _words; // word <id, keypoint index>
-	std::multimap<int, cv::Point3f> _words3; // word <id, point> // in base_link frame (localTransform applied))
+	std::vector<cv::KeyPoint> _wordsKpts;
-	std::multimap<int, cv::Mat> _wordsDescriptors;
+	std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
+	cv::Mat _wordsDescriptors;
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
 	int _invalidWordsCount;

corelib/include/rtabmap/core/Statistics.h

@@ -65,6 +65,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Map_id,);
 	RTABMAP_STATS(Loop, Visual_words,);
 	RTABMAP_STATS(Loop, Visual_inliers,);
+	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
 	RTABMAP_STATS(Loop, Distance_since_last_loc,);
 	RTABMAP_STATS(Loop, Last_id,);
@@ -149,6 +150,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 	RTABMAP_STATS(Memory, RAM_usage, MB);
+	RTABMAP_STATS(Memory, RAM_estimated, MB);
 	RTABMAP_STATS(Memory, Triangulated_points, );
 
 	RTABMAP_STATS(Timing, Memory_update, ms);

corelib/include/rtabmap/core/VWDictionary.h

@@ -100,7 +100,8 @@ public:
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
 	unsigned int getIndexedWordsCount() const;
-	unsigned int getIndexMemoryUsed() const;
+	unsigned int getIndexMemoryUsed() const; // KB
+	unsigned long getMemoryUsed(bool estimate = true) const; //Bytes
 	bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized
 	bool isIncremental() const {return _incrementalDictionary;}
 	bool isIncrementalFlann() const {return _incrementalFlann;}

corelib/include/rtabmap/core/VisualWord.h

@@ -43,6 +43,7 @@ public:
 
 	void addRef(int signatureId);
 	int removeAllRef(int signatureId);
+	unsigned long getMemoryUsed() const;
 
 	int getTotalReferences() const {return _totalReferences;}
 	int id() const {return _id;}

corelib/src/BayesFilter.cpp

@@ -417,6 +417,20 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 	return prediction;
 }
 
+unsigned long BayesFilter::getMemoryUsed() const
+{
+	long memoryUsage = sizeof(BayesFilter);
+	memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, float>);
+	memoryUsage += _prediction.total() * _prediction.elemSize();
+	memoryUsage += _predictionLC.size() * sizeof(double);
+	memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::map<int, int> >);
+	for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
+	{
+		memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
+	}
+	return memoryUsage;
+}
+
 void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
 {
 	UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);

corelib/src/DBDriver.cpp

@@ -107,9 +107,9 @@ bool DBDriver::isConnected() const
 }
 
 // In bytes
-long DBDriver::getMemoryUsed() const
+unsigned long DBDriver::getMemoryUsed() const
 {
-	long bytes;
+	unsigned long bytes;
 	_dbSafeAccessMutex.lock();
 	bytes = getMemoryUsedQuery();
 	_dbSafeAccessMutex.unlock();

corelib/src/DBDriverSqlite3.cpp

@@ -486,7 +486,7 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
 	}
 }
 
-long DBDriverSqlite3::getMemoryUsedQuery() const
+unsigned long DBDriverSqlite3::getMemoryUsedQuery() const
 {
 	if(_dbInMemory)
 	{
@@ -3079,9 +3079,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			const void * descriptor = 0;
 			int dRealSize = 0;
 			cv::KeyPoint kpt;
-			std::multimap<int, cv::KeyPoint> visualWords;
+			std::multimap<int, int> visualWords;
-			std::multimap<int, cv::Point3f> visualWords3;
+			std::vector<cv::KeyPoint> visualWordsKpts;
-			std::multimap<int, cv::Mat> descriptors;
+			std::vector<cv::Point3f> visualWords3;
+			cv::Mat descriptors;
 			bool allWords3NaN = true;
 			cv::Point3f depth(0,0,0);
 
@@ -3131,8 +3132,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					depth.z = sqlite3_column_double(ppStmt, index++);
 				}
 
-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
+				visualWordsKpts.push_back(kpt);
-				visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
+				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
+				visualWords3.push_back(depth);
 
 				if(allWords3NaN && util3d::isFinite(depth))
 				{
@@ -3165,7 +3167,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 						memcpy(d.data, descriptor, dRealSize);
 
-						descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
+						descriptors.push_back(d);
 					}
 				}
 
@@ -3179,13 +3181,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			}
 			else
 			{
-				(*iter)->setWords(visualWords);
+				if(allWords3NaN)
-				if(!allWords3NaN)
 				{
-					(*iter)->setWords3(visualWords3);
+					visualWords3.clear();
 				}
-				(*iter)->setWordsDescriptors(descriptors);
+				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
-				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
 			}
 
 			//reset
@@ -4275,30 +4276,25 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
+			UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
 			UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
-			UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
+			UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
 
-			std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
+			for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
-			std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
-			for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 			{
 				cv::Point3f pt(nanFloat,nanFloat,nanFloat);
-				if(p!=(*i)->getWords3().end())
+				if(!(*i)->getWords3().empty())
 				{
-					UASSERT(w->first == p->first); // must be same id!
+					pt = (*i)->getWords3()[w->second];
-					pt = p->second;
-					++p;
 				}
 
 				cv::Mat descriptor;
-				if(d!=(*i)->getWordsDescriptors().end())
+				if(!(*i)->getWordsDescriptors().empty())
 				{
-					UASSERT(w->first == d->first); // must be same id!
+					descriptor = (*i)->getWordsDescriptors().row(w->second);
-					descriptor = d->second;
-					++d;
 				}
 
-				stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
+				stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
 			}
 		}
 		// Finalize (delete) the statement

corelib/src/DBReader.cpp

@@ -510,23 +510,9 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					data.gps().stamp()!=0.0?1:0,
 					gravityTransform.isNull()?0:1);
 
-			cv::Mat descriptors;
+			cv::Mat descriptors = s->getWordsDescriptors().clone();
-			if(!s->getWordsDescriptors().empty())
+			const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
-			{
+			const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
-				descriptors = cv::Mat(
-					s->getWordsDescriptors().size(),
-					s->getWordsDescriptors().begin()->second.cols,
-					s->getWordsDescriptors().begin()->second.type());
-				int i=0;
-				for(std::multimap<int, cv::Mat>::const_iterator iter=s->getWordsDescriptors().begin();
-					iter!=s->getWordsDescriptors().end();
-					++iter, ++i)
-				{
-					iter->second.copyTo(descriptors.row(i));
-				}
-			}
-			std::vector<cv::KeyPoint> keypoints = uValues(s->getWords());
-			std::vector<cv::Point3f> keypoints3D = uValues(s->getWords3());
 			if(!keypoints.empty() &&
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))

corelib/src/EpipolarGeometry.cpp

@@ -70,15 +70,21 @@ bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
 	}
 	ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
 
-	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+	std::list<std::pair<int, std::pair<int, int> > > pairsId;
 
-	findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
+	findPairsUnique(ssA->getWords(), ssB->getWords(), pairsId);
 
-	if((int)pairs.size()<_matchCountMinAccepted)
+	if((int)pairsId.size()<_matchCountMinAccepted)
 	{
 		return false;
 	}
 
+	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+	for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
+	{
+		pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->getWordsKpts()[iter->second.first], ssB->getWordsKpts()[iter->second.second])));
+	}
+
 	std::vector<uchar> status;
 	cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
 
@@ -406,136 +412,6 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 	return K.inv().t()*E*K.inv();
 }
 
-/**
- * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
- * realPairsCount = 4
- */
-int EpipolarGeometry::findPairs(
-		const std::map<int, cv::KeyPoint> & wordsA,
-		const std::map<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	int realPairsCount = 0;
-	pairs.clear();
-	for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
-		{
-			std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
-			if(ptB != wordsB.end())
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
-				++realPairsCount;
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
-	std::multimap<int, cv::KeyPoint>::const_iterator iterB;
-	pairs.clear();
-	int realPairsCount = 0;
-	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
-		{
-			iterA = wordsA.find(*i);
-			iterB = wordsB.find(*i);
-			while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
-				++iterA;
-				++iterB;
-				++realPairsCount;
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairsUnique(
-		const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	int realPairsCount = 0;
-	pairs.clear();
-	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
-		{
-			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
-			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
-			if(ptsA.size() == 1 && ptsB.size() == 1)
-			{
-				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
-				++realPairsCount;
-			}
-			else if(ptsA.size()>1 && ptsB.size()>1)
-			{
-				// just update the count
-				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-			}
-		}
-	}
-	return realPairsCount;
-}
-
-/**
- * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
- * realPairsCount = 5
- */
-int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
-		const std::multimap<int, cv::KeyPoint> & wordsB,
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
-		bool ignoreInvalidIds)
-{
-	UTimer timer;
-	timer.start();
-	const std::list<int> & ids = uUniqueKeys(wordsA);
-	pairs.clear();
-	int realPairsCount = 0;;
-	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
-	{
-		if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
-		{
-			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
-			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
-
-			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-
-			for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
-			{
-				for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
-				{
-					pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
-				}
-			}
-		}
-	}
-	ULOGGER_DEBUG("time = %f", timer.ticks());
-	return realPairsCount;
-}
-
-
 
 /**
  source = SfM toy library: https://github.com/royshil/SfM-Toy-Library

corelib/src/FlannIndex.cpp

@@ -107,32 +107,36 @@ unsigned int FlannIndex::indexedFeatures() const
 	}
 }
 
-// return KB
+// return Bytes
-unsigned int FlannIndex::memoryUsed() const
+unsigned long FlannIndex::memoryUsed() const
 {
 	if(!index_)
 	{
 		return 0;
 	}
+	unsigned long memoryUsage = sizeof(FlannIndex);
+	memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, cv::Mat>);
+	memoryUsage += sizeof(std::list<int>) + removedIndexes_.size() * sizeof(int);
 	if(featuresType_ == CV_8UC1)
 	{
-		return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
+		memoryUsage += ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory();
 	}
 	else
 	{
 		if(useDistanceL1_)
 		{
-			return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory();
 		}
 		else if(featuresDim_ <= 3)
 		{
-			return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory();
 		}
 		else
 		{
-			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+			memoryUsage += ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory();
 		}
 	}
+	return memoryUsage;
 }
 
 void FlannIndex::buildLinearIndex(
@@ -177,10 +181,21 @@ void FlannIndex::buildLinearIndex(
 		}
 	}
 
-	// incremental FLANN
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
 		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
+		nextIndex_ += features.rows;
-	nextIndex_ = features.rows;
+	}
 	UDEBUG("");
 }
 
@@ -227,10 +242,21 @@ void FlannIndex::buildKDTreeIndex(
 		}
 	}
 
-	// incremental FLANN
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
 		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
+		nextIndex_ += features.rows;
-	nextIndex_ = features.rows;
+	}
 	UDEBUG("");
 }
 
@@ -278,10 +304,21 @@ void FlannIndex::buildKDTreeSingleIndex(
 		}
 	}
 
-	// incremental FLANN
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
 		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
+		nextIndex_ += features.rows;
-	nextIndex_ = features.rows;
+	}
 	UDEBUG("");
 }
 
@@ -305,10 +342,21 @@ void FlannIndex::buildLSHIndex(
 	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
 	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
 
-	// incremental FLANN
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	if(rebalancingFactor_ > 1.0f)
+	{
+		for(int i=0; i<features.rows; ++i)
+		{
+			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+		}
+	}
+	else
+	{
+		// tree won't ever be rebalanced, so just keep only one header for the data
 		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-
+		nextIndex_ += features.rows;
-	nextIndex_ = features.rows;
+	}
 	UDEBUG("");
 }
 
@@ -317,12 +365,12 @@ bool FlannIndex::isBuilt()
 	return index_!=0;
 }
 
-unsigned int FlannIndex::addPoints(const cv::Mat & features)
+std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 {
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
-		return 0;
+		return std::vector<unsigned int>();
 	}
 	UASSERT(features.type() == featuresType_);
 	UASSERT(features.cols == featuresDim_);
@@ -401,11 +449,16 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
 		removedIndexes_.clear();
 	}
 
-	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+	// incremental FLANN: we should add all headers separately in case we remove
+	// some indexes (to keep underlying matrix data allocated)
+	std::vector<unsigned int> indexes;
+	for(int i=0; i<features.rows; ++i)
+	{
+		indexes.push_back(nextIndex_);
+		addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
+	}
 
-	int r = nextIndex_;
+	return indexes;
-	nextIndex_ += features.rows;
-	return r;
 }
 
 void FlannIndex::removePoint(unsigned int index)

corelib/src/Graph.cpp

@@ -1129,19 +1129,22 @@ std::multimap<int, Link> filterDuplicateLinks(
 
 std::multimap<int, Link> filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType)
+		Link::Type filteredType,
+		bool inverted)
 {
 	std::multimap<int, Link> output;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if(iter->second.from() != iter->second.to())
+			if((!inverted && iter->second.from() != iter->second.to())||
+			   (inverted && iter->second.from() == iter->second.to()))
 			{
 				output.insert(*iter);
 			}
 		}
-		else if(iter->second.type() != filteredType)
+		else if((!inverted && iter->second.type() != filteredType)||
+				(inverted && iter->second.type() == filteredType))
 		{
 			output.insert(*iter);
 		}
@@ -1151,19 +1154,22 @@ std::multimap<int, Link> filterLinks(
 
 std::map<int, Link> filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType)
+		Link::Type filteredType,
+		bool inverted)
 {
 	std::map<int, Link> output;
 	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if(iter->second.from() != iter->second.to())
+			if((!inverted && iter->second.from() != iter->second.to())||
+			   (inverted && iter->second.from() == iter->second.to()))
 			{
 				output.insert(*iter);
 			}
 		}
-		else if(iter->second.type() != filteredType)
+		else if((!inverted && iter->second.type() != filteredType)||
+				(inverted && iter->second.type() == filteredType))
 		{
 			output.insert(*iter);
 		}

corelib/src/Memory.cpp

@@ -362,7 +362,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			const std::map<int, Signature *> & signatures = this->getSignatures();
 			for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
+				const std::multimap<int, int> & words = i->second->getWords();
 				std::list<int> keys = uUniqueKeys(words);
 				for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
 				{
@@ -413,11 +413,11 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			Signature * s = this->_getSignature(i->first);
 			UASSERT(s != 0);
 
-			const std::multimap<int, cv::KeyPoint> & words = s->getWords();
+			const std::multimap<int, int> & words = s->getWords();
 			if(words.size())
 			{
 				UDEBUG("node=%d, word references=%d", s->id(), words.size());
-				for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
+				for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
 					if(iter->first > 0)
 					{
@@ -2753,20 +2753,16 @@ Transform Memory::computeTransform(
 		if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
 		{
 			UDEBUG("");
-			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpFrom.removeAllWords();
-			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
-			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpTo.removeAllWords();
-			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
-			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 		else if(useKnownCorrespondencesIfPossible)
 		{
 			// This will make RegistrationVis bypassing the correspondences computation
-			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpFrom.setWordsDescriptors(cv::Mat());
-			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.setWordsDescriptors(cv::Mat());
 		}
 
 		bool isNeighborRefining = fromS.getLinks().find(toS.id()) != fromS.getLinks().end() && fromS.getLinks().find(toS.id())->second.type() == Link::kNeighbor;
@@ -2795,23 +2791,28 @@ Transform Memory::computeTransform(
 			   !tmpTo.getWords().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
 			   !tmpFrom.getWords().empty() &&
-			   !tmpFrom.getWords3().empty())
+			   !tmpFrom.getWords3().empty() &&
+			   fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
 		{
-			std::multimap<int, cv::Point3f> words3DMap;
+			std::multimap<int, int> words;
-			std::multimap<int, cv::KeyPoint> wordsMap;
+			std::vector<cv::Point3f> words3DMap;
-			std::multimap<int, cv::Mat> wordsDescriptorsMap;
+			std::vector<cv::KeyPoint> wordsMap;
+			cv::Mat wordsDescriptorsMap;
 
 			const std::multimap<int, Link> & links = fromS.getLinks();
+			if(!fromS.getWords3().empty())
 			{
-				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
+				const std::map<int, int> & wordsFrom = uMultimapToMapUnique(fromS.getWords());
-				UDEBUG("fromS.getWords3()=%d  uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
+				UDEBUG("fromS.getWords()=%d  uniques=%d", (int)fromS.getWords().size(), (int)wordsFrom.size());
-				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
+				for(std::map<int, int>::const_iterator jter=wordsFrom.begin(); jter!=wordsFrom.end(); ++jter)
 				{
-					if(util3d::isFinite(jter->second))
+					const cv::Point3f & pt = fromS.getWords3()[jter->second];
+					if(util3d::isFinite(pt))
 					{
-						words3DMap.insert(*jter);
+						words.insert(std::make_pair(jter->first, words.size()));
-						wordsMap.insert(*fromS.getWords().find(jter->first));
+						words3DMap.push_back(pt);
-						wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
+						wordsMap.push_back(fromS.getWordsKpts()[jter->second]);
+						wordsDescriptorsMap.push_back(fromS.getWordsDescriptors().row(jter->second));
 					}
 				}
 			}
@@ -2820,21 +2821,23 @@ Transform Memory::computeTransform(
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
 				int id = iter->first;
-				if(id != fromS.id())
+				if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
 				{
 					const Signature * s = this->getSignature(id);
-					if(s)
+					if(s && !s->getWords3().empty())
 					{
-						const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
+						const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
-						for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
+						for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
 						{
+							const cv::Point3f & pt = s->getWords3()[jter->second];
 							if( jter->first > 0 &&
-								util3d::isFinite(jter->second) &&
+								util3d::isFinite(pt) &&
-								words3DMap.find(jter->first) == words3DMap.end())
+								words.find(jter->first) == words.end())
 							{
-								words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
+								words.insert(words.end(), std::make_pair(jter->first, words.size()));
-								wordsMap.insert(*s->getWords().find(jter->first));
+								words3DMap.push_back(util3d::transformPoint(pt, iter->second.transform()));
-								wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
+								wordsMap.push_back(s->getWordsKpts()[jter->second]);
+								wordsDescriptorsMap.push_back(s->getWordsDescriptors().row(jter->second));
 							}
 						}
 					}
@@ -2842,24 +2845,29 @@ Transform Memory::computeTransform(
 			}
 			UDEBUG("words3DMap=%d", (int)words3DMap.size());
 			Signature tmpFrom2(fromS.id());
-			tmpFrom2.setWords3(words3DMap);
+			tmpFrom2.setWords(words, wordsMap, words3DMap, wordsDescriptorsMap);
-			tmpFrom2.setWords(wordsMap);
-			tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
 
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);
 
-			if(!transform.isNull() && info)
+			if(!transform.isNull() && info && !tmpFrom2.getWords3().empty())
 			{
-				std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
+				std::map<int, cv::Point3f> points3DMap;
+				std::map<int, int> wordsMap = uMultimapToMapUnique(tmpFrom2.getWords());
+				for(std::map<int, int>::iterator iter=wordsMap.begin(); iter!=wordsMap.end(); ++iter)
+				{
+					points3DMap.insert(std::make_pair(iter->first, tmpFrom2.getWords3()[iter->second]));
+				}
 				std::map<int, Transform> bundlePoses;
 				std::multimap<int, Link> bundleLinks;
 				std::map<int, CameraModel> bundleModels;
 				std::map<int, std::map<int, FeatureBA> > wordReferences;
 
 				std::multimap<int, Link> links = fromS.getLinks();
+				links = graph::filterLinks(links, Link::kNeighbor, true); // assemble only neighbors for the local feature map
 				links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
 				links.insert(std::make_pair(fromS.id(), Link()));
 
+				int totalWordReferences = 0;
 				for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
 					int id = iter->first;
@@ -2912,33 +2920,41 @@ Transform Memory::computeTransform(
 								bundlePoses.insert(std::make_pair(id, iter->second.transform()));
 							}
 
-							const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
+							const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
-							for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
+							for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
 							{
 								if(points3DMap.find(jter->first)!=points3DMap.end() &&
-									(id == tmpTo.id() || jter->first > 0))
+										(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
 								{
-									std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
+									//get depth
-									cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
+									float d = 0.0f;
+									if( !s->getWords3().empty() &&
+										util3d::isFinite(s->getWords3()[jter->second]))
+									{
+										//move back point in camera frame (to get depth along z)
+										d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
+									}
 									wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
-									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
+									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
+									++totalWordReferences;
 								}
 							}
 						}
 					}
 				}
 
+
 				UDEBUG("sba...start");
 				// set root negative to fix all other poses
 				std::set<int> sbaOutliers;
 				UTimer bundleTimer;
-				OptimizerG2O sba;
+				OptimizerG2O sba(parameters_);
 				sba.setIterations(5);
 				UTimer bundleTime;
 				bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
 				UDEBUG("sba...end");
 
-				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
+				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());
 
 				UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
 				if(!bundlePoses.rbegin()->second.isNull())
@@ -3406,21 +3422,25 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
 			{
 				if(words3D)
 				{
-					const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
+					if(!ss->getWords3().empty())
-					for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 					{
+						const std::multimap<int, int> & ref = ss->getWords();
+						for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+						{
+							const cv::Point3f & pt = ss->getWords3()[jter->second];
 							//show only valid point according to current parameters
-						if(pcl::isFinite(jter->second) &&
+							if(pcl::isFinite(pt) &&
-						   (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
+							   (pt.x != 0 || pt.y != 0 || pt.z != 0))
 							{
 								fprintf(foutSign, "%d ", (*jter).first);
 							}
 						}
 					}
+				}
 				else
 				{
-					const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
+					const std::multimap<int, int> & ref = ss->getWords();
-					for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+					for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 					{
 						fprintf(foutSign, "%d ", (*jter).first);
 					}
@@ -3490,6 +3510,47 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 
 }
 
+unsigned long Memory::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(Memory);
+	memoryUsage += _signatures.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Signature *>);
+	for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
+	{
+		memoryUsage += iter->second->getMemoryUsed(true);
+	}
+	if(_vwd)
+	{
+		memoryUsage += _vwd->getMemoryUsed();
+	}
+	memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
+	memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, double>);
+	memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
+	memoryUsage += _labels.size() * (sizeof(int)+sizeof(std::string) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::string>);
+	for(std::map<int, std::string>::const_iterator iter=_labels.begin(); iter!=_labels.end(); ++iter)
+	{
+		memoryUsage+=iter->second.size();
+	}
+	memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
+	memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
+	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
+	{
+		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
+	}
+	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
+	{
+		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
+	}
+	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+	memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+	memoryUsage += sizeof(Registration);
+	memoryUsage += sizeof(RegistrationIcp);
+	memoryUsage += _occupancy->getMemoryUsed();
+	memoryUsage += sizeof(MarkerDetector);
+	memoryUsage += sizeof(DBDriver);
+
+	return memoryUsage;
+}
+
 void Memory::rehearsal(Signature * signature, Statistics * stats)
 {
 	UTimer timer;
@@ -3881,9 +3942,10 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
 }
 
 void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
-		std::multimap<int, cv::KeyPoint> & words,
+		std::multimap<int, int> & words,
-		std::multimap<int, cv::Point3f> & words3,
+		std::vector<cv::KeyPoint> & wordsKpts,
-		std::multimap<int, cv::Mat> & wordsDescriptors,
+		std::vector<cv::Point3f> & words3,
+		cv::Mat & wordsDescriptors,
 		std::vector<GlobalDescriptor> & globalDescriptors) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
@@ -3891,6 +3953,7 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 	if(s)
 	{
 		words = s->getWords();
+		wordsKpts = s->getWordsKpts();
 		words3 = s->getWords3();
 		wordsDescriptors = s->getWordsDescriptors();
 		globalDescriptors = s->sensorData().globalDescriptors();
@@ -3906,6 +3969,7 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 		if(signatures.size())
 		{
 			words = signatures.front()->getWords();
+			wordsKpts = signatures.front()->getWordsKpts();
 			words3 = signatures.front()->getWords3();
 			wordsDescriptors = signatures.front()->getWordsDescriptors();
 			globalDescriptors = signatures.front()->sensorData().globalDescriptors();
@@ -3975,7 +4039,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 	{
 		// words 2d
 		this->disableWordsRef(to->id());
-		to->setWords(from->getWords());
+		to->setWords(from->getWords(), from->getWordsKpts(), from->getWords3(), from->getWordsDescriptors());
 		std::list<int> id;
 		id.push_back(to->id());
 		this->enableWordsRef(id);
@@ -3992,8 +4056,6 @@ void Memory::copyData(const Signature * from, Signature * to)
 		to->sensorData().setId(to->id());
 
 		to->setPose(from->getPose());
-		to->setWords3(from->getWords3());
-		to->setWordsDescriptors(from->getWordsDescriptors());
 	}
 	else
 	{
@@ -4649,9 +4711,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("id %d is a bad signature", id);
 	}
 
-	std::multimap<int, cv::KeyPoint> words;
+	std::multimap<int, int> words;
-	std::multimap<int, cv::Point3f> words3D;
+	std::vector<cv::KeyPoint> wordsKpts;
-	std::multimap<int, cv::Mat> wordsDescriptors;
+	std::vector<cv::Point3f> words3D;
+	cv::Mat wordsDescriptors;
 	int words3DValid = 0;
 	if(wordIds.size() > 0)
 	{
@@ -4671,11 +4734,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				kpt.size *= decimationRatio;
 				kpt.octave += log2value;
 			}
-			words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
+			words.insert(std::make_pair(*iter, words.size()));
+			wordsKpts.push_back(kpt);
 
 			if(keypoints3D.size())
 			{
-				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
+				words3D.push_back(keypoints3D.at(i));
 				if(util3d::isFinite(keypoints3D.at(i)))
 				{
 					++words3DValid;
@@ -4683,7 +4747,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			if(_rawDescriptorsKept)
 			{
-				wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
+				wordsDescriptors.push_back(descriptors.row(i));
 			}
 		}
 	}
@@ -4803,18 +4867,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 			Signature cpPrevious(2);
 			// IDs should be unique so that registration doesn't override them
-			std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
+			std::map<int, int> uniqueWordsOld = uMultimapToMapUnique(previousS->getWords());
-			std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
+			std::vector<cv::KeyPoint> uniqueWordsKpts;
+			cv::Mat uniqueWordsDescriptors;
+			std::multimap<int, int> uniqueWords;
+			for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
+			{
+				uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
+				uniqueWordsKpts.push_back(previousS->getWordsKpts()[iter->second]);
+				uniqueWordsDescriptors.push_back(previousS->getWordsDescriptors().row(iter->second));
+			}
 			cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
-			cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
+			cpPrevious.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
-			cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 			Signature cpCurrent(1);
-			uniqueWords = uMultimapToMapUnique(words);
+			uniqueWordsOld = uMultimapToMapUnique(words);
-			uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
+			uniqueWordsKpts.clear();
+			uniqueWordsDescriptors = cv::Mat();
+			uniqueWords.clear();
+			for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
+			{
+				uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
+				uniqueWordsKpts.push_back(wordsKpts[iter->second]);
+				uniqueWordsDescriptors.push_back(wordsDescriptors.row(iter->second));
+			}
 			cpCurrent.sensorData().setCameraModels(cameraModels);
 			// This will force comparing descriptors between both images directly
-			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
+			cpCurrent.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
-			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 
 			// The following is used only to re-estimate the correspondences, the returned transform is ignored
 			Transform tmpt;
@@ -4832,9 +4910,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UDEBUG("t=%s", tmpt.prettyPrint().c_str());
 
 			// compute 3D words by epipolar geometry with the previous signature using odometry motion
+			std::map<int, int> currentUniqueWords = uMultimapToMapUnique(cpCurrent.getWords());
+			std::map<int, int> previousUniqueWords = uMultimapToMapUnique(cpPrevious.getWords());
+			std::map<int, cv::KeyPoint> currentWords;
+			std::map<int, cv::KeyPoint> previousWords;
+			for(std::map<int, int>::iterator iter=currentUniqueWords.begin(); iter!=currentUniqueWords.end(); ++iter)
+			{
+				currentWords.insert(std::make_pair(iter->first, cpCurrent.getWordsKpts()[iter->second]));
+			}
+			for(std::map<int, int>::iterator iter=previousUniqueWords.begin(); iter!=previousUniqueWords.end(); ++iter)
+			{
+				previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
+			}
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
-					uMultimapToMapUnique(cpCurrent.getWords()),
+					currentWords,
-					uMultimapToMapUnique(cpPrevious.getWords()),
+					previousWords,
 					cameraModels[0],
 					cameraTransform);
 
@@ -4845,32 +4935,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UASSERT(words3D.size() == 0 || words.size() == words3D.size());
 			bool words3DWasEmpty = words3D.empty();
 			int added3DPointsWithoutDepth = 0;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+			for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
 				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
-				std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
+				if(words3DWasEmpty)
-				if(iter3D == words3D.end())
 				{
 					if(jter != inliers.end())
 					{
-						words3D.insert(std::make_pair(iter->first, jter->second));
+						words3D.push_back(jter->second);
 						++added3DPointsWithoutDepth;
 					}
 					else
 					{
-						words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+						words3D.push_back(cv::Point3f(bad_point,bad_point,bad_point));
 					}
 				}
-				else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
+				else if(!util3d::isFinite(words3D[iter->second]) && jter != inliers.end())
 				{
-					iter3D->second = jter->second;
+					words3D[iter->second] = jter->second;
 					++added3DPointsWithoutDepth;
 				}
-				else if(words3DWasEmpty && jter == inliers.end())
-				{
-					// duplicate
-					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
-				}
 			}
 			UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
 			if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
@@ -5123,9 +5207,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData));
 	}
 
-	s->setWords(words);
+	s->setWords(words, wordsKpts, words3D, wordsDescriptors);
-	s->setWords3(words3D);
-	s->setWordsDescriptors(wordsDescriptors);
 
 	// set raw data
 	if(!cameraModels.empty())
@@ -5290,7 +5372,7 @@ void Memory::disableWordsRef(int signatureId)
 	Signature * ss = this->_getSignature(signatureId);
 	if(ss && ss->isEnabled())
 	{
-		const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
+		const std::multimap<int, int> & words = ss->getWords();
 		const std::list<int> & keys = uUniqueKeys(words);
 		int count = _vwd->getTotalActiveReferences();
 		// First remove all references

corelib/src/OccupancyGrid.cpp

@@ -1532,4 +1532,36 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	return updated;
 }
 
+unsigned long OccupancyGrid::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(OccupancyGrid);
+	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+
+	memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
+	for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter)
+	{
+		memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize();
+		memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize();
+		memoryUsage += iter->second.second.total() * iter->second.second.elemSize();
+	}
+	memoryUsage += map_.total() * map_.elemSize();
+	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
+	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<int, int> >);
+	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
+
+	if(assembledGround_.get())
+	{
+		memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledObstacles_.get())
+	{
+		memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledEmptyCells_.get())
+	{
+		memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	return memoryUsage;
+}
+
 }

corelib/src/Optimizer.cpp

@@ -617,8 +617,8 @@ void Optimizer::computeBACorrespondences(
 
 					if(!rematchFeatures)
 					{
-						sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+						sFrom.setWordsDescriptors(cv::Mat());
-						sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+						sTo.setWordsDescriptors(cv::Mat());
 					}
 
 					RegistrationInfo info;
@@ -633,13 +633,13 @@ void Optimizer::computeBACorrespondences(
 							// set descriptors for the output
 							if(sFrom.getWords().size() &&
 							   sFrom.getWordsDescriptors().empty() &&
-							   sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().size())
+							   (int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
 							{
 								sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
 							}
 							if(sTo.getWords().size() &&
 							   sTo.getWordsDescriptors().empty() &&
-							   sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().size())
+							   (int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
 							{
 								sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
 							}
@@ -649,11 +649,13 @@ void Optimizer::computeBACorrespondences(
 						UASSERT(!pose.isNull());
 						for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
 						{
-							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+							int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
+							cv::Point3f p = sFrom.getWords3()[indexFrom];
 							if(p.x > 0.0f) // make sure the point is valid
 							{
-								cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
+								cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
-								cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
+								int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
+								cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];
 
 								int wordId = -1;
 
@@ -692,10 +694,10 @@ void Optimizer::computeBACorrespondences(
 								if(!fromAlreadyAdded)
 								{
 									cv::Mat descriptorFrom;
-									if(sFrom.getWordsDescriptors().size())
+									if(!sFrom.getWordsDescriptors().empty())
 									{
-										UASSERT(sFrom.getWordsDescriptors().find(info.inliersIDs[i]) != sFrom.getWordsDescriptors().end());
+										UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
-										descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
+										descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
 									}
 									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
 									frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
@@ -705,17 +707,20 @@ void Optimizer::computeBACorrespondences(
 								if(!toAlreadyAdded)
 								{
 									cv::Mat descriptorTo;
-									if(sTo.getWordsDescriptors().size())
+									if(!sTo.getWordsDescriptors().empty())
 									{
-										UASSERT(sTo.getWordsDescriptors().find(info.inliersIDs[i]) != sTo.getWordsDescriptors().end());
+										UASSERT(indexTo < sTo.getWordsDescriptors().rows);
-										descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
+										descriptorTo = sTo.getWordsDescriptors().row(indexTo);
 									}
 									float depth = 0.0f;
-									std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
+									if(!sTo.getWords3().empty())
-									if( iterTo!=sTo.getWords3().end() &&
-										iterTo->second.x > 0)
 									{
-										depth = iterTo->second.x;
+										UASSERT(indexTo < (int)sTo.getWords3().size());
+										const cv::Point3f & pt = sTo.getWords3()[indexTo];
+										if( pt.x > 0)
+										{
+											depth = pt.x;
+										}
 									}
 									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
 									frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));

corelib/src/RegistrationVis.cpp

@@ -303,7 +303,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
-			(int)fromSignature.getWordsDescriptors().size(),
+			(int)fromSignature.getWordsDescriptors().rows,
 			(int)fromSignature.sensorData().keypoints().size(),
 			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows,
@@ -316,7 +316,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
-			(int)toSignature.getWordsDescriptors().size(),
+			(int)toSignature.getWordsDescriptors().rows,
 			(int)toSignature.sensorData().keypoints().size(),
 			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows,
@@ -349,16 +349,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				fromSignature.getWords3().empty() ||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
 		UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
-				fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
+				(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
-				fromSignature.sensorData().descriptors().rows == 0 ||
+				fromSignature.sensorData().descriptors().empty() ||
-				fromSignature.getWordsDescriptors().size() == 0);
+				fromSignature.getWordsDescriptors().empty() == 0);
 		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
-				toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
+				(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
-				toSignature.sensorData().descriptors().rows == 0 ||
+				toSignature.sensorData().descriptors().empty() ||
-				toSignature.getWordsDescriptors().size() == 0);
+				toSignature.getWordsDescriptors().empty());
 		UASSERT(fromSignature.sensorData().imageRaw().empty() ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC3);
@@ -371,6 +371,7 @@ Transform RegistrationVis::computeTransformationImpl(
 		cv::Mat imageTo = toSignature.sensorData().imageRaw();
 
 		std::vector<int> orignalWordsFromIds;
+		int kptsFromSource = 0;
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
@@ -403,22 +404,26 @@ Transform RegistrationVis::computeTransformationImpl(
 			else
 			{
 				kptsFrom = fromSignature.sensorData().keypoints();
+				kptsFromSource = 1;
 			}
 		}
 		else
 		{
-			kptsFrom.resize(fromSignature.getWords().size());
+			kptsFromSource = 2;
 			orignalWordsFromIds.resize(fromSignature.getWords().size());
 			int i=0;
 			bool allUniques = true;
-			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
+			int previousIdAdded = 0;
+			kptsFrom = fromSignature.getWordsKpts();
+			for(std::multimap<int, int>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
 			{
-				kptsFrom[i] = iter->second;
+				UASSERT(iter->second>=0 && iter->second<(int)orignalWordsFromIds.size());
-				orignalWordsFromIds[i] = iter->first;
+				orignalWordsFromIds[iter->second] = iter->first;
-				if(i>0 && iter->first==orignalWordsFromIds[i-1])
+				if(i>0 && iter->first==previousIdAdded)
 				{
 					allUniques = false;
 				}
+				previousIdAdded = iter->first;
 				++i;
 			}
 			if(!allUniques)
@@ -428,12 +433,14 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 		}
 
-		std::multimap<int, cv::KeyPoint> wordsFrom;
+		std::multimap<int, int> wordsFrom;
-		std::multimap<int, cv::KeyPoint> wordsTo;
+		std::multimap<int, int> wordsTo;
-		std::multimap<int, cv::Point3f> words3From;
+		std::vector<cv::KeyPoint> wordsKptsFrom;
-		std::multimap<int, cv::Point3f> words3To;
+		std::vector<cv::KeyPoint> wordsKptsTo;
-		std::multimap<int, cv::Mat> wordsDescFrom;
+		std::vector<cv::Point3f> words3From;
-		std::multimap<int, cv::Mat> wordsDescTo;
+		std::vector<cv::Point3f> words3To;
+		cv::Mat wordsDescFrom;
+		cv::Mat wordsDescTo;
 		if(_correspondencesApproach == 1) //Optical Flow
 		{
 			UDEBUG("");
@@ -454,7 +461,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			std::vector<cv::Point3f> kptsFrom3D;
 			if(kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsFrom3D = fromSignature.getWords3();
 			}
 			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
 			{
@@ -544,13 +551,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
 				for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
 				{
-					int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+					int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-					wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+					wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-					words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
+					wordsKptsFrom.push_back(kptsFrom[i]);
-					wordsTo.insert(std::make_pair(id, kptsTo[i]));
+					words3From.push_back(kptsFrom3DKept[i]);
-					if(kptsTo3D.size())
+
+					wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
+					wordsKptsTo.push_back(kptsTo[i]);
+					if(!kptsTo3D.empty())
 					{
-						words3To.insert(std::make_pair(id, kptsTo3D[i]));
+						words3To.push_back(kptsTo3D[i]);
 					}
 				}
 				toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
@@ -566,9 +576,10 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(util3d::isFinite(kptsFrom3D[i]))
 					{
-						int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+						int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-						wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+						wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-						words3From.insert(std::make_pair(id, kptsFrom3D[i]));
+						wordsKptsFrom.push_back(kptsFrom[i]);
+						words3From.push_back(kptsFrom3D[i]);
 					}
 				}
 				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
@@ -580,6 +591,7 @@ Transform RegistrationVis::computeTransformationImpl(
 		{
 			UDEBUG("");
 			std::vector<cv::KeyPoint> kptsTo;
+			int kptsToSource = 0;
 			if(toSignature.getWords().empty())
 			{
 				if(toSignature.sensorData().keypoints().empty() &&
@@ -610,33 +622,28 @@ Transform RegistrationVis::computeTransformationImpl(
 				else
 				{
 					kptsTo = toSignature.sensorData().keypoints();
+					kptsToSource = 1;
 				}
 			}
 			else
 			{
-				kptsTo = uValues(toSignature.getWords());
+				kptsTo = toSignature.getWordsKpts();
+				kptsToSource = 2;
 			}
 
 			// extract descriptors
-			UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
+			UDEBUG("kptsFrom=%d kptsFromSource=%d", (int)kptsFrom.size(), kptsFromSource);
-			UDEBUG("kptsTo=%d", (int)kptsTo.size());
+			UDEBUG("kptsTo=%d kptsToSource=%d", (int)kptsTo.size(), kptsToSource);
 			cv::Mat descriptorsFrom;
-			if(fromSignature.getWordsDescriptors().size() &&
+			if(kptsFromSource == 2 &&
-					((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
+				fromSignature.getWordsDescriptors().rows &&
-					 fromSignature.getWordsDescriptors().size() == kptsFrom.size()))
+				((kptsFrom.empty() && fromSignature.getWordsDescriptors().rows) ||
+				 fromSignature.getWordsDescriptors().rows == (int)kptsFrom.size()))
 			{
-				descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
+				descriptorsFrom = fromSignature.getWordsDescriptors();
-						fromSignature.getWordsDescriptors().begin()->second.cols,
-						fromSignature.getWordsDescriptors().begin()->second.type());
-				int i=0;
-				for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
-					iter!=fromSignature.getWordsDescriptors().end();
-					++iter, ++i)
-				{
-					iter->second.copyTo(descriptorsFrom.row(i));
 			}
-			}
+			else if(kptsFromSource == 1 &&
-			else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
+					fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
 			{
 				descriptorsFrom = fromSignature.sensorData().descriptors();
 			}
@@ -656,20 +663,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			cv::Mat descriptorsTo;
 			if(kptsTo.size())
 			{
-				if(toSignature.getWordsDescriptors().size() == kptsTo.size())
+				if(kptsToSource == 2 &&
+					toSignature.getWordsDescriptors().rows == (int)kptsTo.size())
 				{
-					descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
+					descriptorsTo = toSignature.getWordsDescriptors();
-							toSignature.getWordsDescriptors().begin()->second.cols,
-							toSignature.getWordsDescriptors().begin()->second.type());
-					int i=0;
-					for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
-						iter!=toSignature.getWordsDescriptors().end();
-						++iter, ++i)
-					{
-						iter->second.copyTo(descriptorsTo.row(i));
 				}
-				}
+				else if(kptsToSource == 1 &&
-				else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
+						toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
 				{
 					descriptorsTo = toSignature.sensorData().descriptors();
 				}
@@ -689,11 +689,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			// create 3D keypoints
 			std::vector<cv::Point3f> kptsFrom3D;
 			std::vector<cv::Point3f> kptsTo3D;
-			if(kptsFrom.size() == fromSignature.getWords3().size())
+			if(kptsFromSource == 2 &&
+				kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsFrom3D = fromSignature.getWords3();
 			}
-			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			else if(kptsFromSource == 1 &&
+					kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
 			{
 				kptsFrom3D = fromSignature.sensorData().keypoints3D();
 			}
@@ -724,11 +726,12 @@ Transform RegistrationVis::computeTransformationImpl(
 				_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
 			}
 
-			if(kptsTo.size() == toSignature.getWords3().size())
+			if(kptsToSource == 2 && kptsTo.size() == toSignature.getWords3().size())
 			{
-				kptsTo3D = uValues(toSignature.getWords3());
+				kptsTo3D = toSignature.getWords3();
 			}
-			else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
+			else if(kptsToSource == 1 &&
+					kptsTo.size() == toSignature.sensorData().keypoints3D().size())
 			{
 				kptsTo3D = toSignature.sensorData().keypoints3D();
 			}
@@ -858,7 +861,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							UDEBUG("radius search done for guess");
 
 							// Process results (Nearest Neighbor Distance Ratio)
-							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
+							int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
 							std::map<int,int> addedWordsFrom; //<id, index>
 							std::map<int, int> duplicates; //<fromId, toId>
 							int newWords = 0;
@@ -912,7 +915,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								if(matchedIndex >= 0)
 								{
 									matchedIndex = projectedIndexToDescIndex[matchedIndex];
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndex]:matchedIndex;
 
 									if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
 									{
@@ -923,29 +926,32 @@ Transform RegistrationVis::computeTransformationImpl(
 									{
 										addedWordsFrom.insert(std::make_pair(matchedIndex, id));
 
-										if(kptsFrom.size())
+										wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+										if(!kptsFrom.empty())
 										{
-											wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
+											wordsKptsFrom.push_back(kptsFrom[matchedIndex]);
 										}
-										words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
+										words3From.push_back(kptsFrom3D[matchedIndex]);
-										wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
+										wordsDescFrom.push_back(descriptorsFrom.row(matchedIndex));
 									}
 
-									wordsTo.insert(std::make_pair(id, kptsTo[i]));
+									wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
-									wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
+									wordsKptsTo.push_back(kptsTo[i]);
-									if(kptsTo3D.size())
+									wordsDescTo.push_back(descriptorsTo.row(i));
+									if(!kptsTo3D.empty())
 									{
-										words3To.insert(std::make_pair(id, kptsTo3D[i]));
+										words3To.push_back(kptsTo3D[i]);
 									}
 								}
 								else
 								{
 									// gen fake ids
-									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
+									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
-									wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
+									wordsKptsTo.push_back(kptsTo[i]);
-									if(kptsTo3D.size())
+									wordsDescTo.push_back(descriptorsTo.row(i));
+									if(!kptsTo3D.empty())
 									{
-										words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
+										words3To.push_back(kptsTo3D[i]);
 									}
 
 									++newToId;
@@ -962,10 +968,11 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 								{
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
+									wordsKptsFrom.push_back(kptsFrom[i]);
-									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
+									wordsDescFrom.push_back(descriptorsFrom.row(i));
+									words3From.push_back(kptsFrom3D[i]);
 
 									++addWordsFromNotMatched;
 								}
@@ -1007,7 +1014,7 @@ Transform RegistrationVis::computeTransformationImpl(
 
 								if(indices[i].size())
 								{
-									info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
+									info.projectedIDs.push_back(!orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
 								}
 
 								if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
@@ -1058,26 +1065,28 @@ Transform RegistrationVis::computeTransformationImpl(
 										matchedIndexTo = indices[i].at(0);
 									}
 
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
 									addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
 
-									if(kptsFrom.size())
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
+									if(!kptsFrom.empty())
 									{
-										wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
+										wordsKptsFrom.push_back(kptsFrom[matchedIndexFrom]);
 									}
-									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
+									words3From.push_back(kptsFrom3D[matchedIndexFrom]);
-									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
+									wordsDescFrom.push_back(descriptorsFrom.row(matchedIndexFrom));
 
 									if(	matchedIndexTo >= 0 &&
 										addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
 									{
 										addedWordsTo.insert(matchedIndexTo);
 
-										wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
+										wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
-										wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
+										wordsKptsTo.push_back(kptsTo[matchedIndexTo]);
-										if(kptsTo3D.size())
+										wordsDescTo.push_back(descriptorsTo.row(matchedIndexTo));
+										if(!kptsTo3D.empty())
 										{
-											words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
+											words3To.push_back(kptsTo3D[matchedIndexTo]);
 										}
 									}
 								}
@@ -1089,23 +1098,25 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 								{
-									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
+									wordsKptsFrom.push_back(kptsFrom[i]);
-									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
+									wordsDescFrom.push_back(descriptorsFrom.row(i));
+									words3From.push_back(kptsFrom3D[i]);
 								}
 							}
 
-							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
+							int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
 							for(unsigned int i = 0; i < kptsTo.size(); ++i)
 							{
 								if(addedWordsTo.find(i) == addedWordsTo.end())
 								{
-									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
+									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
-									wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
+									wordsKptsTo.push_back(kptsTo[i]);
-									if(kptsTo3D.size())
+									wordsDescTo.push_back(descriptorsTo.row(i));
+									if(!kptsTo3D.empty())
 									{
-										words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
+										words3To.push_back(kptsTo3D[i]);
 									}
 									++newToId;
 								}
@@ -1271,15 +1282,16 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(fromWordIdsSet.count(*iter) == 1)
 						{
-							if (kptsFrom.size())
+							wordsFrom.insert(wordsFrom.end(), std::make_pair(*iter, wordsFrom.size()));
+							if (!kptsFrom.empty())
 							{
-								wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
+								wordsKptsFrom.push_back(kptsFrom[i]);
 							}
-							if(kptsFrom3D.size())
+							if(!kptsFrom3D.empty())
 							{
-								words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
+								words3From.push_back(kptsFrom3D[i]);
 							}
-							wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
+							wordsDescFrom.push_back(descriptorsFrom.row(i));
 						}
 						++i;
 					}
@@ -1291,11 +1303,12 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(toWordIdsSet.count(*iter) == 1)
 						{
-							wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
+							wordsTo.insert(wordsTo.end(), std::make_pair(*iter, wordsTo.size()));
-							wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
+							wordsKptsTo.push_back(kptsTo[i]);
-							if(kptsTo3D.size())
+							wordsDescTo.push_back(descriptorsTo.row(i));
+							if(!kptsTo3D.empty())
 							{
-								words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
+								words3To.push_back(kptsTo3D[i]);
 							}
 						}
 						++i;
@@ -1308,21 +1321,19 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
 				for(int i=0; i<descriptorsFrom.rows; ++i)
 				{
-					wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
+					wordsFrom.insert(wordsFrom.end(), std::make_pair(i, wordsFrom.size()));
-					wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
+					wordsKptsFrom.push_back(kptsFrom[i]);
-					if(kptsFrom3D.size())
+					wordsDescFrom.push_back(descriptorsFrom.row(i));
+					if(!kptsFrom3D.empty())
 					{
-						words3From.insert(std::make_pair(i, kptsFrom3D[i]));
+						words3From.push_back(kptsFrom3D[i]);
 					}
 				}
 			}
 		}
-		fromSignature.setWords(wordsFrom);
+
-		fromSignature.setWords3(words3From);
+		fromSignature.setWords(wordsFrom, wordsKptsFrom, words3From, wordsDescFrom);
-		fromSignature.setWordsDescriptors(wordsDescFrom);
+		toSignature.setWords(wordsTo, wordsKptsTo, words3To, wordsDescTo);
-		toSignature.setWords(wordsTo);
-		toSignature.setWords3(words3To);
-		toSignature.setWordsDescriptors(wordsDescTo);
 	}
 
 	/////////////////////
@@ -1376,14 +1387,31 @@ Transform RegistrationVis::computeTransformationImpl(
 					Transform cameraTransform;
 					double variance = 1.0f;
 					std::vector<int> matchesV;
+					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
+					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+					std::map<int, cv::KeyPoint> wordsA;
+					std::map<int, cv::Point3f> words3A;
+					std::map<int, cv::KeyPoint> wordsB;
+					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+					{
+						wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
+						if(!signatureA->getWords3().empty())
+						{
+							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+						}
+					}
+					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+					{
+						wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
+					}
 					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-							uMultimapToMapUnique(signatureA->getWords()),
+							wordsA,
-							uMultimapToMapUnique(signatureB->getWords()),
+							wordsB,
 							cameraModel,
 							cameraTransform,
 							_PnPReprojError,
 							0.99f,
-							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
+							words3A, // for scale estimation
 							&variance,
 							&matchesV);
 					covariances[dir] *= variance;
@@ -1459,9 +1487,26 @@ Transform RegistrationVis::computeTransformationImpl(
 
 						std::vector<int> inliersV;
 						std::vector<int> matchesV;
+						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
+						std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+						std::map<int, cv::Point3f> words3A;
+						std::map<int, cv::Point3f> words3B;
+						std::map<int, cv::KeyPoint> wordsB;
+						for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+						{
+							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+						}
+						for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+						{
+							wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
+							if(!signatureB->getWords3().empty())
+							{
+								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
+							}
+						}
 						transforms[dir] = util3d::estimateMotion3DTo2D(
-								uMultimapToMapUnique(signatureA->getWords3()),
+								words3A,
-								uMultimapToMapUnique(signatureB->getWords()),
+								wordsB,
 								cameraModel,
 								_minInliers,
 								_iterations,
@@ -1469,7 +1514,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPFlags,
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-								uMultimapToMapUnique(signatureB->getWords3()),
+								words3B,
 								&covariances[dir],
 								&matchesV,
 								&inliersV);
@@ -1505,9 +1550,21 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					std::vector<int> inliersV;
 					std::vector<int> matchesV;
+					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
+					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
+					std::map<int, cv::Point3f> words3A;
+					std::map<int, cv::Point3f> words3B;
+					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
+					{
+						words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
+					}
+					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
+					{
+						words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
+					}
 					transforms[dir] = util3d::estimateMotion3DTo3D(
-							uMultimapToMapUnique(signatureA->getWords3()),
+							words3A,
-							uMultimapToMapUnique(signatureB->getWords3()),
+							words3B,
 							_minInliers,
 							_inlierDistance,
 							_iterations,
@@ -1679,7 +1736,8 @@ Transform RegistrationVis::computeTransformationImpl(
 			for(unsigned int i=0; i<allInliers.size(); ++i)
 			{
 				int wordId = allInliers[i];
-				const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
+				int indexFrom = fromSignature.getWords().find(wordId)->second;
+				const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
 				if(!util3d::isFinite(pt3D))
 				{
 					UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
@@ -1690,20 +1748,21 @@ Transform RegistrationVis::computeTransformationImpl(
 				points3DMap.insert(std::make_pair(wordId, pt3D));
 
 				std::map<int, FeatureBA> ptMap;
-				if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
+				if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
 				{
 					float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
-					const cv::KeyPoint & kpt = fromSignature.getWords().find(wordId)->second;
+					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
 					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
 				}
-				if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
+				if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
 				{
+					int indexTo = toSignature.getWords().find(wordId)->second;
 					float depthTo = 0.0f;
-					if(toSignature.getWords3().find(wordId) != toSignature.getWords3().end())
+					if(!toSignature.getWords3().empty())
 					{
-						depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
+						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
 					}
-					const cv::KeyPoint & kpt = toSignature.getWords().find(wordId)->second;
+					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
 					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
 				}
 
@@ -1841,24 +1900,25 @@ Transform RegistrationVis::computeTransformationImpl(
 			{
 				if(_maxInliersMeanDistance>0.0f)
 				{
-					std::multimap<int, cv::Point3f>::const_iterator words3Iter = fromSignature.getWords3().find(allInliers[i]);
+					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					if(words3Iter != fromSignature.getWords3().end())
+					if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty())
 					{
-						if(uIsFinite(words3Iter->second.x))
+						const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second];
+						if(uIsFinite(pt.x))
 						{
-							cv::Point3f pt = util3d::transformPoint(words3Iter->second, transformInv);
+							distances.push_back(util3d::transformPoint(pt, transformInv).x);
-							distances.push_back(pt.x);
 						}
 					}
 				}
 
 				if(!pcaData.empty())
 				{
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
+					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					UASSERT(wordsIter != fromSignature.getWords().end());
+					UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
 					float * ptr = pcaData.ptr<float>(i, 0);
-					ptr[0] = (wordsIter->second.pt.x-cx) / w;
+					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
-					ptr[1] = (wordsIter->second.pt.y-cy) / h;
+					ptr[0] = (kpt.pt.x-cx) / w;
+					ptr[1] = (kpt.pt.y-cy) / h;
 				}
 			}
 
@@ -1899,6 +1959,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	}
 
 	info.inliers = inliersCount;
+	info.inliersRatio = !toSignature.getWords().empty()?float(inliersCount)/float(toSignature.getWords().size()):0;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
 	info.covariance = covariance;

corelib/src/Rtabmap.cpp

@@ -678,19 +678,6 @@ int Rtabmap::getTotalMemSize() const
 	return 0;
 }
 
-std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
-{
-	if(_memory)
-	{
-		const Signature * s = _memory->getSignature(locationId);
-		if(s)
-		{
-			return s->getWords();
-		}
-	}
-	return std::multimap<int, cv::KeyPoint>();
-}
-
 bool Rtabmap::isInSTM(int locationId) const
 {
 	if(_memory)
@@ -713,25 +700,7 @@ const Statistics & Rtabmap::getStatistics() const
 {
 	return statistics_;
 }
-/*
+
-bool Rtabmap::getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const
-{
-	if(_memory)
-	{
-		const Signature * s = _memory->getSignature(locationId);
-		if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
-		{
-			rgb = s->getImage();
-			depth = s->getDepth();
-			depthConstant = s->getDepthConstant();
-			pose = _optimizedPoses.at(s->id());
-			localTransform = s->getLocalTransform();
-			return true;
-		}
-	}
-	return false;
-}
-*/
 Transform Rtabmap::getPose(int locationId) const
 {
 	return uValue(_optimizedPoses, locationId, Transform());
@@ -2235,6 +2204,7 @@ bool Rtabmap::process(
 	//============================================================
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
+	float loopClosureVisualInliersRatio = 0.0f;
 	int loopClosureVisualMatches = 0;
 	float loopClosureLinearVariance = 0.0f;
 	float loopClosureAngularVariance = 0.0f;
@@ -2386,6 +2356,7 @@ bool Rtabmap::process(
 									lastProximitySpaceClosureId = nearestId;
 
 									loopClosureVisualInliers = info.inliers;
+									loopClosureVisualInliersRatio = info.inliersRatio;
 									loopClosureVisualMatches = info.matches;
 
 									loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -2589,6 +2560,7 @@ bool Rtabmap::process(
 			loopClosureVisualInliersDistribution = info.inliersDistribution;
 
 			loopClosureVisualInliers = info.inliers;
+			loopClosureVisualInliersRatio = info.inliersRatio;
 			loopClosureVisualMatches = info.matches;
 			rejectedGlobalLoopClosure = transform.isNull();
 			if(rejectedGlobalLoopClosure)
@@ -3167,6 +3139,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
+			statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
 			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
 			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
@@ -3315,6 +3288,13 @@ bool Rtabmap::process(
 			if(_publishRAMUsage)
 			{
 				statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
+				long estimatedMemoryUsage = sizeof(Rtabmap);
+				estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
+				estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
+				estimatedMemoryUsage += _memory->getMemoryUsed();
+				estimatedMemoryUsage += _bayesFilter->getMemoryUsed();
+				estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
+				statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB
 			}
 
 			if(_publishLikelihood || _publishPdf)
@@ -4463,18 +4443,30 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 				groundTruth,
 				data);
 
+		std::multimap<int, Link> links = _memory->getLinks(id, true, true);
+		for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+		{
+			if(iter->second.type() == Link::kLandmark)
+			{
+				s.addLandmark(iter->second);
+			}
+			else
+			{
+				s.addLink(iter->second);
+			}
+		}
+
 		if(withWords || withGlobalDescriptors)
 		{
-			std::multimap<int, cv::KeyPoint> words;
+			std::multimap<int, int> words;
-			std::multimap<int, cv::Point3f> words3;
+			std::vector<cv::KeyPoint> wordsKpts;
-			std::multimap<int, cv::Mat> wordsDescriptors;
+			std::vector<cv::Point3f> words3;
+			cv::Mat wordsDescriptors;
 			std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
-			_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
+			_memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors);
 			if(withWords)
 			{
-				s.setWords(words);
+				s.setWords(words, wordsKpts, words3, wordsDescriptors);
-				s.setWords3(words3);
-				s.setWordsDescriptors(wordsDescriptors);
 			}
 			if(withGlobalDescriptors)
 			{
@@ -4620,7 +4612,7 @@ int Rtabmap::detectMoreLoopClosures(
 	std::map<int, Transform> posesToCheckLoopClosures;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> links;
-	std::map<int, Signature> signatures;
+	std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
 	this->getGraph(poses, links, true, true, &signatures);
 
 	std::map<int, int> mapIds;
@@ -4706,6 +4698,7 @@ int Rtabmap::detectMoreLoopClosures(
 
 						if(!t.isNull())
 						{
+							UWARN(t.prettyPrint().c_str());
 							bool updateConstraints = true;
 							if(_optimizationMaxError > 0.0f)
 							{

corelib/src/SensorData.cpp

@@ -763,9 +763,10 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
 	_descriptors = descriptors;
 }
 
-long SensorData::getMemoryUsed() const // Return memory usage in Bytes
+unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
-	return _imageCompressed.total()*_imageCompressed.elemSize() +
+	return sizeof(SensorData) +
+			_imageCompressed.total()*_imageCompressed.elemSize() +
 			_imageRaw.total()*_imageRaw.elemSize() +
 			_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
 			_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
@@ -779,8 +780,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
 			_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
 			_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
-			_keypoints.size() * sizeof(float) * 7 +
+			_keypoints.size() * sizeof(cv::KeyPoint) +
-			_keypoints3D.size() * sizeof(float)*3 +
+			_keypoints3D.size() * sizeof(cv::Point3f) +
 			_descriptors.total()*_descriptors.elemSize();
 }
 

corelib/src/Signature.cpp

@@ -132,6 +132,18 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 	{
 		return _links.find(idTo) != _links.end();
 	}
+	if(idTo==0)
+	{
+		for(std::multimap<int, Link>::const_iterator iter=_links.begin(); iter!=_links.end(); ++iter)
+		{
+			if(type == iter->second.type())
+			{
+				return true;
+			}
+		}
+	}
+	else
+	{
 		for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
 		{
 			if(type == iter->second.type())
@@ -139,6 +151,7 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 				return true;
 			}
 		}
+	}
 	return false;
 }
 
@@ -209,11 +222,11 @@ void Signature::removeVirtualLinks()
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
-	const std::multimap<int, cv::KeyPoint> & words = s.getWords();
+	const std::multimap<int, int> & words = s.getWords();
 
 	if(!s.isBadSignature() && !this->isBadSignature())
 	{
-		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+		std::list<std::pair<int, std::pair<int, int> > > pairs;
 		int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
 		UASSERT(totalWords > 0);
 		EpipolarGeometry::findPairs(words, _words, pairs);
@@ -225,11 +238,9 @@ float Signature::compareTo(const Signature & s) const
 
 void Signature::changeWordsRef(int oldWordId, int activeWordId)
 {
-	std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
+	std::list<int> words = uValues(_words, oldWordId);
-	if(kps.size())
+	if(words.size())
 	{
-		std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
-		std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
 		if(oldWordId<=0)
 		{
 			_invalidWordsCount-=(int)_words.erase(oldWordId);
@@ -239,37 +250,41 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 		{
 			_words.erase(oldWordId);
 		}
-		_words3.erase(oldWordId);
+
-		_wordsDescriptors.erase(oldWordId);
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
-		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
+		for(std::list<int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 		{
-			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
+			_words.insert(std::pair<int, int>(activeWordId, (*iter)));
-		}
-		for(std::list<cv::Point3f>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
-		{
-			_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
-		}
-		for(std::list<cv::Mat>::const_iterator iter=descriptors.begin(); iter!=descriptors.end(); ++iter)
-		{
-			_wordsDescriptors.insert(std::pair<int, cv::Mat>(activeWordId, (*iter)));
 		}
 	}
 }
 
-void Signature::setWords(const std::multimap<int, cv::KeyPoint> & words)
+void Signature::setWords(const std::multimap<int, int> & words,
+		const std::vector<cv::KeyPoint> & keypoints,
+		const std::vector<cv::Point3f> & points,
+		const cv::Mat & descriptors)
 {
-	_enabled = false;
+	UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
-	_words = words;
+	UASSERT_MSG(points.empty() || points.size() == words.size(),  uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
+	UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(),  uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
+	UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
+
 	_invalidWordsCount = 0;
-	for(std::multimap<int, cv::KeyPoint>::iterator iter=_words.begin(); iter!=_words.end(); ++iter)
+	for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 	{
-		if(iter->first>0)
+		if(iter->first<=0)
 		{
-			break;
-		}
 			++_invalidWordsCount;
 		}
+		// make sure indexes are all valid!
+		UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
+	}
+
+	_enabled = false;
+	_words = words;
+	_wordsKpts = keypoints;
+	_words3 = points;
+	_wordsDescriptors = descriptors.clone();
 }
 
 bool Signature::isBadSignature() const
@@ -280,24 +295,30 @@ bool Signature::isBadSignature() const
 void Signature::removeAllWords()
 {
 	_words.clear();
+	_wordsKpts.clear();
 	_words3.clear();
-	_wordsDescriptors.clear();
+	_wordsDescriptors = cv::Mat();
 	_invalidWordsCount = 0;
 }
 
-void Signature::removeWord(int wordId)
+void Signature::setWordsDescriptors(const cv::Mat & descriptors)
 {
-	if(wordId<=0)
+	if(descriptors.empty())
 	{
-		_invalidWordsCount-=(int)_words.erase(wordId);
+		if(_wordsKpts.empty() && _words3.empty())
-		UASSERT(_invalidWordsCount>=0);
+		{
+			removeAllWords();
 		}
 		else
 		{
-		_words.erase(wordId);
+			_wordsDescriptors = cv::Mat();
+		}
+	}
+	else
+	{
+		UASSERT(descriptors.rows == (int)_words.size());
+		_wordsDescriptors = descriptors.clone();
 	}
-	_words3.erase(wordId);
-	_wordsDescriptors.clear();
 }
 
 cv::Mat Signature::getPoseCovariance() const
@@ -321,19 +342,23 @@ cv::Mat Signature::getPoseCovariance() const
 	return covariance;
 }
 
-long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
+unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
 {
-	long total =  _words.size() * sizeof(float) * 8 +
+	unsigned long total = sizeof(Signature);
-				  _words3.size() * sizeof(float) * 4;
+	total += _words.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, cv::KeyPoint>);
-	if(!_wordsDescriptors.empty())
+	total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
-	{
+	total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
-		total += _wordsDescriptors.size() * sizeof(int);
+	total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
-		total += _wordsDescriptors.size() * _wordsDescriptors.begin()->second.total() * _wordsDescriptors.begin()->second.elemSize();
+	total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
-	}
 	if(withSensorData)
 	{
 		total+=_sensorData.getMemoryUsed();
 	}
+	total += _pose.size() * (sizeof(Transform) + sizeof(float)*12);
+	total += _groundTruthPose.size() * (sizeof(Transform) + sizeof(float)*12);
+	total += _velocity.size() * sizeof(float);
+	total += _links.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, Link>);
+	total += _landmarks.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
 	return total;
 }
 

corelib/src/VWDictionary.cpp

@@ -330,8 +330,11 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
 	bool update = _strategy != strategy;
 	_strategy = strategy;
 	if(update)
+	{
+		if(_notIndexedWords.size() != _visualWords.size() || !_dataTree.empty())
 		{
 			UINFO("Nearest neighbor strategy has changed, re-initialize search tree.");
+		}
 		_dataTree = cv::Mat();
 		_notIndexedWords = uKeysSet(_visualWords);
 		_removedIndexedWords.clear();
@@ -363,6 +366,38 @@ unsigned int VWDictionary::getIndexMemoryUsed() const
 	return _flannIndex->memoryUsed();
 }
 
+unsigned long VWDictionary::getMemoryUsed(bool estimate) const
+{
+	long memoryUsage = sizeof(VWDictionary);
+	memoryUsage += getIndexMemoryUsed();
+	memoryUsage += _dataTree.total()*_dataTree.elemSize();
+	if(estimate)
+	{
+		if(!_visualWords.empty())
+		{
+			memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.begin()->second->getMemoryUsed()+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
+		}
+	}
+	else
+	{
+		for(std::map<int, VisualWord *>::const_iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
+		{
+			memoryUsage += sizeof(int) + iter->second->getMemoryUsed();
+		}
+		memoryUsage += _visualWords.size()*(sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
+	}
+	if(!_unusedWords.empty())
+	{
+		// they are the same words than in _visualWords, so just add the pointer size
+		memoryUsage += _unusedWords.size()*(sizeof(int) + sizeof(VisualWord *)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
+	}
+	memoryUsage += _mapIndexId.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
+	memoryUsage += _mapIdIndex.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
+	memoryUsage += _notIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
+	memoryUsage += _removedIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
+	return memoryUsage;
+}
+
 cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat)
 {
 	if(byteToFloat)
@@ -521,7 +556,8 @@ void VWDictionary::update()
 					{
 						UASSERT(descriptor.cols == _flannIndex->featuresDim());
 						UASSERT(descriptor.type() == _flannIndex->featuresType());
-						index = _flannIndex->addPoints(descriptor);
+						UASSERT(descriptor.rows == 1);
+						index = _flannIndex->addPoints(descriptor).front();
 					}
 					std::pair<std::map<int, int>::iterator, bool> inserted;
 					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
@@ -628,15 +664,15 @@ void VWDictionary::update()
 				switch(_strategy)
 				{
 				case kNNFlannNaive:
-					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
+					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 					break;
 				case kNNFlannKdTree:
 					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
+					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 					break;
 				case kNNFlannLSH:
 					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
+					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
 					break;
 				default:
 					break;

corelib/src/VisualWord.cpp

@@ -69,4 +69,13 @@ int VisualWord::removeAllRef(int signatureId)
 	return removed;
 }
 
+unsigned long VisualWord::getMemoryUsed() const
+{
+	unsigned long memoryUsage = sizeof(VisualWord);
+	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
+	memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
+	memoryUsage += _descriptor.total() * _descriptor.elemSize();
+	return memoryUsage;
+}
+
 } // namespace rtabmap

corelib/src/odometry/OdometryF2F.cpp

@@ -137,9 +137,7 @@ Transform OdometryF2F::computeTransform(
 		{
 			tmpRefFrame = refFrame_;
 			// reset matches, but keep already extracted features in newFrame.sensorData()
-			newFrame.setWords(std::multimap<int, cv::KeyPoint>());
+			newFrame.removeAllWords();
-			newFrame.setWords3(std::multimap<int, cv::Point3f>());
-			newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
 			// If optical flow is used, switch temporary to feature matching
 			int visCorTypeBackup = Parameters::defaultVisCorType();
@@ -176,18 +174,18 @@ Transform OdometryF2F::computeTransform(
 
 		if(info && this->isInfoDataFilled())
 		{
-			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
+			std::list<std::pair<int, std::pair<int, int> > > pairs;
 			EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
 			info->refCorners.resize(pairs.size());
 			info->newCorners.resize(pairs.size());
 			std::map<int, int> idToIndex;
 			int i=0;
-			for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
+			for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter=pairs.begin();
 				iter!=pairs.end();
 				++iter)
 			{
-				info->refCorners[i] = iter->second.first.pt;
+				info->refCorners[i] = tmpRefFrame.getWordsKpts()[iter->second.first].pt;
-				info->newCorners[i] = iter->second.second.pt;
+				info->newCorners[i] = newFrame.getWordsKpts()[iter->second.second].pt;
 				idToIndex.insert(std::make_pair(iter->first, i));
 				++i;
 			}
@@ -199,12 +197,21 @@ Transform OdometryF2F::computeTransform(
 			}
 
 			Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
-			for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
+			if(!tmpRefFrame.getWords3().empty())
 			{
-				info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
+				for(std::multimap<int, int>::const_iterator iter=tmpRefFrame.getWords().begin(); iter!=tmpRefFrame.getWords().end(); ++iter)
+				{
+					info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(tmpRefFrame.getWords3()[iter->second], t)));
+				}
 			}
 			info->localMapSize = tmpRefFrame.getWords3().size();
-			info->words = newFrame.getWords();
+			if(!newFrame.getWordsKpts().empty())
+			{
+				for(std::multimap<int, int>::const_iterator iter=newFrame.getWords().begin(); iter!=newFrame.getWords().end(); ++iter)
+				{
+					info->words.insert(std::make_pair(iter->first, newFrame.getWordsKpts()[iter->second]));
+				}
+			}
 
 			info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().size();
 
@@ -232,7 +239,7 @@ Transform OdometryF2F::computeTransform(
 			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
-			int features = newFrame.getWordsDescriptors().size();
+			int features = newFrame.getWordsDescriptors().rows;
 			if(registrationPipeline_->isImageRequired() && features == 0)
 			{
 				newFrame = Signature(data);
@@ -251,9 +258,7 @@ Transform OdometryF2F::computeTransform(
 			{
 				refFrame_ = newFrame;
 
-				refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
+				refFrame_.removeAllWords();
-				refFrame_.setWords3(std::multimap<int, cv::Point3f>());
-				refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
 
 				//reset motion
 				lastKeyFramePose_.setNull();

corelib/src/odometry/OdometryF2M.cpp

@@ -133,6 +133,27 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	}
 	uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
 
+	int estType = Parameters::defaultVisEstimationType();
+	Parameters::parse(parameters, Parameters::kVisEstimationType(), estType);
+	if(estType > 1)
+	{
+		UWARN("%s=%d is not supported by OdometryF2M, using 2D->3D approach instead (type=1).",
+				Parameters::kVisEstimationType().c_str(),
+				estType);
+		estType = 1;
+	}
+	uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
+
+	bool forwardEst = Parameters::defaultVisForwardEstOnly();
+	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
+	if(!forwardEst)
+	{
+		UWARN("%s=false is not supported by OdometryF2M, setting to true.",
+				Parameters::kVisForwardEstOnly().c_str());
+		forwardEst = true;
+	}
+	uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
+
 	regPipeline_ = Registration::create(bundleParameters);
 	if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
 	{
@@ -272,9 +293,7 @@ Transform OdometryF2M::computeTransform(
 			{
 				tmpMap = *map_;
 				// reset matches, but keep already extracted features in lastFrame_->sensorData()
-				lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
+				lastFrame_->removeAllWords();
-				lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
-				lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
 
 				points3DMap.clear();
 				bundlePoses.clear();
@@ -393,11 +412,9 @@ Transform OdometryF2M::computeTransform(
 								int wordId =regInfo.inliersIDs[i];
 
 								// 3D point
-								std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
+								std::multimap<int, int>::const_iterator iter3D = tmpMap.getWords().find(wordId);
-								UASSERT(iter3D!=tmpMap.getWords3().end());
+								UASSERT(iter3D!=tmpMap.getWords().end() && !tmpMap.getWords3().empty());
-								points3DMap.insert(*iter3D);
+								points3DMap.insert(std::make_pair(wordId, tmpMap.getWords3()[iter3D->second]));
-
-								std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
 
 								// all other references
 								std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
@@ -427,12 +444,19 @@ Transform OdometryF2M::computeTransform(
 									}
 								}
 
+								std::multimap<int, int>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
 								if(iter2D!=lastFrame_->getWords().end())
 								{
-									UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
+									UASSERT(!lastFrame_->getWordsKpts().empty());
+									//get depth
+									float d = 0.0f;
+									if( !lastFrame_->getWords3().empty() &&
+										util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
+									{
 										//move back point in camera frame (to get depth along z)
-									cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
+										d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], invLocalTransform).z;
-									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
+									}
+									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(lastFrame_->getWordsKpts()[iter2D->second], d)));
 								}
 								wordReferences.insert(std::make_pair(wordId, references));
 
@@ -557,9 +581,10 @@ Transform OdometryF2M::computeTransform(
 
 				// fields to update
 				LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
-				std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
+				std::multimap<int, int> mapWords = tmpMap.getWords();
-				std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
+				std::vector<cv::KeyPoint> mapWordsKpts = tmpMap.getWordsKpts();
-				std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
+				std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
+				cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
 
 				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
 						 (keyFrameThr_ == 0.0f ||
@@ -590,8 +615,9 @@ Transform OdometryF2M::computeTransform(
 
 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
-					UASSERT(mapPoints.size() == mapDescriptors.size());
+					UASSERT(mapWords.size() == mapWordsKpts.size());
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					UASSERT((int)mapPoints.size() == mapDescriptors.rows);
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
 
 					std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
 					if(bundleAdjustment_>0)
@@ -613,17 +639,15 @@ Transform OdometryF2M::computeTransform(
 						// update local map 3D points (if bundle adjustment was done)
 						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 						{
-							UASSERT(mapPoints.count(iter->first) == 1);
+							UASSERT(mapWords.count(iter->first) == 1);
-							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
+							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
-							mapPoints.find(iter->first)->second = iter->second;
+							mapPoints[mapWords.find(iter->first)->second] = iter->second;
 						}
 					}
 
 					// sort by feature response
 					std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-					std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
 					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
 					std::set<int> seenStatusUpdated;
 					Transform invLocalTransform;
@@ -648,11 +672,11 @@ Transform OdometryF2M::computeTransform(
 					if(!visDepthAsMask && validDepthRatio_ < 1.0f)
 					{
 						int ptsWithDepth = 0;
-						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+						for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
 							iter != lastFrame_->getWords3().end();
 							++iter)
 						{
-							if(util3d::isFinite(iter->second))
+							if(util3d::isFinite(*iter))
 							{
 								++ptsWithDepth;
 							}
@@ -666,27 +690,29 @@ Transform OdometryF2M::computeTransform(
 						}
 					}
 
-					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
+					for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
 					{
-						if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
+						const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
+						const cv::KeyPoint & kpt = lastFrame_->getWordsKpts()[iter->second];
+						if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
 						{
-							if(util3d::isFinite(iter->second) || addPointsWithoutDepth)
+							if(util3d::isFinite(pt) || addPointsWithoutDepth)
 							{
 								newIds.insert(
-										std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
+										std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
 												std::make_pair(iter->first,
-														std::make_pair(iter2D->second,
+														std::make_pair(kpt,
-																std::make_pair(iter->second, iterDesc->second)))));
+																std::make_pair(pt, lastFrame_->getWordsDescriptors().row(iter->second))))));
 							}
 						}
 						else if(bundleAdjustment_>0)
 						{
 							if(lastFrame_->getWords().count(iter->first) == 1)
 							{
-								std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
+								std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
-								if(iterKpts!=mapWords.end())
+								if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
 								{
-									iterKpts->second.octave = iter2D->second.octave;
+									mapWordsKpts[iterKpts->second].octave = kpt.octave;
 								}
 
 								UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
@@ -694,19 +720,19 @@ Transform OdometryF2M::computeTransform(
 
 								//move back point in camera frame (to get depth along z)
 								float depth = 0.0f;
-								if(util3d::isFinite(iter->second))
+								if(util3d::isFinite(pt))
 								{
-									depth = util3d::transformPoint(iter->second, invLocalTransform).z;
+									depth = util3d::transformPoint(pt, invLocalTransform).z;
 								}
 								if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
 								{
 									std::map<int, FeatureBA> framePt;
-									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
 									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
 								else
 								{
-									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
+									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
 								}
 							}
 						}
@@ -747,7 +773,8 @@ Transform OdometryF2M::computeTransform(
 								}
 							}
 
-							mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
+							mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
+							mapWordsKpts.push_back(iter->second.second.first);
 							cv::Point3f pt = iter->second.second.second.first;
 							if(!util3d::isFinite(pt))
 							{
@@ -783,8 +810,8 @@ Transform OdometryF2M::computeTransform(
 								float scaleInf = (0.05 * model.fx()) / 0.01;
 								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
 							}
-							mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(pt, newFramePose)));
+							mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
-							mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
+							mapDescriptors.push_back(iter->second.second.second.second);
 							if(lastFrameOldestNewId_ > iter->second.first)
 							{
 								lastFrameOldestNewId_ = iter->second.first;
@@ -794,7 +821,7 @@ Transform OdometryF2M::computeTransform(
 					}
 
 					// remove words in map if max size is reached
-					if((int)mapPoints.size() > maximumMapSize_)
+					if((int)mapWords.size() > maximumMapSize_)
 					{
 						// remove oldest outliers first
 						std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
@@ -813,7 +840,7 @@ Transform OdometryF2M::computeTransform(
 							ids.resize(regInfo.matchesIDs.size()+oi);
 							UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
 						}
-						for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
+						for(unsigned int i=0; i<ids.size() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size(); ++i)
 						{
 							int id = ids.at(i);
 							if(inliers.find(id) == inliers.end())
@@ -831,18 +858,14 @@ Transform OdometryF2M::computeTransform(
 									bundleWordReferences_.erase(iterRef);
 								}
 
-								mapPoints.erase(id);
-								mapDescriptors.erase(id);
 								mapWords.erase(id);
 								++removed;
 							}
 						}
 
 						// remove oldest first
-						std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
+						for(std::multimap<int, int>::iterator iter = mapWords.begin();
-						std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
+							iter!=mapWords.end() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size();)
-						for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
-							iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
 						{
 							if(inliers.find(iter->first) == inliers.end())
 							{
@@ -859,19 +882,36 @@ Transform OdometryF2M::computeTransform(
 									bundleWordReferences_.erase(iterRef);
 								}
 
-								mapPoints.erase(iter++);
+								mapWords.erase(iter++);
-								mapDescriptors.erase(iterMapDescriptors++);
-								mapWords.erase(iterMapWords++);
 								++removed;
 							}
 							else
 							{
 								++iter;
-								++iterMapDescriptors;
-								++iterMapWords;
 							}
 						}
 
+						if(mapWords.size() != mapPoints.size())
+						{
+							UDEBUG("Remove points");
+							std::vector<cv::KeyPoint> mapWordsKptsClean(mapWords.size());
+							std::vector<cv::Point3f> mapPointsClean(mapWords.size());
+							cv::Mat mapDescriptorsClean(mapWords.size(), mapDescriptors.cols, mapDescriptors.type());
+							int index = 0;
+							for(std::multimap<int, int>::iterator iter = mapWords.begin(); iter!=mapWords.end(); ++iter, ++index)
+							{
+								mapWordsKptsClean[index] = mapWordsKpts[iter->second];
+								mapPointsClean[index] = mapPoints[iter->second];
+								mapDescriptors.row(iter->second).copyTo(mapDescriptorsClean.row(index));
+								iter->second = index;
+							}
+							mapWordsKpts = mapWordsKptsClean;
+							mapWordsKptsClean.clear();
+							mapPoints = mapPointsClean;
+							mapPointsClean.clear();
+							mapDescriptors = mapDescriptorsClean;
+						}
+
 						Link * previousLink = 0;
 						for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
 						{
@@ -1099,9 +1139,7 @@ Transform OdometryF2M::computeTransform(
 										newFramePose.translation()));
 					}
 
-					map_->setWords(mapWords);
+					map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
-					map_->setWords3(mapPoints);
-				 	map_->setWordsDescriptors(mapDescriptors);
 				}
 			}
 
@@ -1112,7 +1150,14 @@ Transform OdometryF2M::computeTransform(
 				info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
 				if(this->isInfoDataFilled())
 				{
-					info->localMap = uMultimapToMap(tmpMap.getWords3());
+					info->localMap.clear();
+					if(!tmpMap.getWords3().empty())
+					{
+						for(std::multimap<int, int>::const_iterator iter=tmpMap.getWords().begin(); iter!=tmpMap.getWords().end(); ++iter)
+						{
+							info->localMap.insert(std::make_pair(iter->first, tmpMap.getWords3()[iter->second]));
+						}
+					}
 					info->localScanMap = tmpMap.sensorData().laserScanRaw();
 				}
 			}
@@ -1139,11 +1184,12 @@ Transform OdometryF2M::computeTransform(
 			if(regPipeline_->isImageRequired())
 			{
 				int ptsWithDepth = 0;
-				for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+				for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
-					iter != lastFrame_->getWords3().end();
+					iter != lastFrame_->getWords().end();
 					++iter)
 				{
-					if(util3d::isFinite(iter->second))
+					if(!lastFrame_->getWords3().empty() && 
+					   util3d::isFinite(lastFrame_->getWords3()[iter->second]))
 					{
 						++ptsWithDepth;
 					}
@@ -1153,26 +1199,29 @@ Transform OdometryF2M::computeTransform(
 				{
 					frameValid = true;
 					// update local map
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
 
-					std::multimap<int, cv::KeyPoint> words;
+					std::multimap<int, int> words;
-					std::multimap<int, cv::Point3f> transformedPoints;
+					std::vector<cv::KeyPoint> wordsKpts;
+					std::vector<cv::Point3f> transformedPoints;
 					std::multimap<int, int> mapPointWeights;
-					std::multimap<int, cv::Mat> descriptors;
+					cv::Mat descriptors;
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
+					if(!lastFrame_->getWords3().empty())
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
-					for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-						iter != lastFrame_->getWords3().end();
-						++iter, ++descIter, ++wordsIter)
 					{
-						if (util3d::isFinite(iter->second))
+						for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
+							iter != lastFrame_->getWords().end();
+							++iter)
 						{
-							words.insert(*wordsIter);
+							const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
-							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
+							if (util3d::isFinite(pt))
+							{
+								words.insert(words.end(), std::make_pair(iter->first, words.size()));
+								wordsKpts.push_back(lastFrame_->getWordsKpts()[iter->second]);
+								transformedPoints.push_back(util3d::transformPoint(pt, newFramePose));
 								mapPointWeights.insert(std::make_pair(iter->first, 0));
-							descriptors.insert(*descIter);
+								descriptors.push_back(lastFrame_->getWordsDescriptors().row(iter->second));
+							}
 						}
 					}
 
@@ -1193,7 +1242,9 @@ Transform OdometryF2M::computeTransform(
 						}
 
 						// update bundleWordReferences_: used for bundle adjustment
-						for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+						if(!wordsKpts.empty())
+						{
+							for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 							{
 								if(words.count(iter->first) == 1)
 								{
@@ -1202,18 +1253,20 @@ Transform OdometryF2M::computeTransform(
 
 									//get depth
 									float d = 0.0f;
-								if(lastFrame_->getWords3().count(iter->first) == 1 &&
+									if(lastFrame_->getWords().count(iter->first) == 1 &&
-									util3d::isFinite(lastFrame_->getWords3().find(iter->first)->second))
+									  !lastFrame_->getWords3().empty() &&
+										util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
 									{
 										//move back point in camera frame (to get depth along z)
-									d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform).z;
+										d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], invLocalTransform).z;
 									}
 
 
-								framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second, d)));
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(wordsKpts[iter->second], d)));
 									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
 							}
+						}
 
 						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
 
@@ -1246,9 +1299,7 @@ Transform OdometryF2M::computeTransform(
 						}
 					}
 
-					map_->setWords(words);
+					map_->setWords(words, wordsKpts, transformedPoints, descriptors);
-					map_->setWords3(transformedPoints);
-					map_->setWordsDescriptors(descriptors);
 					addKeyFrame = true;
 				}
 				else
@@ -1347,7 +1398,14 @@ Transform OdometryF2M::computeTransform(
 
 				if(this->isInfoDataFilled())
 				{
-					info->localMap = uMultimapToMap(map_->getWords3());
+					info->localMap.clear();
+					if(!map_->getWords3().empty())
+					{
+						for(std::multimap<int, int>::const_iterator iter=map_->getWords().begin(); iter!=map_->getWords().end(); ++iter)
+						{
+							info->localMap.insert(std::make_pair(iter->first, map_->getWords3()[iter->second]));
+						}
+					}
 					info->localScanMap = map_->sensorData().laserScanRaw();
 				}
 			}
@@ -1360,7 +1418,14 @@ Transform OdometryF2M::computeTransform(
 		{
 			if(regPipeline_->isImageRequired())
 			{
-				info->words = lastFrame_->getWords();
+				info->words.clear();
+				if(!lastFrame_->getWordsKpts().empty())
+				{
+					for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
+					{
+						info->words.insert(std::make_pair(iter->first, lastFrame_->getWordsKpts()[iter->second]));
+					}
+				}
 			}
 		}
 	}

corelib/src/odometry/OdometryMono.cpp

@@ -283,15 +283,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 						newCorners[oi] = imagePoints[i];
 						if(localMap_.count(ids[i]) == 1)
 						{
-							if(prevS->getWords().count(ids[i]) == 1)
+							if(prevS->getWords().count(ids[i]) == 1 && !prevS->getWordsKpts().empty())
 							{
 								// set guess if unique
-								refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
+								refCorners[oi] = prevS->getWordsKpts()[prevS->getWords().find(ids[i])->second].pt;
 							}
-							if(newS->getWords().count(ids[i]) == 1)
+							if(newS->getWords().count(ids[i]) == 1 && !newS->getWordsKpts().empty())
 							{
 								// set guess if unique
-								newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
+								newCorners[oi] = newS->getWordsKpts()[newS->getWords().find(ids[i])->second].pt;
 							}
 						}
 						objectPointsTmp[oi] = objectPoints[i];
@@ -338,9 +338,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 						if(this->isInfoDataFilled() && info)
 						{
 							cv::KeyPoint kpt;
-							if(newS->getWords().count(matches[i]) == 1)
+							if(newS->getWords().count(matches[i]) == 1 && !newS->getWordsKpts().empty())
 							{
-								kpt = newS->getWords().find(matches[i])->second;
+								kpt = newS->getWordsKpts()[newS->getWords().find(matches[i])->second];
 							}
 							kpt.pt = newCorners[i];
 							info->words.insert(std::make_pair(matches[i], kpt));
@@ -437,9 +437,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 							for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
 							{
 								const Signature * s = memory_->getSignature(*iter);
-								for(std::multimap<int, cv::KeyPoint>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
+								for(std::multimap<int, int>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
 								{
-									if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end())
+									if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end() && !s->getWordsKpts().empty())
 									{
 										if(wordReferences.find(jter->first)==wordReferences.end())
 										{
@@ -451,7 +451,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 										{
 											depth = keyFrameWords3D_.at(s->id()).at(jter->first).x;
 										}
-										wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(jter->second, depth, cv::Mat())));
+										const cv::KeyPoint & kpts = s->getWordsKpts()[jter->second];
+										wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(kpts, depth, cv::Mat())));
 									}
 								}
 							}
@@ -502,9 +503,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 							}
 							else if(float(inliers)/float(imagePoints.size()) < keyFrameThr_)
 							{
+								std::map<int, int> uniqueWordsPrevious = uMultimapToMapUnique(previousS->getWords());
+								std::map<int, int> uniqueWordsNew = uMultimapToMapUnique(newS->getWords());
+								std::map<int, cv::KeyPoint> wordsPrevious;
+								std::map<int, cv::KeyPoint> wordsNew;
+								for(std::map<int, int>::iterator iter=uniqueWordsPrevious.begin(); iter!=uniqueWordsPrevious.end(); ++iter)
+								{
+									wordsPrevious.insert(std::make_pair(iter->first, previousS->getWordsKpts()[iter->second]));
+								}
+								for(std::map<int, int>::iterator iter=uniqueWordsNew.begin(); iter!=uniqueWordsNew.end(); ++iter)
+								{
+									wordsNew.insert(std::make_pair(iter->first, newS->getWordsKpts()[iter->second]));
+								}
 								std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-										uMultimapToMapUnique(previousS->getWords()),
+										wordsPrevious,
-										uMultimapToMapUnique(newS->getWords()),
+										wordsNew,
 										cameraModel,
 										cameraTransform,
 										fundMatrixReprojError_,
@@ -626,9 +639,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		int ii=0;
 		for(std::map<int, cv::Point2f>::iterator iter=firstFrameGuessCorners_.begin(); iter!=firstFrameGuessCorners_.end(); ++iter)
 		{
-			std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
+			std::multimap<int, int>::const_iterator jter=refS->getWords().find(iter->first);
-			UASSERT(jter != refS->getWords().end());
+			UASSERT(jter != refS->getWords().end() && !refS->getWordsKpts().empty());
-			refCorners[ii] = jter->second.pt;
+			refCorners[ii] = refS->getWordsKpts()[jter->second].pt;
 			refCornersGuess[ii] = iter->second;
 			cornerIds[ii] = iter->first;
 			++ii;
@@ -800,14 +813,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			// generate kpts
 		if(memory_->update(SensorData(data)))
 		{
-			const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
+			const Signature * s = memory_->getLastWorkingSignature();
-			if((int)words.size() > minInliers_)
+			const std::multimap<int, int> & words = s->getWords();
+			if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
 			{
-				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+				for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 				{
 					if(words.count(iter->first) == 1)
 					{
-						firstFrameGuessCorners_.insert(std::make_pair(iter->first, iter->second.pt));
+						firstFrameGuessCorners_.insert(std::make_pair(iter->first, s->getWordsKpts()[iter->second].pt));
 					}
 				}
 			}

guilib/include/rtabmap/utilite/UPlot.h

@@ -398,6 +398,7 @@ public:
 	bool isFlat() const {return _flat;}
 	void addItem(UPlotCurve * curve);
 	bool remove(const UPlotCurve * curve);
+	QString getAllCurveDataAsText() const;
 
 private Q_SLOTS:
 	void removeLegendItem(const UPlotCurve * curve);
@@ -510,6 +511,7 @@ public:
 	QStringList curveNames();
 	bool contains(const QString & curveName);
 	void removeCurves();
+	QString getAllCurveDataAsText() const;
 	/**
 	 * Add a threshold to the plot.
 	 */

guilib/src/DatabaseViewer.cpp

@@ -4163,9 +4163,14 @@ void DatabaseViewer::update(int value,
 				std::list<Signature*> signatures;
 				dbDriver_->loadSignatures(ids, signatures);
 
-				if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
+				if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
 				{
-					view->setFeatures(signatures.front()->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
+					std::multimap<int, cv::KeyPoint> keypoints;
+					for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
+					{
+						keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
+					}
+					view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
 				}
 
 				Transform odomPose, g;
@@ -4522,16 +4527,19 @@ void DatabaseViewer::update(int value,
 					}
 
 					//words
-					if(ui_->checkBox_showWords->isChecked() && signatures.size())
+					if(ui_->checkBox_showWords->isChecked() &&
+						!signatures.empty() &&
+						!(*signatures.begin())->getWords3().empty())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 						cloud->resize((*signatures.begin())->getWords3().size());
 						int i=0;
-						for(std::multimap<int, cv::Point3f>::const_iterator iter=(*signatures.begin())->getWords3().begin();
+						for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
-							iter!=(*signatures.begin())->getWords3().end();
+							iter!=(*signatures.begin())->getWords().end();
 							++iter)
 						{
-							cloud->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+							const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
+							cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 						}
 
 						if(cloud->size())
@@ -5593,20 +5601,25 @@ void DatabaseViewer::updateConstraintView(
 					cloudTo->resize(sTo->getWords3().size());
 				}
 				int i=0;
-				for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
+				if(!sFrom->getWords3().empty())
-					iter!=sFrom->getWords3().end();
+				{
+					for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
+						iter!=sFrom->getWords().end();
 						++iter)
 					{
-					cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+						const cv::Point3f & pt = sFrom->getWords3()[iter->second];
+						cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+					}
 				}
 				i=0;
-				if(sTo)
+				if(sTo && !sTo->getWords3().empty())
 				{
-					for(std::multimap<int, cv::Point3f>::const_iterator iter=sTo->getWords3().begin();
+					for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
-						iter!=sTo->getWords3().end();
+						iter!=sTo->getWords().end();
 						++iter)
 					{
-						cloudTo->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+						const cv::Point3f & pt = sTo->getWords3()[iter->second];
+						cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 					}
 				}
 
@@ -7074,13 +7087,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 
 		if(reextractVisualFeatures)
 		{
-			fromS->setWords(std::multimap<int, cv::KeyPoint>());
+			fromS->removeAllWords();
-			fromS->setWords3(std::multimap<int, cv::Point3f>());
-			fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 			fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-			toS->setWords(std::multimap<int, cv::KeyPoint>());
+			toS->removeAllWords();
-			toS->setWords3(std::multimap<int, cv::Point3f>());
-			toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 			toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 
@@ -7192,17 +7201,33 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			if(toS && fromS->id() > 0 && toS->id() > 0)
 			{
 				updateLoopClosuresSlider(fromS->id(), toS->id());
+				std::multimap<int, cv::KeyPoint> keypointsFrom;
+				std::multimap<int, cv::KeyPoint> keypointsTo;
+				if(!fromS->getWordsKpts().empty())
+				{
+					for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
+					{
+						keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
+					}
+				}
+				if(!toS->getWordsKpts().empty())
+				{
+					for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
+					{
+						keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
+					}
+				}
 				if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
 				{
 					this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
-					ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+					ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
-					ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+					ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
 				}
 				else
 				{
 					this->updateConstraintView(newLink, true, *fromS, *toS);
-					ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+					ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
-					ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+					ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
 				}
 
 				updateWordsMatching(info.inliersIDs);
@@ -7218,8 +7243,32 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		if(toS && fromS->id() > 0 && toS->id() > 0)
 		{
 			// just update matches in the views
-			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			std::multimap<int, cv::KeyPoint> keypointsFrom;
-			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+			std::multimap<int, cv::KeyPoint> keypointsTo;
+			if(!fromS->getWordsKpts().empty())
+			{
+				for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
+				{
+					keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
+				}
+			}
+			if(!toS->getWordsKpts().empty())
+			{
+				for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
+				{
+					keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
+				}
+			}
+			if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
+			{
+				ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
+				ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
+			}
+			else
+			{
+				ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
+				ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
+			}
 			updateWordsMatching(info.inliersIDs);
 		}
 
@@ -7306,13 +7355,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			toS->sensorData().uncompressData();
 			if(reextractVisualFeatures)
 			{
-				fromS->setWords(std::multimap<int, cv::KeyPoint>());
+				fromS->removeAllWords();
-				fromS->setWords3(std::multimap<int, cv::Point3f>());
-				fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 				fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-				toS->setWords(std::multimap<int, cv::KeyPoint>());
+				toS->removeAllWords();
-				toS->setWords3(std::multimap<int, cv::Point3f>());
-				toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 				toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			}
 		}
@@ -7565,8 +7610,24 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				this->updateConstraintView(newLink, false, *fromS, *toS);
 			}
 
-			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			std::multimap<int, cv::KeyPoint> keypointsFrom;
-			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+			std::multimap<int, cv::KeyPoint> keypointsTo;
+			if(!fromS->getWordsKpts().empty())
+			{
+				for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
+				{
+					keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
+				}
+			}
+			if(!toS->getWordsKpts().empty())
+			{
+				for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
+				{
+					keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
+				}
+			}
+			ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
+			ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
 			updateWordsMatching(info.inliersIDs);
 		}
 		else if(updateConstraints)

guilib/src/MainWindow.cpp

@@ -1715,13 +1715,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					_cachedSignatures.insert(signature.id(), signature);
 					_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
 					unsigned int count = 0;
-					for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().upper_bound(-1); jter!=signature.getWords3().end(); ++jter)
+					if(!signature.getWords3().empty())
 					{
-						if(util3d::isFinite(jter->second))
+						for(std::multimap<int, int>::const_iterator jter=signature.getWords().upper_bound(-1); jter!=signature.getWords().end(); ++jter)
+						{
+							if(util3d::isFinite(signature.getWords3()[jter->second]))
 							{
 								++count;
 							}
 						}
+					}
 					_cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
 				}
 			}
@@ -1952,7 +1955,17 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			UDEBUG("time= %d ms", time.restart());
 
 			// do it after scaling
-			this->drawKeypoints(signature.getWords(), loopSignature.getWords());
+			std::multimap<int, cv::KeyPoint> wordsA;
+			std::multimap<int, cv::KeyPoint> wordsB;
+			for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
+			{
+				wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
+			}
+			for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
+			{
+				wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
+			}
+			this->drawKeypoints(wordsA, wordsB);
 
 			UDEBUG("time= %d ms", time.restart());
 
@@ -3706,19 +3719,35 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
 		cloud->resize(iter->getWords3().size());
 		int oi=0;
 		UASSERT(iter->getWords().size() == iter->getWords3().size());
-		std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
 		float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
 		UDEBUG("rgb.channels()=%d");
-		for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
+		if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
-				jter!=iter->getWords3().end(); ++jter, ++kter)
 		{
-			if(util3d::isFinite(jter->second) && (maxDepth == 0.0f || jter->second.z < maxDepth))
+			Transform invLocalTransform = Transform::getIdentity();
+			if(iter.value().sensorData().cameraModels().size() == 1 && iter.value().sensorData().cameraModels().at(0).isValidForProjection())
 			{
-				(*cloud)[oi].x = jter->second.x;
+				invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().inverse();
-				(*cloud)[oi].y = jter->second.y;
+			}
-				(*cloud)[oi].z = jter->second.z;
+			else if(iter.value().sensorData().stereoCameraModel().isValidForProjection())
-				int u = kter->second.pt.x+0.5;
+			{
-				int v = kter->second.pt.y+0.5;
+				invLocalTransform = iter.value().sensorData().stereoCameraModel().left().localTransform().inverse();
+			}
+
+			for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
+			{
+				const cv::Point3f & pt = iter->getWords3()[jter->second];
+				if(util3d::isFinite(pt) &&
+					(maxDepth == 0.0f ||
+							//move back point in camera frame (to get depth along z), ignore for multi-camera
+							(iter.value().sensorData().cameraModels().size()<=1 &&
+							 util3d::transformPoint(pt, invLocalTransform).z < maxDepth)))
+				{
+					(*cloud)[oi].x = pt.x;
+					(*cloud)[oi].y = pt.y;
+					(*cloud)[oi].z = pt.z;
+					const cv::KeyPoint & kpt = iter->getWordsKpts()[jter->second];
+					int u = kpt.pt.x+0.5;
+					int v = kpt.pt.y+0.5;
 					if(!rgb.empty() &&
 						uIsInBounds(u, 0, rgb.cols-1) &&
 						uIsInBounds(v, 0, rgb.rows-1))
@@ -3742,6 +3771,7 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
 					++oi;
 				}
 			}
+		}
 		cloud->resize(oi);
 		if(!_cloudViewer->addCloud(cloudName, cloud, pose, color))
 		{
@@ -4062,13 +4092,16 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 				_cachedSignatures.insert(iter->first, iter->second);
 				_cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
 				unsigned int count = 0;
-				for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->second.getWords3().upper_bound(-1); jter!=iter->second.getWords3().end(); ++jter)
+				if(!iter->second.getWords3().empty())
 				{
-					if(util3d::isFinite(jter->second))
+					for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
+					{
+						if(util3d::isFinite(iter->second.getWords3()[jter->second]))
 						{
 							++count;
 						}
 					}
+				}
 				_cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
 				++addedSignatures;
 			}
@@ -5874,13 +5907,9 @@ void MainWindow::postProcessing()
 										}
 										else
 										{
-											signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
+											signatureFrom.removeAllWords();
-											signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
-											signatureFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
 											signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-											signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
+											signatureTo.removeAllWords();
-											signatureTo.setWords3(std::multimap<int, cv::Point3f>());
-											signatureTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 											signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 										}
 									}

guilib/src/utilite/UPlot.cpp

@@ -1678,15 +1678,8 @@ void UPlotLegend::moveDown(UPlotLegendItem * item)
 	}
 }
 
-void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
+QString UPlotLegend::getAllCurveDataAsText() const
 {
-	QAction * action = _menu->exec(event->globalPos());
-	if(action == _aUseFlatButtons)
-	{
-		this->setFlat(_aUseFlatButtons->isChecked());
-	}
-	else if(action == _aCopyAllCurvesToClipboard)
-	{
 	QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
 	if(items.size())
 	{
@@ -1759,10 +1752,27 @@ void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
 					text.append("\n");
 				}
 			}
-				QClipboard * clipboard = QApplication::clipboard();
+			return text;
-				clipboard->setText(text);
 		}
 	}
+	return "";
+}
+
+void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
+{
+	QAction * action = _menu->exec(event->globalPos());
+	if(action == _aUseFlatButtons)
+	{
+		this->setFlat(_aUseFlatButtons->isChecked());
+	}
+	else if(action == _aCopyAllCurvesToClipboard)
+	{
+		QString data = getAllCurveDataAsText();
+		if(!data.isEmpty())
+		{
+			QClipboard * clipboard = QApplication::clipboard();
+			clipboard->setText(data);
+		}
 	}
 	else if(action == _aShowAllStdDevMeanMax)
 	{
@@ -3262,3 +3272,12 @@ void UPlot::moveCurve(const UPlotCurve * curve, int index)
 		this->update();
 	}
 }
+
+QString UPlot::getAllCurveDataAsText() const
+{
+	if(_legend)
+	{
+		return _legend->getAllCurveDataAsText();
+	}
+	return "";
+}

tools/Matcher/main.cpp

@@ -275,7 +275,7 @@ int main(int argc, char * argv[])
 
 		if(reg.getNNType()==6 &&
 		   !dataFrom.getWordsDescriptors().empty() &&
-		   dataFrom.getWordsDescriptors().begin()->second.type()!=CV_32F)
+		   dataFrom.getWordsDescriptors().type()!=CV_32F)
 		{
 			UWARN("PyMatcher is selected for matching but binary features "
 				  "are not compatible. BruteForce with CrossCheck (%s=5) "
@@ -298,7 +298,7 @@ int main(int argc, char * argv[])
 				.arg(Parameters::kVisCorNNType().c_str())
 				.arg(reg.getNNType())
 				.arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():
-						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().begin()->second.type()!=CV_32F)?"BFCrossCheck":
+						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().type()!=CV_32F)?"BFCrossCheck":
 						reg.getNNType()==6?QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", ""):
 						reg.getNNType()==7?"GMS":"?")
 				.arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
@@ -322,9 +322,21 @@ int main(int argc, char * argv[])
 			if(reg.getEstimationType() == 2)
 			{
 				// triangulate 3D words based on the transform computed
+				std::map<int, int> wordsFrom = uMultimapToMapUnique(dataFrom.getWords());
+				std::map<int, int> wordsTo = uMultimapToMapUnique(dataTo.getWords());
+				std::map<int, cv::KeyPoint> kptsFrom;
+				std::map<int, cv::KeyPoint> kptsTo;
+				for(std::map<int, int>::iterator iter=wordsFrom.begin(); iter!=wordsFrom.end(); ++iter)
+				{
+					kptsFrom.insert(std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
+				}
+				for(std::map<int, int>::iterator iter=wordsTo.begin(); iter!=wordsTo.end(); ++iter)
+				{
+					kptsTo.insert(std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
+				}
 				std::map<int, cv::Point3f> points3d = util3d::generateWords3DMono(
-						uMultimapToMapUnique(dataFrom.getWords()),
+						kptsFrom,
-						uMultimapToMapUnique(dataTo.getWords()),
+						kptsTo,
 						model.isValidForProjection()?model:stereoModel.left(),
 						t);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
@@ -353,11 +365,12 @@ int main(int argc, char * argv[])
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
 					cloudWordsFrom->resize(dataFrom.getWords3().size());
 					int i=0;
-					for(std::multimap<int, cv::Point3f>::const_iterator iter=dataFrom.getWords3().begin();
+					for(std::multimap<int, int>::const_iterator iter=dataFrom.getWords().begin();
-						iter!=dataFrom.getWords3().end();
+						iter!=dataFrom.getWords().end();
 						++iter)
 					{
-						cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+						const cv::Point3f & pt = dataFrom.getWords3()[iter->second];
+						cloudWordsFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 					}
 					if(cloudWordsFrom->size())
 					{
@@ -371,22 +384,23 @@ int main(int argc, char * argv[])
 				}
 				if(!dataTo.getWords3().empty())
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsTo(new pcl::PointCloud<pcl::PointXYZ>);
-					cloudWordsFrom->resize(dataTo.getWords3().size());
+					cloudWordsTo->resize(dataTo.getWords3().size());
 					int i=0;
-					for(std::multimap<int, cv::Point3f>::const_iterator iter=dataTo.getWords3().begin();
+					for(std::multimap<int, int>::const_iterator iter=dataTo.getWords().begin();
-						iter!=dataTo.getWords3().end();
+						iter!=dataTo.getWords().end();
 						++iter)
 					{
-						cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
+						const cv::Point3f & pt = dataTo.getWords3()[iter->second];
+						cloudWordsTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
 					}
-					if(cloudWordsFrom->size())
+					if(cloudWordsTo->size())
 					{
-						cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
+						cloudWordsTo = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsTo);
 					}
-					if(cloudWordsFrom->size())
+					if(cloudWordsTo->size())
 					{
-						viewer->addCloud("wordsTo", cloudWordsFrom, t, Qt::cyan);
+						viewer->addCloud("wordsTo", cloudWordsTo, t, Qt::cyan);
 						viewer->setCloudPointSize("wordsTo", 5);
 					}
 				}
@@ -443,8 +457,24 @@ int main(int argc, char * argv[])
 			viewB->setImageDepth(uCvMat2QImage(toDepth, false, uCvQtDepthRedToBlue));
 			viewB->setImageDepthShown(true);
 		}
-		viewA->setFeatures(dataFrom.getWords());
+		std::multimap<int, cv::KeyPoint> keypointsFrom;
-		viewB->setFeatures(dataTo.getWords());
+		std::multimap<int, cv::KeyPoint> keypointsTo;
+		if(!dataFrom.getWordsKpts().empty())
+		{
+			for(std::map<int, int>::const_iterator iter=dataFrom.getWords().begin(); iter!=dataFrom.getWords().end(); ++iter)
+			{
+				keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
+			}
+		}
+		if(!dataTo.getWordsKpts().empty())
+		{
+			for(std::map<int, int>::const_iterator iter=dataTo.getWords().begin(); iter!=dataTo.getWords().end(); ++iter)
+			{
+				keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
+			}
+		}
+		viewA->setFeatures(keypointsFrom);
+		viewB->setFeatures(keypointsTo);
 		std::set<int> inliersSet(info.inliersIDs.begin(), info.inliersIDs.end());
 
 		const QMultiMap<int, KeypointItem*> & wordsA = viewA->getFeatures();

tools/Report/main.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef WITH_QT
 #include <rtabmap/utilite/UPlot.h>
 #include <QApplication>
+#include <QFile>
 #endif
 
 using namespace rtabmap;
@@ -69,6 +70,8 @@ void showUsage()
 			"                       database, the inverse will be done. \n"
 			"    --ids              Use IDs for x axis instead of time in the figures. \n"
 			"    --start #          Start from this node ID for the figures.\n"
+			"    --export           Export figures' data to txt files.\n"
+			"    --export_prefix    Prefix to filenames of exported figures' data (default is \"Stat\").\n"
 #endif
 			"    --report           Export all evaluation statistics values in report.txt \n"
 			"    --loc #            Show localization statistics for each \"Statistic/Id\" per\n"
@@ -134,6 +137,8 @@ int main(int argc, char * argv[])
 	bool invertFigures = false;
 	bool useIds = false;
 	int startId = 0;
+	bool exportFigures = false;
+	std::string exportPrefix = "Stat";
 	int showLoc = 0;
 	float locDelay = 60;
 	std::vector<std::string> statsToShow;
@@ -186,6 +191,24 @@ int main(int argc, char * argv[])
 		{
 			useIds = true;
 		}
+		else if(strcmp(argv[i],"--export") == 0)
+		{
+			exportFigures = true;
+		}
+		else if(strcmp(argv[i],"--export_prefix") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				exportPrefix = argv[i];
+				printf("Export prefix=%s (--export_prefix)\n", exportPrefix.c_str());
+			}
+			else
+			{
+				printf("Missing value for \"--export_prefix\" option.\n");
+				showUsage();
+			}
+		}
 		else if(strcmp(argv[i],"--loc") == 0)
 		{
 			++i;
@@ -518,14 +541,14 @@ int main(int argc, char * argv[])
 
 								if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
 								{
-									if(w>=0)
+									if(w!=-1)
 									{
 										slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
 									}
 								}
 								else if(uContains(stat, Statistics::kTimingTotal()))
 								{
-									if(w>=0)
+									if(w!=-1)
 									{
 										slamTime.push_back(stat.at(Statistics::kTimingTotal()));
 									}
@@ -1171,10 +1194,35 @@ int main(int argc, char * argv[])
 			{
 				iter->second->frameData();
 			}
+			if(exportFigures)
+			{
+				QString data = iter->second->getAllCurveDataAsText();
+				if(!data.isEmpty())
+				{
+					QString filePath = QString(exportPrefix.c_str()) + (exportPrefix.empty()?"":"-") + iter->second->windowTitle().replace('/', "-") + ".txt";
+					QFile file(filePath);
+					if(file.open(QIODevice::Text | QIODevice::WriteOnly))
+					{
+						file.write(data.toUtf8());
+						file.close();
+						printf("Exported \"%s\".\n", filePath.toStdString().c_str());
+					}
+					else
+					{
+						printf("ERROR: could not open file \"%s\" for writing!\n", filePath.toStdString().c_str());
+					}
+				}
+			}
+			else
+			{
 				iter->second->show();
 			}
+		}
+		if(!exportFigures)
+		{
 			return app.exec();
 		}
+	}
 #endif
 	return 0;
 }