Added noEventsExample

corelib/include/rtabmap/core/SensorData.h

@@ -136,8 +136,8 @@ public:
 			_laserScanCompressed.empty() &&
 			_cameraModels.size() == 0 &&
 			!_stereoCameraModel.isValid() &&
-			!_userDataRaw.empty() &&
+			_userDataRaw.empty() &&
-			!_userDataCompressed.empty() &&
+			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
 			_descriptors.empty());
 	}

examples/CMakeLists.txt

@@ -4,6 +4,7 @@ ADD_SUBDIRECTORY( BOWMapping )
 IF(TARGET rtabmap_gui)
    ADD_SUBDIRECTORY( RGBDMapping )
    ADD_SUBDIRECTORY( WifiMapping )
+   ADD_SUBDIRECTORY( NoEventsExample )
 ELSE()
    MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
 ENDIF()

examples/NoEventsExample/CMakeLists.txt

@@ -0,0 +1,33 @@
+
+SET(INCLUDE_DIRS
+	${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    INCLUDE(${QT_USE_FILE})
+ENDIF()
+
+SET(LIBRARIES
+	${OpenCV_LIBRARIES}
+	${QT_LIBRARIES} 
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    QT4_WRAP_CPP(moc_srcs MapBuilder.h)
+ELSE()
+    QT5_WRAP_CPP(moc_srcs MapBuilder.h)
+ENDIF()
+
+ADD_EXECUTABLE(noEventsExample main.cpp ${moc_srcs})
+  
+TARGET_LINK_LIBRARIES(noEventsExample rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( noEventsExample 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-noEventsExample)

examples/NoEventsExample/MapBuilder.h

@@ -0,0 +1,241 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef MAPBUILDER_H_
+#define MAPBUILDER_H_
+
+#include <QVBoxLayout>
+#include <QtCore/QMetaType>
+#include <QAction>
+
+#ifndef Q_MOC_RUN // Mac OS X issue
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Signature.h"
+#endif
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+
+using namespace rtabmap;
+
+// This class receives RtabmapEvent and construct/update a 3D Map
+class MapBuilder : public QWidget
+{
+	Q_OBJECT
+public:
+	//Camera ownership is not transferred!
+	MapBuilder() :
+		odometryCorrection_(Transform::getIdentity()),
+		paused_(false)
+	{
+		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);
+
+		QAction * pause = new QAction(this);
+		this->addAction(pause);
+		pause->setShortcut(Qt::Key_Space);
+		connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
+	}
+
+	virtual ~MapBuilder()
+	{
+	}
+
+	bool isPaused() const {return paused_;}
+
+	void processOdometry(
+			const SensorData & data,
+			Transform pose,
+			const rtabmap::OdometryInfo & odom)
+	{
+		if(!this->isVisible())
+		{
+			return;
+		}
+
+		if(pose.isNull())
+		{
+			//Odometry lost
+			cloudViewer_->setBackgroundColor(Qt::darkRed);
+
+			pose = lastOdomPose_;
+		}
+		else
+		{
+			cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
+		}
+		if(!pose.isNull())
+		{
+			lastOdomPose_ = pose;
+
+			// 3d cloud
+			if(data.depthOrRightRaw().cols == data.imageRaw().cols &&
+			   data.depthOrRightRaw().rows == data.imageRaw().rows &&
+			   !data.depthOrRightRaw().empty() &&
+			   (data.stereoCameraModel().isValid() || data.cameraModels().size()))
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+					data,
+					4,     // decimation
+					0.0f); // max depth
+				if(cloud->size())
+				{
+					if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose))
+					{
+						UERROR("Adding cloudOdom to viewer failed!");
+					}
+				}
+				else
+				{
+					cloudViewer_->setCloudVisibility("cloudOdom", false);
+					UWARN("Empty cloudOdom!");
+				}
+			}
+
+			if(!pose.isNull())
+			{
+				// update camera position
+				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*pose);
+			}
+		}
+		cloudViewer_->update();
+	}
+
+
+	void processStatistics(const rtabmap::Statistics & stats)
+	{
+
+		//============================
+		// Add RGB-D clouds
+		//============================
+		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(uContains(stats.getSignatures(), iter->first))
+				{
+					Signature s = stats.getSignatures().at(iter->first);
+					s.sensorData().uncompressData(); // make sure data is uncompressed
+					// Add the new cloud
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+							s.sensorData(),
+							4,     // decimation
+							4.0f); // max depth
+					if(cloud->size())
+					{
+						if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
+						{
+							UERROR("Adding cloud %d to viewer failed!", iter->first);
+						}
+					}
+					else
+					{
+						UWARN("Empty cloud %d!", iter->first);
+					}
+				}
+			}
+			else
+			{
+				UWARN("Null pose for %d ?!?", iter->first);
+			}
+		}
+
+		//============================
+		// Add 3D graph (show all poses)
+		//============================
+		cloudViewer_->removeAllGraphs();
+		cloudViewer_->removeCloud("graph_nodes");
+		if(poses.size())
+		{
+			// Set graph
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+			}
+			*graphNodes = *graph;
+
+
+			// add graph
+			cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
+			cloudViewer_->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
+			cloudViewer_->setCloudPointSize("graph_nodes", 5);
+		}
+
+		odometryCorrection_ = stats.mapCorrection();
+
+		cloudViewer_->update();
+	}
+
+protected slots:
+	void pauseDetection()
+	{
+		paused_ = !paused_;
+	}
+
+protected:
+	CloudViewer * cloudViewer_;
+	Transform lastOdomPose_;
+	Transform odometryCorrection_;
+	bool paused_;
+};
+
+
+#endif /* MAPBUILDER_H_ */

