added new statistics (features extracted current image, local map size) shown in GUI for Odometry

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

corelib/include/rtabmap/core/DBReader.h

@@ -53,7 +53,14 @@ public:
 
 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, Transform & pose);
+	void getNextImage(cv::Mat & image,
+			cv::Mat & depth,
+			cv::Mat & depth2d,
+			float & fx, float & fy,
+			float & cx, float & cy,
+			Transform & localTransform,
+			Transform & pose,
+			int & seq);
 
 protected:
 	virtual void mainLoopBegin();

corelib/include/rtabmap/core/Odometry.h

@@ -54,7 +54,7 @@ class RTABMAP_EXP Odometry
 {
 public:
 	virtual ~Odometry() {}
-	Transform process(SensorData & data, int * quality = 0);
+	Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
 	virtual void reset();
 
 	bool isLargeEnoughTransform(const Transform & transform);
@@ -72,7 +72,7 @@ public:
 	int getLocalHistory() const {return _localHistory;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0) = 0;
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;
 
 private:
 	int _maxFeatures;
@@ -106,7 +106,7 @@ public:
 	const Memory * getMemory() const {return _memory;}
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0);
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
 
 private:
 	Memory * _memory;
@@ -127,7 +127,7 @@ public:
 	void reset();
 
 private:
-	virtual Transform computeTransform(const SensorData & image, int * quality = 0);
+	virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
 
 private:
 	int _decimation;

corelib/include/rtabmap/core/OdometryEvent.h

@@ -37,10 +37,13 @@ class OdometryEvent : public UEvent
 {
 public:
 	OdometryEvent(
-		const SensorData & data, int quality = -1, float time = 0.0f) :
+		const SensorData & data, int quality = -1, float time = 0.0f, int features = 0, int localMapSize = 0) :
 			_data(data),
 			_quality(quality),
-			_time(time){}
+			_time(time),
+			_features(features),
+			_localMapSize(localMapSize)
+	{}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
@@ -48,11 +51,15 @@ public:
 	const SensorData & data() const {return _data;}
 	int quality() const {return _quality;}
 	float time() const {return _time;} // seconds
+	int features() const {return _features;}
+	int localMapSize() const {return _localMapSize;}
 
 private:
 	SensorData _data;
 	int _quality;
 	float _time; // seconds
+	int _features;
+	int _localMapSize;
 };
 
 class OdometryResetEvent : public UEvent

corelib/include/rtabmap/core/Rtabmap.h

@@ -183,6 +183,7 @@ private:
 	// strategies for a type of signature or configuration.
 	EpipolarGeometry * _epipolarGeometry;
 	BayesFilter * _bayesFilter;
+	ParametersMap _lastParameters;
 
 	Memory * _memory;
 

corelib/src/DBReader.cpp

@@ -134,7 +134,8 @@ void DBReader::mainLoop()
 	cv::Mat image, depth, depth2d;
 	float fx,fy,cx,cy;
 	Transform localTransform, pose;
-	this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose);
+	int seq = 0;
+	this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
 	if(!image.empty())
 	{
 		if(depth.empty())
@@ -145,7 +146,7 @@ void DBReader::mainLoop()
 		{
 			if(!_odometryIgnored)
 			{
-				SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
+				SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform, seq);
 				this->post(new OdometryEvent(data));
 				if(pose.isNull())
 				{
@@ -157,7 +158,7 @@ void DBReader::mainLoop()
 			else
 			{
 				// without odometry
-				this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform));
+				this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform, seq));
 			}
 		}
 	}
@@ -179,7 +180,8 @@ void DBReader::getNextImage(
 		float & cx,
 		float & cy,
 		Transform & localTransform,
-		Transform & pose)
+		Transform & pose,
+		int & seq)
 {
 	if(_dbDriver)
 	{
@@ -211,6 +213,7 @@ void DBReader::getNextImage(
 			int mapId;
 			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
 			_dbDriver->getPose(*_currentId, pose, mapId);
+			seq = *_currentId;
 			++_currentId;
 			if(imageBytes.empty())
 			{

corelib/src/Odometry.cpp

@@ -99,9 +99,9 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
 			fabs(yaw) > _angularUpdate;
 }
 
-Transform Odometry::process(SensorData & data, int * quality)
+Transform Odometry::process(SensorData & data, int * quality, int * features, int * localMapSize)
 {
-	Transform t = this->computeTransform(data, quality);
+	Transform t = this->computeTransform(data, quality, features, localMapSize);
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -210,7 +210,7 @@ std::multimap<int,pcl::PointXYZ> OdometryBOW::getLocalMeansMap() const
 }
 
 // return not null transform if odometry is correctly computed
-Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
+Transform OdometryBOW::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
 {
 	UTimer timer;
 	Transform output;
@@ -388,6 +388,15 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 		_memory->emptyTrash();
 	}
 
+	if(features)
+	{
+		*features = nFeatures;
+	}
+	if(localMapSize)
+	{
+		*localMapSize = (int)localMap_.size();
+	}
+
 	UINFO("Odom update time = %fs features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
 			timer.elapsed(),
 			nFeatures,
@@ -430,7 +439,7 @@ void OdometryICP::reset()
 }
 
 // return not null transform if odometry is correctly computed
-Transform OdometryICP::computeTransform(const SensorData & data, int * quality)
+Transform OdometryICP::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
 {
 	UTimer timer;
 	Transform output;
@@ -605,10 +614,12 @@ void OdometryThread::mainLoop()
 	if(data.isValid())
 	{
 		int quality = -1;
+		int features = -1;
+		int localMapSize = -1;
 		UTimer time;
-		Transform pose = _odometry->process(data, &quality);
+		Transform pose = _odometry->process(data, &quality, &features, &localMapSize);
 		data.setPose(pose); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(data, quality, time.elapsed()));
+		this->post(new OdometryEvent(data, quality, time.elapsed(), features, localMapSize));
 	}
 }
 

corelib/src/Rtabmap.cpp

@@ -422,6 +422,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	{
 		_bayesFilter->parseParameters(parameters);
 	}
+
+	for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		uInsert(_lastParameters, ParametersPair(iter->first, iter->second));
+	}
 }
 
 std::string Rtabmap::getDatabasePath() const
@@ -1274,19 +1279,20 @@ bool Rtabmap::process(const SensorData & data)
 			std::string rejectedMsg;
 			if(_reextractLoopClosureFeatures)
 			{
-				ParametersMap customParameters;
-				customParameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(_memory->getBowInlierDistance())));
-				customParameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(_memory->getBowIterations())));
-				customParameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(_memory->getBowMinInliers())));
-				customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_memory->getBowMaxDepth())));
-				customParameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uNumber2Str(_memory->getBowForce2D())));
-				customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-				customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
-				customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
-				customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
-				customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
-				customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
-				customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+				ParametersMap customParameters = _lastParameters;
+				// override some parameters
+				uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+				uInsert(customParameters, ParametersPair(Parameters::kMemImageKept(), "false"));
+				uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
+				uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
+				uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
+				uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
+				uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+
+				for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
+				{
+					UINFO("%s=%s", iter->first.c_str(), iter->second.c_str());
+				}
 
 				Memory memory(customParameters);