Updated CameraDatabase : loading also the associated actions with image loaded

Memory : Saving neighbors of looped locations (for a trace in DB)

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

corelib/ConsoleApp/main.cpp

@@ -406,11 +406,11 @@ int main(int argc, char * argv[])
 	std::list<std::vector<float> > actions;
 	while(loopDataset <= repeat && g_forever)
 	{
-		image = camera->takeImage();
+		SMState * smState = camera->takeImage();
 		int i=0;
-		while(image && g_forever)
+		while(smState && g_forever)
 		{
-			SMState * smState = imageToSMState.process(image);
+			imageToSMState.process(smState);
 			++imagesProcessed;
 			iterationTimer.start();
 			if(i<maxTeleopActions)
@@ -435,7 +435,7 @@ int main(int argc, char * argv[])
 			{
 				++countLoopDetected;
 			}
-			image = camera->takeImage();
+			smState = camera->takeImage();
 			if(++count % 100 == 0)
 			{
 				printf(" count = %d, loop closures = %d\n", count, countLoopDetected);

corelib/include/rtabmap/core/Camera.h

@@ -38,7 +38,7 @@ class KeypointDescriptor;
 class SMState;
 
 /**
- * Only encapsulate the image in a newly created SMState.
+ * No treatment
  */
 class RTABMAP_EXP CamPostTreatment
 {
@@ -47,7 +47,7 @@ public:
 		this->parseParameters(parameters);
 	}
 	virtual ~CamPostTreatment() {}
-	virtual SMState * process(const IplImage * image) const;
+	virtual void process(SMState * smState) const;
 	virtual void parseParameters(const ParametersMap & parameters) {}
 };
 
@@ -68,7 +68,7 @@ public:
 		this->parseParameters(parameters);
 	}
 	virtual ~CamKeypointTreatment();
-	virtual SMState * process(const IplImage * image) const;
+	virtual void process(SMState * smState) const;
 	virtual void parseParameters(const ParametersMap & parameters);
 	DetectorStrategy detectorStrategy() const;
 private:
@@ -90,7 +90,7 @@ public:
 public:
 	virtual ~Camera();
 
-	virtual IplImage * takeImage() = 0;
+	virtual SMState * takeImage() = 0;
 	virtual bool init() = 0;
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
@@ -145,7 +145,7 @@ public:
 			unsigned int imageHeight = 0);
 	virtual ~CameraImages();
 
-	virtual IplImage * takeImage();
+	virtual SMState * takeImage();
 	virtual bool init();
 
 private:
@@ -187,7 +187,7 @@ public:
 			unsigned int imageHeight = 0);
 	virtual ~CameraVideo();
 
-	virtual IplImage * takeImage();
+	virtual SMState * takeImage();
 	virtual bool init();
 
 private:
@@ -222,7 +222,7 @@ public:
 			unsigned int imageHeight = 0);
 	virtual ~CameraDatabase();
 
-	virtual IplImage * takeImage();
+	virtual SMState * takeImage();
 	virtual bool init();
 
 private:

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -52,7 +52,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Vp_likelihood,);
 	RTABMAP_STATS(Loop, ReactivateId,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
-	RTABMAP_STATS(Loop, Retrieval_margin,)
+	RTABMAP_STATS(Loop, Retrieval_margin,);
+	RTABMAP_STATS(Loop, Actions,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);

corelib/src/Camera.cpp

@@ -36,10 +36,9 @@
 namespace rtabmap
 {
 
-SMState * CamPostTreatment::process(const IplImage * image) const
+void CamPostTreatment::process(SMState * smState) const
 {
 	//no threatment...
-	return new SMState();
 }
 
 CamKeypointTreatment::~CamKeypointTreatment()
@@ -53,17 +52,15 @@ CamKeypointTreatment::~CamKeypointTreatment()
 		delete _keypointDescriptor;
 	}
 }
