removed ImagesDbExtractor tool (already inside databaseViewer)

add optional delay to DBReader
added more options to odometryViewer tool


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

corelib/include/rtabmap/core/DBReader.h

@@ -27,7 +27,8 @@ class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
 public:
 	DBReader(const std::string & databasePath,
 			 float frameRate = 0.0f,
-			 bool odometryIgnored = false);
+			 bool odometryIgnored = false,
+			 float delayToStartSec = 0.0f);
 	virtual ~DBReader();
 
 	bool init(int startIndex=0);
@@ -42,6 +43,7 @@ private:
 	std::string _path;
 	float _frameRate;
 	bool _odometryIgnored;
+	float _delayToStartSec;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/src/DBReader.cpp

@@ -20,10 +20,12 @@ namespace rtabmap {
 
 DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
-				   bool odometryIgnored) :
+				   bool odometryIgnored,
+				   float delayToStartSec) :
 	_path(databasePath),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
+	_delayToStartSec(delayToStartSec),
 	_dbDriver(0),
 	_currentId(_ids.end())
 {
@@ -100,6 +102,10 @@ void DBReader::setFrameRate(float frameRate)
 
 void DBReader::mainLoopBegin()
 {
+	if(_delayToStartSec > 0.0f)
+	{
+		uSleep(_delayToStartSec*1000.0f);
+	}
 	_timer.start();
 }
 

corelib/src/Odometry.cpp

@@ -187,7 +187,7 @@ Transform OdometryBinary::computeTransform(Image & image)
 
 	if(_lastKeypoints.size())
 	{
-		if(newDescriptors.rows)
+		if(newDescriptors.rows && newDescriptors.rows > _lastKeypoints.size()/2) // at least 50% keypoints
 		{
 			cv::Mat results;
 			cv::Mat dists;
@@ -333,6 +333,11 @@ Transform OdometryBinary::computeTransform(Image & image)
 				UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
 			}
 		}
+		else if(newDescriptors.rows)
+		{
+			UWARN("At least 50%% keypoints of the last image required. New=%d last=%d",
+					newDescriptors.rows, _lastKeypoints.size());
+		}
 		else
 		{
 			UWARN("No feature extracted!");
@@ -437,7 +442,12 @@ Transform OdometryBOW::computeTransform(Image & image)
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
-			if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
+			if(newSignature->getWords3().size() < previousSignature->getWords3().size()/2)
+			{
+				UWARN("At least 50%% keypoints of the last image required. New=%d last=%d",
+						newSignature->getWords3().size(), previousSignature->getWords3().size());
+			}
+			else if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new

guilib/include/rtabmap/gui/CloudViewer.h

@@ -106,6 +106,9 @@ public:
 
 	const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
 
+	void setCameraTargetLocked();
+	void setCameraTargetFollow();
+
 public slots:
 	void render();
 	void setBackgroundColor(const QColor & color);

guilib/src/CloudViewer.cpp

@@ -536,6 +536,16 @@ void CloudViewer::setCloudPointSize(const std::string & id, int size)
 	}
 }
 
+void CloudViewer::setCameraTargetLocked()
+{
+	_aLockCamera->setChecked(true);
+}
+
+void CloudViewer::setCameraTargetFollow()
+{
+	_aFollowCamera->setChecked(true);
+}
+
 Eigen::Vector3f rotatePointAroundAxe(
 		const Eigen::Vector3f & point,
 		const Eigen::Vector3f & axis,

guilib/src/DatabaseViewer.cpp

@@ -132,14 +132,19 @@ bool DatabaseViewer::openDatabase(const QString & path)
 	UDEBUG("Open database \"%s\"", path.toStdString().c_str());
 	if(QFile::exists(path))
 	{
-		QStringList types;
-		types << "Keypoint" << "Sensorimotor";
-
 		if(memory_)
 		{
 			delete memory_;
 			memory_ = 0;
 			ids_.clear();
+			idToIndex_.clear();
+			neighborLinks_.clear();
+			loopLinks_.clear();
+			graphes_.clear();
+			poses_.clear();
+			links_.clear();
+			scans_.clear();
+			ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
 		}
 
 		std::string driverType = "sqlite3";

guilib/src/PreferencesDialog.cpp

@@ -2658,7 +2658,7 @@ void PreferencesDialog::testOdometry(OdomTest test)
 		window->setMinimumHeight(600);
 		connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanOdometryTest()) );
 