examples/NoEventsExample/main.cpp

@@ -0,0 +1,168 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/CameraStereo.h>
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/utilite/UThread.h>
+#include "MapBuilder.h"
+#include <pcl/visualization/cloud_viewer.h>
+#include <QApplication>
+#include <stdio.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-noEventsExample camera_rate odom_update map_update calibration_dir calibration_name path_left_images path_right_images\n"
+			"Description:\n"
+			"    camera_rate          Rate (Hz) of the camera.\n"
+			"    odom_update          Do odometry update each X camera frames.\n"
+			"    map_update           Do map update each X odometry frames.\n"
+			"\n"
+			"Example:\n"
+			"     (with images from \"https://github.com/introlab/rtabmap/wiki/Stereo-mapping#process-a-directory-of-stereo-images\") \n"
+			"     $ rtabmap-noEventsExample 20 2 10 stereo_20hz stereo_20Hz stereo_20hz/left stereo_20hz/right\n"
+			"       Camera rate = 20 Hz\n"
+			"       Odometry update rate = 10 Hz\n"
+			"       Map update rate = 1 Hz\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	if(argc < 8)
+	{
+		showUsage();
+	}
+
+	int argIndex = 1;
+	int cameraRate = atoi(argv[argIndex++]);
+	if(cameraRate <= 0)
+	{
+		printf("camera_rate should be > 0\n");
+		showUsage();
+	}
+	int odomUpdate = atoi(argv[argIndex++]);
+	if(odomUpdate <= 0)
+	{
+		printf("odom_update should be > 0\n");
+		showUsage();
+	}
+	int mapUpdate = atoi(argv[argIndex++]);
+	if(mapUpdate <= 0)
+	{
+		printf("map_update should be > 0\n");
+		showUsage();
+	}
+
+	printf("Camera rate = %d Hz\n", cameraRate);
+	printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
+	printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
+
+	std::string calibrationDir = argv[argIndex++];
+	std::string calibrationName = argv[argIndex++];
+	std::string pathLeftImages = argv[argIndex++];
+	std::string pathRightImages = argv[argIndex++];
+
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	CameraStereoImages camera(
+			pathLeftImages,
+			pathRightImages,
+			false, // no timestamps in filenames
+			"",    // no file with timestamps
+			false, // assume that images are already rectified
+			0.0f,  // inf rate
+			opticalRotation);
+
+	if(camera.init(calibrationDir, calibrationName))
+	{
+		OdometryBOW odom;
+		Rtabmap rtabmap;
+		rtabmap.init();
+
+		QApplication app(argc, argv);
+		MapBuilder mapBuilder;
+		mapBuilder.show();
+		QApplication::processEvents();
+		float sleepTime = 1.0f/float(cameraRate);
+
+		SensorData data = camera.takeImage();
+		int cameraIteration = 0;
+		int odometryIteration = 0;
+		printf("Press \"Space\" in the window to pause\n");
+		while(data.isValid() && mapBuilder.isVisible())
+		{
+			if(cameraIteration++ % odomUpdate == 0)
+			{
+				OdometryInfo info;
+				Transform pose = odom.process(data, &info);
+
+				if(odometryIteration++ % mapUpdate == 0)
+				{
+					if(rtabmap.process(data, pose))
+					{
+						mapBuilder.processStatistics(rtabmap.getStatistics());
+						if(rtabmap.getLoopClosureId() > 0)
+						{
+							printf("Loop closure detected!\n");
+						}
+					}
+				}
+
+				mapBuilder.processOdometry(data, pose, info);
+			}
+
+			QApplication::processEvents();
+
+			while(mapBuilder.isPaused() && mapBuilder.isVisible())
+			{
+				uSleep(100);
+				QApplication::processEvents();
+			}
+
+			data = camera.takeImage();
+			uSleep(sleepTime);
+		}
+
+		if(mapBuilder.isVisible())
+		{
+			printf("Processed all frames\n");
+			app.exec();
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	return 0;
+}