-SMState * CamKeypointTreatment::process(const IplImage * image) const
+void CamKeypointTreatment::process(SMState * smState) const
 {
-	if(image)
+	if(smState && smState->getImage() && smState->getKeypoints().size() == 0 && smState->getSensors().size() == 0)
 	{
-		std::list<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(image);
-		std::list<std::vector<float> > descriptors = _keypointDescriptor->generateDescriptors(image, keypoints);
-		SMState * smState = new SMState(descriptors, std::list<std::vector<float> >());
+		std::list<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(smState->getImage());
+		std::list<std::vector<float> > descriptors = _keypointDescriptor->generateDescriptors(smState->getImage(), keypoints);
+		smState->setSensors(descriptors);
 		smState->setKeypoints(keypoints);
-		return smState;
 	}
-	return 0;
 }
 
 void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
@@ -284,15 +281,12 @@ void Camera::process()
 {
 	UTimer timer;
 	ULOGGER_DEBUG("Camera::process()");
-	IplImage * image = this->takeImage();
-	if(image)
+	SMState * smState = this->takeImage();
+	if(smState)
 	{
-		SMState * smState = _postThreatement->process(image);
-		if(smState)
-		{
-			smState->setImage(image);
-			this->post(new SMStateEvent(smState));
-		}
+		_postThreatement->process(smState);
+		this->post(new SMStateEvent(smState));
+
 		double elapsed = timer.ticks();
 		UDEBUG("Post treatment time = %fs", elapsed);
 		if(_imageRate>0)
@@ -370,7 +364,7 @@ bool CameraImages::init()
 	return _dir != 0;
 }
 
-IplImage * CameraImages::takeImage()
+SMState * CameraImages::takeImage()
 {
 	IplImage * img = 0;
 	if(_dir)
@@ -428,7 +422,11 @@ IplImage * CameraImages::takeImage()
 		cvReleaseImage(&img);
 		img = resampledImg;
 	}
-	return img;
+	if(img)
+	{
+		return new SMState(img);
+	}
+	return 0;
 }
 
 
@@ -505,7 +503,7 @@ bool CameraVideo::init()
 	return true;
 }
 
-IplImage * CameraVideo::takeImage()
+SMState * CameraVideo::takeImage()
 {
 	IplImage * img = 0;  // Null image
 	if(_capture)
@@ -543,7 +541,11 @@ IplImage * CameraVideo::takeImage()
 		img = cvCloneImage(img);
 	}
 
-	return img;
+	if(img)
+	{
+		return new SMState(img);
+	}
+	return 0;
 }
 
 
@@ -606,20 +608,43 @@ bool CameraDatabase::init()
 	}
 	else
 	{
-		// TODO load all signatures only if ignoreChildren is false
 		_dbDriver->getAllSignatureIds(_ids);
 		_indexIter = _ids.begin();
 	}
 	return true;
 }
 
-IplImage * CameraDatabase::takeImage()
+SMState * CameraDatabase::takeImage()
 {
 	IplImage * img = 0;
 	if(_dbDriver && _indexIter != _ids.end())
 	{
-		_dbDriver->getImage(*_indexIter, &img);
-		++_indexIter;
+		if(_ignoreChildren)
+		{
+			bool ignore = true;
+			while(img == 0 && _indexIter != _ids.end() && ignore)
+			{
+				ignore = false;
+				int loopId = 0;
+				if(_dbDriver->getLoopClosureId(*_indexIter, loopId))
+				{
+					if(loopId == *_indexIter+1)
+					{
+						ignore = true;
+					}
+					else
+					{
+						_dbDriver->getImage(*_indexIter, &img);
+					}
+				}
+				++_indexIter;
+			}
+		}
+		else
+		{
+			_dbDriver->getImage(*_indexIter, &img);
+			++_indexIter;
+		}
 	}
 	else if(!_dbDriver)
 	{
@@ -647,7 +672,27 @@ IplImage * CameraDatabase::takeImage()
 		img = resampledImg;
 	}
 
-	return img;
+	if(img)
+	{
+		SMState * smState = new SMState(img);
+		if(_dbDriver && _indexIter!=_ids.begin())
+		{
+			std::set<int>::iterator iter = _indexIter;
+			--iter;
+			std::map<int, std::list<std::vector<float> > > neighbors;
+			_dbDriver->loadNeighbors(*iter, neighbors);
+			for(std::map<int, std::list<std::vector<float> > >::iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+			{
+				if(i->first > *iter && i->second.size())
+				{
+					smState->setActuators(i->second);
+					break;
+				}
+			}
+		}
+		return smState;
+	}
+	return 0;
 }
 
 } // namespace rtabmap

