Added c++ example of RTAB-Map RGB-D version

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

examples/BOWMapping/CMakeLists.txt

@@ -0,0 +1,18 @@
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/corelib/include
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES} 
+)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(bow_mapping main.cpp)
+TARGET_LINK_LIBRARIES(bow_mapping rtabmap_core ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( bow_mapping 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-bow_mapping)

examples/BOWMapping/main.cpp

@@ -25,7 +25,7 @@
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-example [options] \"path\"\n"
+			"rtabmap-bow_mapping [options] \"path\"\n"
 			"  path       Path to a directory of images\n "
 			"  Options:"
 			"     -l   localization mode: use already built RTAB-Map database to localize\n ");

examples/CMakeLists.txt

@@ -1,26 +1,11 @@
 
-SET(SRC_FILES
-    main.cpp
-)
+ADD_SUBDIRECTORY( BOWMapping )
 
-SET(INCLUDE_DIRS
-	${PROJECT_SOURCE_DIR}/corelib/include
-    ${OpenCV_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-)
+IF(TARGET rtabmap_gui)
+	ADD_SUBDIRECTORY( RGBDMapping )
+ELSE()
+   MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping example will not be built...")
+ENDIF()
 
-SET(LIBRARIES
-	${OpenCV_LIBRARIES} 
-	${PCL_LIBRARIES}
-)
 
-add_definitions(${PCL_DEFINITIONS})
-
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
-ADD_EXECUTABLE(example ${SRC_FILES})
-TARGET_LINK_LIBRARIES(example rtabmap_core ${LIBRARIES})
-
-SET_TARGET_PROPERTIES( example 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-example)
 

examples/RGBDMapping/CMakeLists.txt

@@ -0,0 +1,27 @@
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+INCLUDE(${QT_USE_FILE})
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES}
+	${QT_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+QT4_WRAP_CPP(moc_srcs MapBuilder.h)
+
+ADD_EXECUTABLE(rgbd_mapping main.cpp ${moc_srcs})
+  
+TARGET_LINK_LIBRARIES(rgbd_mapping rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( rgbd_mapping 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_mapping)

examples/RGBDMapping/MapBuilder.h

@@ -0,0 +1,206 @@
+/*
+ * MapBuilder.h
+ *
+ *  Created on: 2014-02-24
+ *      Author: mathieu
+ */
+
+#ifndef MAPBUILDER_H_
+#define MAPBUILDER_H_
+
+#include <QtGui/QVBoxLayout>
+#include <QtCore/QMetaType>
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/OdometryEvent.h"
+
+using namespace rtabmap;
+
+// This class receives RtabmapEvent and construct/update a 3D Map
+class MapBuilder : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	MapBuilder()
+	{
+		this->setWindowFlags(Qt::Dialog);
+		this->setWindowTitle(tr("3D Map"));
+		this->setMinimumWidth(800);
+		this->setMinimumHeight(600);
+
+		cloudViewer_ = new CloudViewer(this);
+
+		QVBoxLayout *layout = new QVBoxLayout();
+		layout->addWidget(cloudViewer_);
+		this->setLayout(layout);
+
+		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
+		qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
+	}
+
+	virtual ~MapBuilder()
+	{
+		this->unregisterFromEventsManager();
+	}
+
+private slots:
+	void processOdometry(const rtabmap::Image & data)
+	{
+		if(!this->isVisible())
+		{
+			return;
+		}
+
+		Transform pose = data.pose();
+		if(pose.isNull())
+		{
+			//Odometry lost
+			cloudViewer_->setBackgroundColor(Qt::darkRed);
+
+			pose = lastOdomPose_;
+		}
+		else
+		{
+			cloudViewer_->setBackgroundColor(Qt::black);
+		}
+		if(!pose.isNull())
+		{
+			lastOdomPose_ = pose;
+
+			// 3d cloud
+			if(data.depth().cols == data.image().cols &&
+			   data.depth().rows == data.image().rows &&
+			   !data.depth().empty() &&
+			   data.depthConstant() > 0.0f)
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
+					data.image(),
+					data.depth(),
+					float(data.depth().cols/2),
+					float(data.depth().rows/2),
+					1.0f/data.depthConstant(),
+					1.0f/data.depthConstant(),
+					2); // decimation // high definition
+				if(cloud->size())
+				{
+					cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
+					if(cloud->size())
+					{
+						cloud = util3d::transformPointCloud(cloud, data.localTransform());
+					}
+				}
+				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, pose))
+				{
+					UERROR("Adding cloudOdom to viewer failed!");
+				}
+			}
+
+			if(!data.pose().isNull())
+			{
+				// update camera position
+				cloudViewer_->updateCameraPosition(data.pose());
+			}
+		}
+		cloudViewer_->render();
+	}
+
+
+	void processStatistics(const rtabmap::Statistics & stats)
+	{
+		if(!this->isVisible())
+		{
+			return;
+		}
+
+		const std::map<int, Transform> & poses = stats.poses();
+		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(!iter->second.isNull())
+			{
+				std::string cloudName = uFormat("cloud%d", iter->first);
+
+				// 3d point cloud
+				if(clouds.contains(cloudName))
+				{
+					// Update only if the pose has changed
+					Transform tCloud;
+					cloudViewer_->getPose(cloudName, tCloud);
+					if(tCloud.isNull() || iter->second != tCloud)
+					{
+						if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
+						{
+							UERROR("Updating pose cloud %d failed!", iter->first);
+						}
+					}
+					cloudViewer_->setCloudVisibility(cloudName, true);
+				}
+				else if(iter->first == stats.refImageId() &&
+						uContains(stats.getImages(), iter->first) &&
+						uContains(stats.getDepths(), iter->first) &&
+						uContains(stats.getDepthConstants(), iter->first) &&
+						uContains(stats.getLocalTransforms(), iter->first))
+				{
+					// Add the new cloud
+					cv::Mat rgb = util3d::uncompressImage(stats.getImages().at(iter->first));
+					cv::Mat depth = util3d::uncompressImage(stats.getDepths().at(iter->first));
+					float depthConstant = stats.getDepthConstants().at(iter->first);
+					Transform localTransform = stats.getLocalTransforms().at(iter->first);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
+							rgb,
+							depth,
+						   float(depth.cols/2),
+						   float(depth.rows/2),
+						   1.0f/depthConstant,
+						   1.0f/depthConstant,
+						   8); // decimation
+
+					if(cloud->size())
+					{
+						cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
+						if(cloud->size())
+						{
+							cloud = util3d::transformPointCloud(cloud, localTransform);
+						}
+					}
+					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
+					{
+						UERROR("Adding cloud %d to viewer failed!", iter->first);
+					}
+				}
+			}
+		}
+
+		cloudViewer_->render();
+	}
+
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("RtabmapEvent") == 0)
+		{
+			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));
+		}
+		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::Image, odomEvent->data()));
+		}
+	}
+
+private:
+	CloudViewer * cloudViewer_;
+	Transform lastOdomPose_;
+};
+
+
+#endif /* MAPBUILDER_H_ */

