Enabled binary visual words for dictionary (for loop closure detection).

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

corelib/include/rtabmap/core/OdometryEvent.h

@@ -37,19 +37,22 @@ class OdometryEvent : public UEvent
 {
 public:
 	OdometryEvent(
-		const SensorData & data, int quality = -1) :
+		const SensorData & data, int quality = -1, float time = 0.0f) :
 			_data(data),
-			_quality(quality) {}
+			_quality(quality),
+			_time(time){}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
 	bool isValid() const {return !_data.pose().isNull();}
 	const SensorData & data() const {return _data;}
 	int quality() const {return _quality;}
+	float time() const {return _time;} // seconds
 
 private:
 	SensorData _data;
 	int _quality;
+	float _time; // seconds
 };
 
 class OdometryResetEvent : public UEvent

corelib/include/rtabmap/core/Parameters.h

@@ -167,7 +167,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.2, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8, 		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0, 			"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/BRIEF 6=GFTT/BRIEF.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0, 			"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF.");
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, false, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
 	RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", 		"Region of interest ratios [left, right, top, bottom].");
@@ -251,7 +251,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
 
 	// Odometry
-	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
+	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF.");
 	RTABMAP_PARAM(Odom, LinearUpdate,           float, 0.0, 	"Min linear displacement to update odometry.");
 	RTABMAP_PARAM(Odom, AngularUpdate,          float, 0.0, 	"Min angular displacement to update odometry.");
 	RTABMAP_PARAM(Odom, MaxWords,               int, 0, 		"0 no limits.");

corelib/src/DBDriverSqlite3.cpp

@@ -1271,11 +1271,22 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 				descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
 				dRealSize = sqlite3_column_bytes(ppStmt, index++);
 
-				if(dRealSize/int(sizeof(float)) != descriptorSize)
+				cv::Mat d;
+				if(dRealSize == descriptorSize)
 				{
-					UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
+					// CV_8U binary descriptors
+					d = cv::Mat(1, descriptorSize, CV_8U);
 				}
-				cv::Mat d(1, descriptorSize, CV_32F);
+				else if(dRealSize/int(sizeof(float)) == descriptorSize)
+				{
+					// CV_32F
+					d = cv::Mat(1, descriptorSize, CV_32F);
+				}
+				else
+				{
+					UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
+				}
+
 				memcpy(d.data, descriptor, dRealSize);
 				VisualWord * vw = new VisualWord(id, d);
 				vw->setSaved(true);
@@ -1344,12 +1355,22 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 				descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
 				dRealSize = sqlite3_column_bytes(ppStmt, index++);
 
-				if(dRealSize/int(sizeof(float)) != descriptorSize)
+				cv::Mat d;
+				if(dRealSize == descriptorSize)
 				{
-					UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
+					// CV_8U binary descriptors
+					d = cv::Mat(1, descriptorSize, CV_8U);
+				}
+				else if(dRealSize/int(sizeof(float)) == descriptorSize)
+				{
+					// CV_32F
+					d = cv::Mat(1, descriptorSize, CV_32F);
+				}
+				else
+				{
+					UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
 				}
 
-				cv::Mat d(1, descriptorSize, CV_32F);
 				memcpy(d.data, descriptor, dRealSize);
 				VisualWord * vw = new VisualWord(*iter, d);
 				if(vw)
@@ -1848,7 +1869,17 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 					rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-					rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
+					UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
+					if(w->getDescriptor().type() == CV_32F)
+					{
+						// CV_32F
+						rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
+					}
+					else
+					{
+						// CV_8U
+						rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
+					}
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 					//execute query

corelib/src/Odometry.cpp

@@ -605,9 +605,10 @@ void OdometryThread::mainLoop()
 	if(data.isValid())
 	{
 		int quality = -1;
+		UTimer time;
 		Transform pose = _odometry->process(data, &quality);
 		data.setPose(pose); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(data, quality));
+		this->post(new OdometryEvent(data, quality, time.elapsed()));
 	}
 }
 