corelib/src/Memory.cpp

@@ -955,7 +955,7 @@ void Memory::moveToTrash(Signature * s)
 					UDEBUG("A neighbor is not found (%d)", *iter);
 					_dbDriver->removeNeighbor(*iter, s->id());
 				}
-				s->removeNeighbor(*iter);
+				//s->removeNeighbor(*iter); // Commented to keep a trace in db
 			}
 
 			if(s->getWeight() && s->getLoopClosureId() > 0)

corelib/src/Rtabmap.cpp

@@ -1075,6 +1075,7 @@ void Rtabmap::process()
 			stat->addStatistic(Statistics::kLoopReactivateId(), _reactivateId);
 			stat->addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			stat->addStatistic(Statistics::kLoopRetrieval_margin(), _spreadMargin);
+			stat->addStatistic(Statistics::kLoopActions(), (int)_actions.size());
 
 			// Child count by parent signature on the root of the memory ... for statistics
 			stat->setWeights(_memory->getWeights());

corelib/tests/Tests.cpp

@@ -81,20 +81,21 @@ void Tests::testAvpd()
 
 
 	/* Start thread's task */
-	IplImage * image = 0;
+	SMState * smState = 0;
 
-	image = camera.takeImage();
+	smState = camera.takeImage();
 	int imgCount = 0;
-	while(image)
+	while(smState)
 	{
 		++imgCount;
 		printf("Processing image %d/84...\n", imgCount);
-		ctabmap.process(new SMState(image));
-		image = camera.takeImage();
+		ctabmap.process(smState);
+		smState = camera.takeImage();
 	}
-	if(image)
+	if(smState)
 	{
-		cvReleaseImage(&image);
+		delete smState;
+		smState = 0;
 	}
 
 	CPPUNIT_ASSERT(imgCount == 84);
@@ -141,7 +142,7 @@ void Tests::testCamera()
 {
 	//Logger::setType(Logger::kTypeFile, "LogTestAvpdCore/testCamera.txt", false);
 	std::string path;
-	IplImage * image = 0;
+	SMState * smState = 0;
 	int count;
 
 	//CameraVideo class FIXME add a video in svn and reactivate this test
@@ -171,13 +172,14 @@ void Tests::testCamera()
 	CameraImages cameraImages(path, false, 0, false, 80);
 	CPPUNIT_ASSERT( cameraImages.init() );
 	CPPUNIT_ASSERT( cameraImages.isIdle() == true);
-	image = cameraImages.takeImage();
+	smState = cameraImages.takeImage();
 	count = 0;
-	while(image)
+	while(smState)
 	{
-		cvReleaseImage(&image);
+		delete smState;
+		smState = 0;
 		++count;
-		image = cameraImages.takeImage();
+		smState = cameraImages.takeImage();
 	}
 	CPPUNIT_ASSERT( count == 5 );
 
@@ -186,13 +188,14 @@ void Tests::testCamera()
 	CameraDatabase cameraDatabase(path, false); // ignoreChildren=false;
 	CPPUNIT_ASSERT( cameraDatabase.init() );
 	CPPUNIT_ASSERT( cameraDatabase.isIdle() == true);
-	image = cameraDatabase.takeImage();
+	smState = cameraDatabase.takeImage();
 	count = 0;
-	while(image)
+	while(smState)
 	{
 		++count;
-		cvReleaseImage(&image);
-		image = cameraDatabase.takeImage();
+		delete smState;
+		smState = 0;
+		smState = cameraDatabase.takeImage();
 	}
 	//ULOGGER_INFO("%d", count);
 	CPPUNIT_ASSERT( count == 82 );