Added image sequence to rtabmap::Image class

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

corelib/include/rtabmap/core/Camera.h

@@ -59,7 +59,7 @@ public:
 	CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int cameraId = 0) :
 		UEvent(kCodeFeatures),
 		_cameraId(cameraId),
-		_image(image, descriptors, keypoints)
+		_image(image, 0, descriptors, keypoints)
 	{
 	}
 	CameraEvent(int cameraId = 0) :

corelib/include/rtabmap/core/Image.h

@@ -16,13 +16,18 @@
 namespace rtabmap
 {
 
+/**
+ * An id is automatically generated if id=0.
+ */
 class Image
 {
 public:
 	Image(const cv::Mat & image = cv::Mat(),
+		  int id = 0,
 		  const cv::Mat & descriptors = cv::Mat(),
 		  const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>()) :
 		_image(image),
+		_id(id),
 		_descriptors(descriptors),
 		_keypoints(keypoints)
 	{
@@ -30,11 +35,13 @@ public:
 
 	bool empty() const {return _image.empty() && _descriptors.empty() && _keypoints.size() == 0;}
 	const cv::Mat & image() const {return _image;}
+	int id() const {return _id;};
 	const cv::Mat & descriptors() const {return _descriptors;}
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
 
 private:
 	cv::Mat _image;
+	int _id;
 	cv::Mat _descriptors;
 	std::vector<cv::KeyPoint> _keypoints;
 };

corelib/include/rtabmap/core/Memory.h

@@ -142,7 +142,7 @@ protected:
 
 private:
 	void copyData(const Signature * from, Signature * to);
-	Signature * createSignature(int id,
+	Signature * createSignature(
 			const Image & image,
 			bool keepRawData=false);
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -73,7 +73,7 @@ public:
 	Rtabmap();
 	virtual ~Rtabmap();
 
-	void process(const cv::Mat & image); // for convenience
+	void process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
 	void process(const Image & image); // for convenience
 
 	void init(const ParametersMap & param, bool deleteMemory = true);
@@ -94,6 +94,7 @@ public:
 	std::map<int, int> getWeights();
 	int getTotalMemSize();
 	double getLastProcessTime() const {return _lastProcessTime;};
+	std::multimap<int, cv::KeyPoint> getWords(int nodeId);
 
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 

corelib/src/Memory.cpp

@@ -386,7 +386,7 @@ bool Memory::update(const Image & image, std::map<std::string, float> & stats)
 	//============================================================
 	// Create a signature with the image received.
 	//============================================================
-	Signature * signature = this->createSignature(this->getNextId(), image, this->isRawDataKept());
+	Signature * signature = this->createSignature(image, this->isRawDataKept());
 	if (signature == 0)
 	{
 		UERROR("Failed to create a signature");
@@ -2094,7 +2094,7 @@ private:
 	VWDictionary * _vwp;
 };
 
-Signature * Memory::createSignature(int id, const Image & image, bool keepRawData)
+Signature * Memory::createSignature(const Image & image, bool keepRawData)
 {
 	PreUpdateThread preUpdateThread(_vwd);
 
@@ -2102,6 +2102,11 @@ Signature * Memory::createSignature(int id, const Image & image, bool keepRawDat
 	timer.start();
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
+	int id = image.id();
+	if(!id)
+	{
+		id = this->getNextId();
+	}
 
 	int treeSize= _workingMem.size() + _stMem.size();
 	int nbCommonWords = 0;

corelib/src/Rtabmap.cpp

@@ -502,6 +502,23 @@ int Rtabmap::getTotalMemSize()
 	return memSize;
 }
 
+std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int nodeId)
+{
+	UScopeMutex s(&_threadMutex);
+	std::multimap<int, cv::KeyPoint> words;
+
+	if(_memory)
+	{
+		const Signature * s = _memory->getSignature(nodeId);
+		if(s)
+		{
+			words = s->getWords();
+		}
+	}
+
+	return words;
+}
+
 void Rtabmap::clearBufferedSensors()
 {
 	_imageMutex.lock();
@@ -1466,10 +1483,10 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }
 
-void Rtabmap::process(const cv::Mat & image)
+void Rtabmap::process(const cv::Mat & image, int id)
 {
 	UScopeMutex s(&_threadMutex);
-	this->process(Image(image));
+	this->process(Image(image, id));
 }
 
 void Rtabmap::process(const Image & image)

tools/ConsoleApp/main.cpp

@@ -435,7 +435,7 @@ int main(int argc, char * argv[])
 		cv::Mat descriptors;
 		std::vector<cv::KeyPoint> keypoints;
 		cv::Mat cvImg = camera->takeImage(descriptors, keypoints);
-		Image img(cvImg, descriptors, keypoints);
+		Image img(cvImg, 0, descriptors, keypoints);
 		int i=0;
 		double maxIterationTime = 0.0;
 		int maxIterationTimeId = 0;
@@ -452,7 +452,7 @@ int main(int argc, char * argv[])
 				++countLoopDetected;
 			}
 			cvImg = camera->takeImage(descriptors, keypoints);
-			img = Image(cvImg, descriptors, keypoints);
+			img = Image(cvImg, 0, descriptors, keypoints);
 			if(++count % 100 == 0)
 			{
 				printf(" count = %d, loop closures = %d, max time (at %d) = %fs\n",