-		OdometryViewer * odomViewer = new OdometryViewer(100, 2, 0.0, window);
+		OdometryViewer * odomViewer = new OdometryViewer(10, 2, 0.0, window);
 
 		QVBoxLayout *layout = new QVBoxLayout();
 		layout->addWidget(odomViewer);

tools/CMakeLists.txt

@@ -1,7 +1,6 @@
 
 ADD_SUBDIRECTORY( ConsoleApp )
 ADD_SUBDIRECTORY( ImagesJoiner )
-ADD_SUBDIRECTORY( ImagesDbExtractor )
 ADD_SUBDIRECTORY( VocabularyComparison )
 ADD_SUBDIRECTORY( OdometryViewer )
 ADD_SUBDIRECTORY( DataRecorder )

tools/DataRecorder/main.cpp

@@ -1,5 +1,6 @@
 
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UFile.h>
 #include <rtabmap/core/CameraOpenni.h>
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/CameraThread.h>
@@ -97,6 +98,12 @@ int main (int argc, char * argv[])
 	}
 	fileName = argv[argc-1]; // the last is the output path
 
+	if(UFile::getExtension(fileName.toStdString()).compare("db") != 0)
+	{
+		printf("Database names must end with .db extension\n");
+		showUsage();
+	}
+
 	UINFO("Output = %s", fileName.toStdString().c_str());
 	UINFO("Show = %s", show?"true":"false");
 	UINFO("Openni = %s", openni?"true":"false");

tools/ImagesDbExtractor/CMakeLists.txt

@@ -1,30 +0,0 @@
-
-SET(SRC_FILES
-    main.cpp
-)
-
-SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/utilite/include
-	${PROJECT_SOURCE_DIR}/corelib/include
-    ${CMAKE_CURRENT_SOURCE_DIR}
-    ${UTILITE_INCLUDE_DIRS}
-    ${OpenCV_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-)
-
-SET(LIBRARIES
-    ${UTILITE_LIBRARIES}
-	${OpenCV_LIBRARIES}
-	${PCL_LIBRARIES}
-)
-
-add_definitions(${PCL_DEFINITIONS})
-
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
-ADD_EXECUTABLE(imagesDbExtractor ${SRC_FILES})
-TARGET_LINK_LIBRARIES(imagesDbExtractor rtabmap_core rtabmap_utilite ${LIBRARIES})
-
-SET_TARGET_PROPERTIES( imagesDbExtractor 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-imagesDbExtractor)
-

tools/ImagesDbExtractor/main.cpp

@@ -1,53 +0,0 @@
-#include <opencv2/core/core.hpp>
-#include <opencv2/core/types_c.h>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <iostream>
-#include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UDirectory.h>
-#include "rtabmap/core/Memory.h"
-#include "rtabmap/core/util3d.h"
-
-int main(int argc, char** argv)
-{
-	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
-
-	std::string path = rtabmap::Parameters::defaultRtabmapWorkingDirectory() + "/rtabmap.db";
-	if(argc > 1)
-	{
-		path = argv[1];
-	}
-
-
-    // Open database
-	rtabmap::ParametersMap parameters;
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
-	rtabmap::Memory * memory = new rtabmap::Memory(parameters);
-	if(!memory->init(path))
-	{
-		UWARN("Can't open database \"%s\"", path.c_str());
-	}
-
-	if(memory)
-	{
-		UINFO("Using database %s", path.c_str());
-		std::string saveDirectory = "imagesExtracted/";
-		if(!UDirectory::exists(saveDirectory))
-		{
-			UDirectory::makeDir(saveDirectory);
-		}
-		std::set<int> ids = memory->getAllSignatureIds();
-		for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
-		{
-			cv::Mat image = rtabmap::util3d::uncompressImage(memory->getImage(*iter));
-			std::string fileName = uFormat("%d.png", *iter);
-			cv::imwrite(saveDirectory+fileName, image);
-			UINFO("Saved %s", (saveDirectory+fileName).c_str());
-		}
-	}
-
-    return 0;
-}

