minor changes...

git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@57 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/WebcamCapture/main.cpp

@@ -77,7 +77,7 @@ protected:
 		if(e->getClassName().compare("SMStateEvent") == 0)
 		{
 			const rtabmap::SMStateEvent * event = (const rtabmap::SMStateEvent*)e;
-			const rtabmap::SMState * sm = event->getData();
+			const rtabmap::SMState * sm = event->getSMState();
 			const IplImage * image = 0;
 			if(sm)
 			{

corelib/include/rtabmap/core/SMState.h

@@ -26,6 +26,7 @@
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
 #include <vector>
+#include <utilite/ULogger.h>
 
 namespace rtabmap {
 
@@ -36,42 +37,104 @@ public:
 	// Constructor 1
 	// image and/or keypoints can be passed for debugging (rtabmap will not re-extract keypoints/descriptors from the image if not null, only for debug/visualization)
 	// take image ownership
-	SMState(const std::list<std::vector<float> > & sensorStates, const std::list<std::vector<float> > & actuatorStates, IplImage * image = 0, const std::list<cv::KeyPoint> & keypoints = std::list<cv::KeyPoint>()) :
-		_sensorStates(sensorStates),
-		_actuatorStates(actuatorStates),
+	SMState(const std::list<std::vector<float> > & sensors, const std::list<std::vector<float> > & actuators, IplImage * image = 0, const std::list<cv::KeyPoint> & keypoints = std::list<cv::KeyPoint>()) :
+		_sensors(sensors),
+		_actuators(actuators),
 		_image(image),
-		_keypoints(keypoints),
-		_descriptorsProvided(true)
+		_keypoints(keypoints)
 	{}
-	// Constructor 2 :
+	// Constructor 2 : for convenience with ROS conversion...
+	// Sensors and actuators vectors will be split into a list with smaller vectors of length sensorStep and actuatorStep respectively.
+	// image and/or keypoints can be passed for debugging (rtabmap will not re-extract keypoints/descriptors from the image if not null, only for debug/visualization)
+	// take image ownership
+	SMState(const std::vector<float> & sensors, int sensorStep, const std::vector<float> & actuators, int actuatorStep, IplImage * image = 0, const std::list<cv::KeyPoint> & keypoints = std::list<cv::KeyPoint>()) :
+		_image(image),
+		_keypoints(keypoints)
+	{
+		if(sensorStep && sensors.size() % sensorStep != 0)
+		{
+			UERROR("Sensors must all have the same length.");
+		}
+		if(actuatorStep && actuators.size() % actuatorStep != 0)
+		{
+			UERROR("Actuators must all have the same length.");
+		}
+		for(unsigned int i=0; i<sensors.size() && i<i+sensorStep; i+=sensorStep)
+		{
+			_sensors.push_back(std::vector<float>(sensors.data()+i, sensors.data()+i+actuatorStep));
+		}
+		for(unsigned int i=0; i<actuators.size() && i<i+actuatorStep; i+=actuatorStep)
+		{
+			_actuators.push_back(std::vector<float>(actuators.data()+i, actuators.data()+i+actuatorStep));
+		}
+	}
+	// Constructor 3 :
 	// rtabmap will automatically extract keypoints and descriptors from the image...
 	// take image ownership
-	SMState(IplImage * image, const std::list<std::vector<float> > & actuatorStates = std::list<std::vector<float> >()) :
-		_actuatorStates(actuatorStates),
-		_image(image),
-		_descriptorsProvided(false)
+	SMState(IplImage * image) :
+		_image(image)
 	{}
+	// Constructor 4 :
+	// rtabmap will automatically extract keypoints and descriptors from the image...
+	// take image ownership
+	SMState(IplImage * image, const std::list<std::vector<float> > & actuators) :
+		_actuators(actuators),
+		_image(image)
+	{}
+
 	virtual ~SMState()
 	{
 		if(_image)
+		{
 			cvReleaseImage(&_image);
+		}
 	}
 
-	bool isDescriptorsProvided() const {return _descriptorsProvided;}
 	const IplImage * getImage() const {return _image;}
 	const std::list<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
-	const std::list<std::vector<float> > & getSensorStates() const {return _sensorStates;}
-	const std::list<std::vector<float> > & getActuatorStates() const {return _actuatorStates;}
+	const std::list<std::vector<float> > & getSensors() const {return _sensors;}
+	const std::list<std::vector<float> > & getActuators() const {return _actuators;}
+	void setSensors(const std::list<std::vector<float> > & sensors) {_sensors=sensors;}
+	void setActuators(const std::list<std::vector<float> > & actuators) {_actuators=actuators;}
 
-	void setSensorStates(const std::list<std::vector<float> > & sensorStates) {_sensorStates=sensorStates;}
-	void setActuatorStates(const std::list<std::vector<float> > & actuatorStates) {_actuatorStates=actuatorStates;}
+	void getSensorsMerged(std::vector<float> & sensors, int & step) const
+	{
+		sensors.clear();
+		step = 0;
+		if(_sensors.size())
+		{
+			// here we assume that all sensors have the same length
+			step = _sensors.front().size();
+			for(std::list<std::vector<float> >::const_iterator iter = _sensors.begin();
+					iter != _sensors.end();
+					++iter)
+			{
+				sensors.insert(sensors.end(), iter->begin(), iter->end());
+			}
+		}
+	}
+	void getActuatorsMerged(std::vector<float> & actuators, int & step) const
+	{
+		actuators.clear();
+		step = 0;
+		if(_actuators.size())
+		{
+			// here we assume that all sensors have the same length
+			step = _actuators.front().size();
+			for(std::list<std::vector<float> >::const_iterator iter = _actuators.begin();
+					iter != _actuators.end();
+					++iter)
+			{
+				actuators.insert(actuators.end(), iter->begin(), iter->end());
+			}
+		}
+	}
 
 private:
-	std::list<std::vector<float> > _sensorStates; // descriptors
-	std::list<std::vector<float> > _actuatorStates;
+	std::list<std::vector<float> > _sensors; // descriptors
+	std::list<std::vector<float> > _actuators;
 	IplImage * _image;
 	std::list<cv::KeyPoint> _keypoints;
-	bool _descriptorsProvided;
 };
 
 // Sensorimotor state event
@@ -84,8 +147,8 @@ public:
 		_state(state) {}
 
 	virtual ~SMStateEvent() {if(_state) delete _state;}
-	const SMState * getData() const {return _state;}
-	SMState * getDataOwnership() {SMState * state = _state; _state=0; return state;}
+	const SMState * getSMState() const {return _state;}
+	SMState * getSMStateOwnership() {SMState * state = _state; _state=0; return state;}
 	virtual std::string getClassName() const {return "SMStateEvent";} // TODO : macro?
 
 private:

corelib/src/Camera.cpp

@@ -290,7 +290,10 @@ void Camera::process()
 	if(image)
 	{
 		SMState * smState = _postThreatement->process(image);
-		this->post(new SMStateEvent(smState));
+		if(smState)
+		{
+			this->post(new SMStateEvent(smState));
+		}
 		double elapsed = timer.ticks();
 		UDEBUG("Post treatment time = %fs", elapsed);
 		if(_imageRate>0)

corelib/src/KeypointMemory.cpp

@@ -829,7 +829,7 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
 			nbCommonWords = _vwd->getTotalActiveReferences() / treeSize;
 		}
 
-		if(!smState->isDescriptorsProvided())
+		if(smState->getSensors().empty())
 		{
 			image = smState->getImage();
 			if(image && _keypointDetector)
@@ -847,9 +847,9 @@ Signature * KeypointMemory::createSignature(int id, const SMState * smState, boo
 		}
 		else
 		{
-			if(smState->getSensorStates().size() >= _badSignRatio * nbCommonWords)
+			if(smState->getSensors().size() >= _badSignRatio * nbCommonWords)
 			{
-				descriptors = smState->getSensorStates();
+				descriptors = smState->getSensors();
 				keypoints = smState->getKeypoints();
 			}
 			image = smState->getImage();

corelib/src/Memory.cpp

@@ -270,7 +270,7 @@ bool Memory::update(const SMState * smState, std::list<std::pair<std::string, fl
 
 
 	// It will be added to the short-time memory, no need to delete it...
-	this->addSignatureToStm(signature, smState->getActuatorStates());
+	this->addSignatureToStm(signature, smState->getActuators());
 	_lastSignature = signature;
 
 	if(_lastLoopClosureId == 0)

corelib/src/Rtabmap.cpp

@@ -521,7 +521,7 @@ void Rtabmap::handleEvent(UEvent* event)
 		if(event->getClassName().compare("SMStateEvent") == 0)
 		{
 			SMStateEvent * e = (SMStateEvent*)event;
-			SMState * data = e->getDataOwnership();
+			SMState * data = e->getSMStateOwnership();
 			this->addSMState(data);
 		}
 	}