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;
+				ParametersMap customParameters = _lastParameters;
-				customParameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(_memory->getBowInlierDistance())));
+				// override some parameters
-				customParameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(_memory->getBowIterations())));
+				uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-				customParameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(_memory->getBowMinInliers())));
+				uInsert(customParameters, ParametersPair(Parameters::kMemImageKept(), "false"));
-				customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_memory->getBowMaxDepth())));
+				uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
-				customParameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uNumber2Str(_memory->getBowForce2D())));
+				uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
-				customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+				uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
-				customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+				uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
-				customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+				uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
-				customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
+
-				customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
+				for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
-				customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
+				{
-				customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+					UINFO("%s=%s", iter->first.c_str(), iter->second.c_str());
+				}
 
 				Memory memory(customParameters);
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -137,7 +137,7 @@ private slots:
 	void selectScreenCaptureFormat(bool checked);
 	void takeScreenshot();
 	void updateElapsedTime();
-	void processOdometry(const rtabmap::SensorData & data, int quality, float time);
+	void processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize);
 	void applyAllPrefSettings();
 	void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
 	void applyPrefSettings(const rtabmap::ParametersMap & parameters);
@@ -169,7 +169,7 @@ private slots:
 
 signals:
 	void statsReceived(const rtabmap::Statistics &);
-	void odometryReceived(const rtabmap::SensorData &, int, float);
+	void odometryReceived(const rtabmap::SensorData &, int, float, int, int);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);

guilib/src/DatabaseViewer.cpp

@@ -907,7 +907,10 @@ void DatabaseViewer::detectMoreLoopClosures()
 	std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
 	optimizedPoses = optimizeGraph(ids, poses_, links);
 
-	std::multimap<int, int> clusters = util3d::radiusPosesClustering(optimizedPoses, 0.1, 0.1);
+	std::multimap<int, int> clusters = util3d::radiusPosesClustering(
+			optimizedPoses,
+			ui_->doubleSpinBox_detectMore_radius->value(),
+			ui_->doubleSpinBox_detectMore_angle->value());
 	int added = 0;
 	for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
 	{

guilib/src/MainWindow.cpp

@@ -342,7 +342,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
 
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
-	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float)), this, SLOT(processOdometry(rtabmap::SensorData, int, float)));
+	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float, int, int)), this, SLOT(processOdometry(rtabmap::SensorData, int, float, int, int)));
 
 	connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
 
@@ -531,7 +531,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		   !_processingStatistics)
 		{
 			_lastOdometryProcessed = false; // if we receive too many odometry events!
-			emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time());
+			emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time(), odomEvent->features(), odomEvent->localMapSize());
 		}
 	}
 	else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -554,7 +554,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 }
 
-void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time)
+void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize)
 {
 	Transform pose = data.pose();
 	if(pose.isNull())
@@ -584,6 +584,14 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 	{
 		_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f);
 	}
+	if(features >=0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)features);
+	}
+	if(localMapSize >=0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
+	}
 	if(!pose.isNull())
 	{
 		_lastOdomPose = pose;

guilib/src/ui/DatabaseViewer.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1076</width>
-    <height>582</height>
+    <height>665</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -912,6 +912,58 @@
          </property>
         </widget>
        </item>
+       <item row="8" column="0">
+        <widget class="QLabel" name="label_29">
+         <property name="text">
+          <string>Detect more LCs: radius</string>
+         </property>
+        </widget>
+       </item>
+       <item row="9" column="0">
+        <widget class="QLabel" name="label_30">
+         <property name="text">
+          <string>Detect more LCs: angle</string>
+         </property>
+        </widget>
+       </item>
+       <item row="8" column="1">
+        <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radius">
+         <property name="suffix">
+          <string> m</string>
+         </property>
+         <property name="decimals">
+          <number>2</number>
+         </property>
+         <property name="maximum">
+          <double>10.000000000000000</double>
+         </property>
+         <property name="singleStep">
+          <double>0.100000000000000</double>
+         </property>
+         <property name="value">
+          <double>0.300000000000000</double>
+         </property>
+        </widget>
+       </item>
+       <item row="9" column="1">
+        <widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
+         <property name="suffix">
+          <string> rad</string>
+         </property>
+         <property name="decimals">
+          <number>2</number>
+         </property>
+         <property name="maximum">
+          <double>3.140000000000000</double>
+         </property>
+         <property name="singleStep">
+          <double>0.100000000000000</double>
+         </property>
+         <property name="value">
+          <double>0.300000000000000</double>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>