tools/OdometryViewer/main.cpp

@@ -1,9 +1,11 @@
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UFile.h>
 #include <rtabmap/core/Odometry.h>
 #include <rtabmap/gui/OdometryViewer.h>
 #include <rtabmap/core/CameraOpenni.h>
+#include <rtabmap/core/DBReader.h>
 #include <QtGui/QApplication>
 
 void showUsage()
@@ -15,7 +17,12 @@ void showUsage()
 			"  -bin                      Use binary odometry (FAST+BRIEF)\n"
 			"  -icp                      Use ICP odometry\n"
 			"\n"
-			"  -in #.#                   Inliers maximum distance (default 0.005 m)\n"
+			"  -hz #.#                   Camera rate (default 0, 0 means as fast as the camera can)\n"
+			"  -db \"input.db\"          Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
+			"  -clouds #                 Maximum clouds shown (default 10, zero means inf)\n"
+			"  -sec #.#                  Delay (seconds) before reading the database (if set)\n"
+			"\n"
+			"  -in #.#                   Inliers maximum distance, features/ICP (default 0.005 m)\n"
 			"  -max #                    Max features used for matching (default 0=inf)\n"
 			"  -min #                    Minimum inliers to accept the transform (default 20)\n"
 			"  -depth #.#                Maximum features depth (default 5.0 m)\n"
@@ -23,16 +30,22 @@ void showUsage()
 			"  -lu #                     Linear update (default 0.0 m)\n"
 			"  -au #                     Angular update (default 0.0 radian)\n"
 			"  -reset #                  Reset countdown (default 0 = disabled)\n"
+			"\n"
+			"  -bin_brief_bytes #        BRIEF bytes (default 32)\n"
+			"  -bin_fast_thr #           FAST threshold (default 30)\n"
+			"  -bin_lsh                  Use nearest neighbor LSH (default brute force hamming)\n"
+			"\n"
 			"  -d #                      ICP decimation (default 4)\n"
 			"  -v #                      ICP voxel size (default 0.005)\n"
 			"  -s #                      ICP samples (default 0, not used if voxel is set.)\n"
 			"  -f #.#                    ICP fitness (default 0.01)\n"
+			"\n"
 			"  -debug                    Log debug messages\n"
 			"\n"
 			"Examples:\n"
 			"  odometryViewer -bow 0                                SURF example\n"
 			"  odometryViewer -bow 1                                SIFT example\n"
-			"  odometryViewer -bin                                  FAST/BRIEF example\n"
+			"  odometryViewer -bin -hz 10                           FAST/BRIEF example\n"
 			"  odometryViewer -icp -in 0.05 -i 30                   ICP example\n");
 	exit(1);
 }
@@ -43,6 +56,8 @@ int main (int argc, char * argv[])
 	ULogger::setLevel(ULogger::kInfo);
 
 	// parse arguments
+	float rate = 0.0;
+	std::string inputDatabase;
 	int odomType = 0; // 0=bow 1=bin 2=ICP
 	int bowType = 0;
 	float distance = 0.005;
@@ -57,6 +72,11 @@ int main (int argc, char * argv[])
 	float voxel = 0.005;
 	int samples = 10000;
 	float fitness = 0.01f;