corelib/src/VWDictionary.cpp

@@ -286,11 +286,11 @@ void VWDictionary::update()
 				_flannIndex->build(_dataTree, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
 				break;
 			case kNNFlannKdTree:
-				UASSERT(type == CV_32F);
+				UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
 				_flannIndex->build(_dataTree, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_L2);
 				break;
 			case kNNFlannLSH:
-				UASSERT(type == CV_8U);
+				UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
 				_flannIndex->build(_dataTree, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
 				break;
 			default:

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);
+	void processOdometry(const rtabmap::SensorData & data, int quality, float time);
 	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);
+	void odometryReceived(const rtabmap::SensorData &, int, float);
 	void thresholdsChanged(int, int);
 	void stateChanged(MainWindow::State);
 	void rtabmapEventInitReceived(int status, const QString & info);

guilib/src/DatabaseViewer.cpp

@@ -186,6 +186,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
 		rtabmap::ParametersMap parameters;
 		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
 		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
+		// use BruteForce dictionary because we don't know which type of descriptors are saved in database
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), "3"));
 
 		memory_ = new rtabmap::Memory();
 

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)), this, SLOT(processOdometry(rtabmap::SensorData, int)));
+	connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float)), this, SLOT(processOdometry(rtabmap::SensorData, int, float)));
 
 	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());
+			emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time());
 		}
 	}
 	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)
+void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time)
 {
 	Transform pose = data.pose();
 	if(pose.isNull())
@@ -578,7 +578,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality)
 	}
 	if(quality >= 0)
 	{
-		_ui->statsToolBox->updateStat("/Odom inliers/", (float)data.id(), (float)quality);
+		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)quality);
+	}
+	if(time > 0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f);
 	}
 	if(!pose.isNull())
 	{
@@ -1406,10 +1410,6 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
 {
 	if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
 	{
-		if(_state == kDetecting)
-		{
-			this->pauseDetection();
-		}
 		_initProgressDialog->setAutoClose(true, 1);
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();

guilib/src/PreferencesDialog.cpp

@@ -1345,15 +1345,25 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath)
 
 bool PreferencesDialog::validateForm()
 {
-	//verify odom type vs nearest neighbor approach
+	//verify binary featrues and nearest neighbor
-	if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH)
+
+	// BOW dictionary type
+	if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH && _ui->comboBox_detector_strategy->currentIndex() <= 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("With the selected feature type (SURF or SIFT), parameter \"Visual Word->Nearest Neighbor\" "
+				tr("With the selected feature type (SURF or SIFT), parameter \"Visual word->Nearest Neighbor\" "
 				   "cannot be LSH (used for binary descriptor). KD-tree is set instead."));
 		_ui->comboBox_dictionary_strategy->setCurrentIndex(VWDictionary::kNNFlannKdTree);
 	}
+	else if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannKdTree && _ui->comboBox_detector_strategy->currentIndex() >1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Visual word->Nearest Neighbor\" "
+				   "cannot be KD-Tree (used for float descriptor). BruteForce matching is set instead."));
+		_ui->comboBox_dictionary_strategy->setCurrentIndex(VWDictionary::kNNBruteForce);
+	}
 
+	// odom type
 	if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannLSH && _ui->odom_type->currentIndex() <= 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
@@ -1365,8 +1375,8 @@ bool PreferencesDialog::validateForm()
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Odometry->Nearest Neighbor\" "
-				   "cannot be KD-Tree (used for float descriptor). LSH is set instead."));
+				   "cannot be KD-Tree (used for float descriptor). BruteForce matching is set instead."));
-		_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannLSH);
+		_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
 	}
 
 	return true;

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>9</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3343,6 +3343,31 @@ generate the number of words requested.</string>
                           <string>SIFT</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>ORB</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>FAST+FREAK</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>FAST+BRIEF</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>GFTT+FREAK</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>GFTT+BRIEF</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
                      </layout>