Stereo! The memory can now handle directly stereo images. Disparity can be computed on the fly by keeping left and right images, so for features extraction (and for re-extraction on loop closure), we can compute 3D points precisely. New parameters can be found under "RGB-D Mapping->Stereo". Full image disparity is reconstructed in the GUI (not the core).

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1861 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-10-03 16:27:46 +08:00 · 2014-10-16 00:14:23 +00:00
parent 8f451029e2
commit fcd3301665
18 changed files with 1230 additions and 260 deletions
@@ -46,7 +46,9 @@ class MapBuilder : public QWidget, public UEventsHandler
 {
 	Q_OBJECT
 public:
-	MapBuilder()
+	MapBuilder() :
+		_processingStatistics(false),
+		_lastOdometryProcessed(true)
 	{
 		this->setWindowFlags(Qt::Dialog);
 		this->setWindowTitle(tr("3D Map"));
@@ -60,7 +62,7 @@ public:
 		this->setLayout(layout);

 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
-		qRegisterMetaType<rtabmap::SensorData>("rtabmap::Image");
+		qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 	}

 	virtual ~MapBuilder()
@@ -128,15 +130,14 @@ private slots:
 			}
 		}
 		cloudViewer_->render();
+
+		_lastOdometryProcessed = true;
 	}


 	void processStatistics(const rtabmap::Statistics & stats)
 	{
-		if(!this->isVisible())
-		{
-			return;
-		}
+		_processingStatistics = true;

 		const std::map<int, Transform> & poses = stats.poses();
 		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
@@ -198,6 +199,8 @@ private slots:
 		}

 		cloudViewer_->render();
+
+		_processingStatistics = false;
 	}

 protected:
@@ -208,19 +211,30 @@ protected:
 			RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
 			const Statistics & stats = rtabmapEvent->getStats();
 			// Statistics must be processed in the Qt thread
-			QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
+			if(this->isVisible())
+			{
+				QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
+			}
 		}
 		else if(event->getClassName().compare("OdometryEvent") == 0)
 		{
 			OdometryEvent * odomEvent = (OdometryEvent *)event;
 			// Odometry must be processed in the Qt thread
-			QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
+			if(this->isVisible() &&
+			   _lastOdometryProcessed &&
+			   !_processingStatistics)
+			{
+				_lastOdometryProcessed = false; // if we receive too many odometry events!
+				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
+			}
 		}
 	}

 private:
 	CloudViewer * cloudViewer_;
 	Transform lastOdomPose_;
+	bool _processingStatistics;
+	bool _lastOdometryProcessed;
 };


@@ -36,12 +36,35 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "MapBuilder.h"

+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-rgbd_mapping driver\n"
+			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+	exit(1);
+}
+
 using namespace rtabmap;
 int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);

+	int driver = 0;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		driver = atoi(argv[argc-1]);
+		if(driver < 0 || driver > 4)
+		{
+			UERROR("driver should be between 0 and 4.");
+			showUsage();
+		}
+	}
+
 	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
 	QApplication app(argc, argv);
 	MapBuilder mapBuilder;
@@ -51,8 +74,49 @@ int main(int argc, char * argv[])

 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
-	CameraRGBD * camera = new CameraOpenni("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
-	//CameraRGBD * camera = new CameraFreenect(0, 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+	CameraRGBD * camera = 0;
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	if(driver == 1)
+	{
+		if(!CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNI2(0, opticalRotation);
+	}
+	else if(driver == 2)
+	{
+		if(!CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect(0, 0, opticalRotation);
+	}
+	else if(driver == 3)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(false, 0, opticalRotation);
+	}
+	else if(driver == 4)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(true, 0, opticalRotation);
+	}
+	else
+	{
+		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
+	}
+
 	CameraThread cameraThread(camera);
 	if(!cameraThread.init())
 	{
@@ -61,7 +125,7 @@ int main(int argc, char * argv[])
 	}

 	// Create an odometry thread to process camera events, it will send OdometryEvent.
-	OdometryThread odomThread(new OdometryBOW()); // 0=SURF 1=SIFT
+	OdometryThread odomThread(new OdometryBOW());


 	// Create RTAB-Map to process OdometryEvent