examples/RGBDMapping/main.cpp

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/RtabmapThread.h"
+#include "rtabmap/core/CameraOpenni.h"
+#include "rtabmap/core/Odometry.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include <QtGui/QApplication>
+#include <stdio.h>
+
+#include "MapBuilder.h"
+
+using namespace rtabmap;
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
+	QApplication app(argc, argv);
+	MapBuilder mapBuilder;
+
+	// Here is the pipeline that we will use:
+	// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
+
+	// 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
+	CameraOpenni camera("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+	if(!camera.init())
+	{
+		UERROR("Camera init failed!");
+		exit(1);
+	}
+
+	// Create an odometry thread to process camera events, it will send OdometryEvent.
+	OdometryThread odomThread(new OdometryBOW(0)); // 0=SURF 1=SIFT
+
+
+	// Create RTAB-Map to process OdometryEvent
+	Rtabmap * rtabmap = new Rtabmap();
+	rtabmap->init();
+	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
+	rtabmapThread.setDetectorRate(0.0f); // as fast as we can
+
+	// Setup handlers
+	odomThread.registerToEventsManager();
+	rtabmapThread.registerToEventsManager();
+	mapBuilder.registerToEventsManager();
+
+	// The RTAB-Map is subscribed by default to CameraEvent, but we want
+	// RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent.
+	// We can do that by creating a "pipe" between the camera and odometry, then
+	// only the odometry will receive CameraEvent from that camera. RTAB-Map is
+	// also subscribed to OdometryEvent by default, so no need to create a pipe between
+	// odometry and RTAB-Map.
+	UEventsManager::createPipe(&camera, &odomThread, "CameraEvent");
+
+	// Let's start the threads
+	rtabmapThread.start();
+	odomThread.start();
+	camera.start();
+
+	mapBuilder.show();
+	app.exec(); // main loop
+
+	// remove handlers
+	mapBuilder.unregisterFromEventsManager();
+	rtabmapThread.unregisterFromEventsManager();
+	odomThread.unregisterFromEventsManager();
+
+	// Kill all threads
+	camera.kill();
+	odomThread.join(true);
+	rtabmapThread.join(true);
+
+	return 0;
+}