+	int maxClouds = 10;
+	int briefBytes = 32;
+	int fastThr = 30;
+	bool useLSH = false;
+	float sec = 0.0f;
 
 	for(int i=1; i<argc; ++i)
 	{
@@ -78,6 +98,75 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
+		if(strcmp(argv[i], "-hz") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				rate = std::atof(argv[i]);
+				if(rate < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-db") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				inputDatabase = argv[i];
+				if(UFile::getExtension(inputDatabase).compare("db") != 0)
+				{
+					printf("Database path (%s) should end with \"db\" \n", inputDatabase.c_str());
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-clouds") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				maxClouds = std::atoi(argv[i]);
+				if(maxClouds < 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-sec") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				sec = std::atof(argv[i]);
+				if(sec < 0.0f)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
 		if(strcmp(argv[i], "-in") == 0)
 		{
 			++i;
@@ -282,6 +371,45 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
+		if(strcmp(argv[i], "-bin_brief_bytes") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				briefBytes = std::atoi(argv[i]);
+				if(briefBytes < 1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-bin_fast_thr") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				fastThr = std::atoi(argv[i]);
+				if(fastThr < 1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-bin_lsh") == 0)
+		{
+			useLSH = true;
+			continue;
+		}
 		if(strcmp(argv[i], "-bin") == 0)
 		{
 			odomType = 1;
@@ -302,6 +430,17 @@ int main (int argc, char * argv[])
 		showUsage();
 	}
 
+	if(inputDatabase.size())
+	{
+		UINFO("Using database input \"%s\"", inputDatabase.c_str());
+	}
+	else
+	{
+		UINFO("Using OpenNI camera");
+	}
+	UINFO("Camera rate = %f Hz", rate);
+	UINFO("Maximum clouds shown = %d", maxClouds);
+	UINFO("Delay = %f s", sec);
 	UINFO("Odometry used = %s", odomType==0?bowType==0?"Bag-of-words SURF":"Bag-of-words SIFT":odomType==1?"Binary (FAST+BRIEF)":"ICP");
 	UINFO("Inlier/ICP maximum correspondences distance = %f", distance);
 	UINFO("Max features = %d", maxWords);
@@ -315,10 +454,12 @@ int main (int argc, char * argv[])
 	UINFO("Cloud voxel size = %f", voxel);
 	UINFO("Cloud samples = %d", samples);
 	UINFO("Cloud fitness = %f", fitness);
+	UINFO("Binary BRIEF bytes = %d", briefBytes);
+	UINFO("Binary FAST threshold = %f", fastThr);
+	UINFO("Binary LSH = %f", useLSH?"true":"false");
 
 	QApplication app(argc, argv);
 
-	rtabmap::CameraOpenni camera("", 0, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 	rtabmap::Odometry * odom = 0;
 
 	if(odomType == 0)
@@ -344,7 +485,11 @@ int main (int argc, char * argv[])
 				maxDepth,
 				linearUpdate,
 				angularUpdate,
-				resetCountdown);
+				resetCountdown,
+				briefBytes,
+				fastThr,
+				true,
+				!useLSH);
 	}
 	else // ICP
 	{
@@ -361,17 +506,20 @@ int main (int argc, char * argv[])
 				resetCountdown);
 	}
 	rtabmap::OdometryThread odomThread(odom);
-	rtabmap::OdometryViewer odomViewer(100, 2, 0.0);
+	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0);
 	UEventsManager::addHandler(&odomThread);
 	UEventsManager::addHandler(&odomViewer);
 
 	odomViewer.setWindowTitle("Odometry viewer");
-	odomViewer.setMinimumWidth(500);
+	odomViewer.setMinimumWidth(800);
-	odomViewer.setMinimumHeight(300);
+	odomViewer.setMinimumHeight(500);
-	odomViewer.showMaximized();
+	odomViewer.showNormal();
 
 	app.processEvents();
 
+	if(inputDatabase.size())
+	{
+		rtabmap::DBReader camera(inputDatabase, rate, true, sec);
 		if(camera.init())
 		{
 			odomThread.start();
@@ -382,6 +530,21 @@ int main (int argc, char * argv[])
 			camera.kill();
 			odomThread.join(true);
 		}
+	}
+	else
+	{
+		rtabmap::CameraOpenni camera("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+		if(camera.init())
+		{
+			odomThread.start();
+			camera.start();
+
+			app.exec();
+
+			camera.kill();
+			odomThread.join(true);
+		}
+	}
 
 	return 0;
 }

utilite/src/UEventsManager.cpp

@@ -223,9 +223,13 @@ void UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
 			UEventsHandler * handler = *it;
 			handlersMutex_.unlock();
 
+			// Don't process event if the handler is the same as the sender
+			if(handler != sender)
+			{
 				// To be able to add/remove an handler in a handleEvent call (without a deadlock)
 				// @see _addHandler(), _removeHandler()
 				handler->handleEvent(event);
+			}
 
 			handlersMutex_.lock();
 		}