Merged pcl_integration branch to trunk

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

guilib/src/AboutDialog.cpp

@@ -21,6 +21,7 @@
 #include "rtabmap/core/Rtabmap.h"
 #include "ui_aboutDialog.h"
 #include <opencv2/core/version.hpp>
+#include <pcl/pcl_config.h>
 
 namespace rtabmap {
 
@@ -29,8 +30,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 {
 	_ui = new Ui_aboutDialog();
 	_ui->setupUi(this);
-	_ui->label_version->setText(Rtabmap::getVersion().c_str());
+	QString version = Rtabmap::getVersion().c_str();
+#if DEMO_BUILD
+	version.append(" [DEMO]");
+#endif
+	_ui->label_version->setText(version);
 	_ui->label_opencv_version->setText(CV_VERSION);
+	_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
 }
 
 AboutDialog::~AboutDialog()

guilib/src/CMakeLists.txt

@@ -12,6 +12,10 @@ SET(headers_ui
 	./StatsToolBox.h
 	./DetailedProgressDialog.h
 	./utilite/UPlot.h
+	../include/${PROJECT_PREFIX}/gui/CloudViewer.h
+	../include/${PROJECT_PREFIX}/gui/OdometryViewer.h
+	../include/${PROJECT_PREFIX}/gui/LoopClosureViewer.h
+	../include/${PROJECT_PREFIX}/gui/DataRecorder.h
 )
 
 SET(uis
@@ -20,6 +24,7 @@ SET(uis
     ./ui/aboutDialog.ui
     ./ui/consoleWidget.ui
     ./ui/DatabaseViewer.ui
+    ./ui/loopClosureViewer.ui
 )
 
 SET(qrc 
@@ -43,7 +48,6 @@ SET(SRC_FILES
 	./MainWindow.cpp
 	./PreferencesDialog.cpp
 	./KeypointItem.cpp
-	./qtipl.cpp
 	./ImageView.cpp
 	./PdfPlot.cpp
 	./StatsToolBox.cpp
@@ -52,6 +56,10 @@ SET(SRC_FILES
 	./ConsoleWidget.cpp
 	./DatabaseViewer.cpp
 	./utilite/UPlot.cpp
+	./CloudViewer.cpp
+	./OdometryViewer.cpp
+	./LoopClosureViewer.cpp
+	./DataRecorder.cpp
 	${moc_srcs} 
 	${moc_uis} 
 	${srcs_qrc}
@@ -64,33 +72,42 @@ SET(INCLUDE_DIRS
 	${CMAKE_CURRENT_SOURCE_DIR}
 	${OpenCV_INCLUDE_DIRS}
 	${CMAKE_CURRENT_BINARY_DIR} # for qt ui generated in binary dir
+	${PCL_INCLUDE_DIRS}
 )
 
 INCLUDE(${QT_USE_FILE})
 
+INCLUDE(${VTK_USE_FILE})
+
 SET(LIBRARIES
     ${QT_LIBRARIES} 
 	${OpenCV_LIBS} 
+	${PCL_LIBRARIES}
+	QVTK 
+	vtkHybrid
 )
 
 #include files
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
+add_definitions(${PCL_DEFINITIONS})
+
 # create a library from the source files
-ADD_LIBRARY(rtabmap_guilib ${SRC_FILES})
+ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
 # Linking with Qt libraries
-TARGET_LINK_LIBRARIES(rtabmap_guilib rtabmap_corelib rtabmap_utilite ${LIBRARIES})
+TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES})
 
 SET_TARGET_PROPERTIES(
-rtabmap_guilib 
+rtabmap_gui
 PROPERTIES
  OUTPUT_NAME ${PROJECT_PREFIX}_gui
  INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
 )
 
-INSTALL(TARGETS rtabmap_guilib
-        RUNTIME DESTINATION bin COMPONENT runtime
-        LIBRARY DESTINATION lib COMPONENT devel
-        ARCHIVE DESTINATION lib COMPONENT devel)
+INSTALL(TARGETS rtabmap_gui
+        EXPORT RTABMapTargets
+        RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
+        LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel
+        ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel)
         
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION "${INSTALL_INCLUDE_DIR}" COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)

guilib/src/CloudViewer.cpp

@@ -0,0 +1,558 @@
+/*
+ * CloudViewer.cpp
+ *
+ *  Created on: 2013-10-13
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/gui/CloudViewer.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/util3d.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <QtGui/QMenu>
+#include <QtGui/QAction>
+#include <QtGui/QContextMenuEvent>
+#include <QtGui/QInputDialog>
+#include <QtGui/QWheelEvent>
+
+#include <vtkRenderWindow.h>
+
+namespace rtabmap {
+
+CloudViewer::CloudViewer(QWidget *parent) :
+		QVTKWidget(parent),
+#if defined(WIN32) || defined(__APPLE__)
+		_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer")),
+#else
+		_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)),
+#endif
+		_aLockCamera(0),
+		_aFollowCamera(0),
+		_aResetCamera(0),
+		_aLockViewZ(0),
+		_aShowTrajectory(0),
+		_aSetTrajectorySize(0),
+		_aClearTrajectory(0),
+		_aShowGrid(0),
+		_menu(0),
+		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
+		_maxTrajectorySize(100),
+		_lastPose(Transform::getIdentity())
+{
+	this->setMinimumSize(200, 200);
+
+	this->SetRenderWindow(_visualizer->getRenderWindow());
+
+#if !defined(WIN32) && !defined(__APPLE__)
+	_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
+#endif
+
+	_visualizer->setCameraPosition(
+				-1, 0, 0,
+				0, 0, 0,
+				0, 0, 1);
+
+	//setup menu/actions
+	createMenu();
+}
+
+CloudViewer::~CloudViewer()
+{
+	UDEBUG("");
+	//_visualizer->close();
+	delete _visualizer;
+}
+
+void CloudViewer::createMenu()
+{
+	_aLockCamera = new QAction("Lock target", this);
+	_aLockCamera->setCheckable(true);
+	_aLockCamera->setChecked(false);
+	_aFollowCamera = new QAction("Follow", this);
+	_aFollowCamera->setCheckable(true);
+	_aFollowCamera->setChecked(true);
+	QAction * freeCamera = new QAction("Free", this);
+	freeCamera->setCheckable(true);
+	freeCamera->setChecked(false);
+	_aLockViewZ = new QAction("Lock view Z", this);
+	_aLockViewZ->setCheckable(true);
+	_aLockViewZ->setChecked(true);
+	_aResetCamera = new QAction("Reset position", this);
+	_aShowTrajectory= new QAction("Show trajectory", this);
+	_aShowTrajectory->setCheckable(true);
+	_aShowTrajectory->setChecked(true);
+	_aSetTrajectorySize = new QAction("Set trajectory size...", this);
+	_aClearTrajectory = new QAction("Clear trajectory", this);
+	_aShowGrid = new QAction("Show grid", this);
+	_aShowGrid->setCheckable(true);
+
+	QMenu * cameraMenu = new QMenu("Camera", this);
+	cameraMenu->addAction(_aLockCamera);
+	cameraMenu->addAction(_aFollowCamera);
+	cameraMenu->addAction(freeCamera);
+	cameraMenu->addSeparator();
+	cameraMenu->addAction(_aLockViewZ);
+	cameraMenu->addAction(_aResetCamera);
+	QActionGroup * group = new QActionGroup(this);
+	group->addAction(_aLockCamera);
+	group->addAction(_aFollowCamera);
+	group->addAction(freeCamera);
+
+	QMenu * trajectoryMenu = new QMenu("Trajectory", this);
+	trajectoryMenu->addAction(_aShowTrajectory);
+	trajectoryMenu->addAction(_aSetTrajectorySize);
+	trajectoryMenu->addAction(_aClearTrajectory);
+
+	//menus
+	_menu = new QMenu(this);
+	_menu->addMenu(cameraMenu);
+	_menu->addMenu(trajectoryMenu);
+	_menu->addAction(_aShowGrid);
+}
+
+bool CloudViewer::updateCloudPose(
+		const std::string & id,
+		const Transform & pose)
+{
+	if(_addedClouds.contains(id))
+	{
+		UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
+		if(_addedClouds.find(id).value() == pose ||
+		   _visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose)))
+		{
+			_addedClouds.find(id).value() = pose;
+			return true;
+		}
+	}
+	return false;
+}
+
+bool CloudViewer::updateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & pose)
+{
+	if(_addedClouds.contains(id))
+	{
+		UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
+		int index = _visualizer->getColorHandlerIndex(id);
+		this->removeCloud(id);
+		if(this->addCloud(id, cloud, pose))
+		{
+			_visualizer->updateColorHandlerIndex(id, index);
+			return true;
+		}
+	}
+	return false;
+}
+
+bool CloudViewer::updateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & pose)
+{
+	if(_addedClouds.contains(id))
+	{
+		UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
+		int index = _visualizer->getColorHandlerIndex(id);
+		this->removeCloud(id);
+		if(this->addCloud(id, cloud, pose))
+		{
+			_visualizer->updateColorHandlerIndex(id, index);
+			return true;
+		}
+	}
+	return false;
+}
+
+bool CloudViewer::addOrUpdateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & pose)
+{
+	if(!updateCloud(id, cloud, pose))
+	{
+		return addCloud(id, cloud, pose);
+	}
+	return true;
+}
+
+bool CloudViewer::addOrUpdateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & pose)
+{
+	if(!updateCloud(id, cloud, pose))
+	{
+		return addCloud(id, cloud, pose);
+	}
+	return true;
+}
+
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PCLPointCloud2Ptr & binaryCloud,
+		const Transform & pose,
+		bool rgb)
+{
+	if(!_addedClouds.contains(id))
+	{
+		Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
+		Eigen::Quaternionf orientation = Eigen::Quaternionf(util3d::transformToEigen3f(pose).rotation());
+
+		// add random color channel
+		 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
+		 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
+		 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
+		{
+			 // white
+			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, 255, 255, 255));
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+
+			// x,y,z
+			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+
+			if(rgb)
+			{
+				//rgb
+				colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
+				_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+
+				_visualizer->updateColorHandlerIndex(id, 5);
+			}
+
+			_addedClouds.insert(id, pose);
+			return true;
+		}
+	}
+	return false;
+}
+
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & pose)
+{
+	if(!_addedClouds.contains(id))
+	{
+		UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
+
+		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
+		return addCloud(id, binaryCloud, pose, true);
+	}
+	return false;
+}
+
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & pose)
+{
+	if(!_addedClouds.contains(id))
+	{
+		UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
+
+		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
+		return addCloud(id, binaryCloud, pose, false);
+	}
+	return false;
+}
+
+bool CloudViewer::addCloudMesh(
+	const std::string & id,
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+	const std::vector<pcl::Vertices> & polygons,
+	const Transform & pose)
+{
+	if(!_addedClouds.contains(id))
+	{
+		UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
+		if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id))
+		{
+			_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose));
+			_addedClouds.insert(id, pose);
+			return true;
+		}
+	}
+	return false;
+}
+
+bool CloudViewer::addCloudMesh(
+	const std::string & id,
+	const pcl::PolygonMesh::Ptr & mesh,
+	const Transform & pose)
+{
+	if(!_addedClouds.contains(id))
+	{
+		UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
+		if(_visualizer->addPolygonMesh(*mesh, id))
+		{
+			_visualizer->updatePointCloudPose(id, util3d::transformToEigen3f(pose));
+			_addedClouds.insert(id, pose);
+			return true;
+		}
+	}
+	return false;
+}
+
+void CloudViewer::setTrajectoryShown(bool shown)
+{
+	_aShowTrajectory->setChecked(shown);
+}
+
+void CloudViewer::setTrajectorySize(int value)
+{
+	_maxTrajectorySize = value;
+}
+
+void CloudViewer::removeAllClouds()
+{
+	_addedClouds.clear();
+	_visualizer->removeAllPointClouds();
+}
+
+
+bool CloudViewer::removeCloud(const std::string & id)
+{
+	_addedClouds.remove(id);
+	return _visualizer->removePointCloud(id);
+}
+
+bool CloudViewer::getPose(const std::string & id, Transform & pose)
+{
+	if(_addedClouds.contains(id))
+	{
+		pose = _addedClouds.value(id);
+		return true;
+	}
+	return false;
+}
+
+void CloudViewer::updateCameraPosition(const Transform & pose)
+{
+	if(!pose.isNull())
+	{
+		Eigen::Affine3f m = util3d::transformToEigen3f(pose);
+		Eigen::Vector3f pos = m.translation();
+
+		Eigen::Vector3f lastPos(0,0,0);
+		if(_trajectory->size())
+		{
+			lastPos[0]=_trajectory->back().x;
+			lastPos[1]=_trajectory->back().y;
+			lastPos[2]=_trajectory->back().z;
+		}
+
+		_trajectory->push_back(pcl::PointXYZ(pos[0], pos[1], pos[2]));
+		if(_maxTrajectorySize>0)
+		{
+			while(_trajectory->size() > _maxTrajectorySize)
+			{
+				_trajectory->erase(_trajectory->begin());
+			}
+		}
+		if(_aShowTrajectory->isChecked())
+		{
+			_visualizer->removeShape("trajectory");
+			pcl::PolygonMesh mesh;
+			pcl::Vertices vertices;
+			vertices.vertices.resize(_trajectory->size());
+			for(unsigned int i=0; i<vertices.vertices.size(); ++i)
+			{
+				vertices.vertices[i] = i;
+			}
+			pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
+			mesh.polygons.push_back(vertices);
+			_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory");
+		}
+
+		if(pose != _lastPose)
+		{
+			std::vector<pcl::visualization::Camera> cameras;
+			_visualizer->getCameras(cameras);
+
+			if(_aLockCamera->isChecked())
+			{
+				//update camera position
+				Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
+				cameras.front().pos[0] += diff[0];
+				cameras.front().pos[1] += diff[1];
+				cameras.front().pos[2] += diff[2];
+				cameras.front().focal[0] += diff[0];
+				cameras.front().focal[1] += diff[1];
+				cameras.front().focal[2] += diff[2];
+			}
+			else if(_aFollowCamera->isChecked())
+			{
+				Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
+															  cameras.front().focal[1] - cameras.front().pos[1],
+															  cameras.front().focal[2] - cameras.front().pos[2]).normalized();
+				Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+				Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
+				Eigen::Vector3f xAxis = yAxis.cross(zAxis);
+				Transform PR(xAxis[0], xAxis[1], xAxis[2],0,
+							yAxis[0], yAxis[1], yAxis[2],0,
+							zAxis[0], zAxis[1], zAxis[2],0);
+
+				Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
+							PR[4], PR[5], PR[6], cameras.front().pos[1],
+							PR[8], PR[9], PR[10], cameras.front().pos[2]);
+				Transform F(PR[0], PR[1], PR[2], cameras.front().focal[0],
+							PR[4], PR[5], PR[6], cameras.front().focal[1],
+							PR[8], PR[9], PR[10], cameras.front().focal[2]);
+				Transform N = pose;
+				Transform O = _lastPose;
+				Transform O2N = O.inverse()*N;
+				Transform F2O = F.inverse()*O;
+				Transform T = F2O * O2N * F2O.inverse();
+				Transform Fp = F * T;
+				Transform P2F = P.inverse()*F;
+				Transform Pp = P * P2F * T * P2F.inverse();
+
+				cameras.front().pos[0] = Pp.x();
+				cameras.front().pos[1] = Pp.y();
+				cameras.front().pos[2] = Pp.z();
+				cameras.front().focal[0] = Fp.x();
+				cameras.front().focal[1] = Fp.y();
+				cameras.front().focal[2] = Fp.z();
+				//FIXME: the view up is not set properly...
+				cameras.front().view[0] = Fp[8];
+				cameras.front().view[1] = Fp[9];
+				cameras.front().view[2] = Fp[10];
+			}
+
+			if(_aLockViewZ->isChecked())
+			{
+				cameras.front().view[0] = 0;
+				cameras.front().view[1] = 0;
+				cameras.front().view[2] = 1;
+			}
+
+			_visualizer->setCameraPosition(
+					cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
+					cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
+					cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+		}
+		_visualizer->removeCoordinateSystem();
+		_visualizer->addCoordinateSystem(0.2, m);
+	}
+
+	_lastPose = pose;
+}
+
+void CloudViewer::render()
+{
+	this->GetRenderWindow()->Render();
+}
+
+void CloudViewer::setBackgroundColor(const QColor & color)
+{
+	_visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
+}
+
+void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
+{
+	pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
+	pcl::visualization::CloudActorMap::iterator iter = cloudActorMap->find(id);
+	if(iter != cloudActorMap->end())
+	{
+		iter->second.actor->SetVisibility(isVisible?1:0);
+	}
+	else
+	{
+		UERROR("Cannot find actor named \"%s\".", id.c_str());
+	}
+}
+
+void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
+{
+	_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
+}
+
+void CloudViewer::setCloudPointSize(const std::string & id, int size)
+{
+	_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
+}
+
+void CloudViewer::contextMenuEvent(QContextMenuEvent * event)
+{
+	QAction * a = _menu->exec(event->globalPos());
+	if(a)
+	{
+		handleAction(a);
+	}
+}
+
+void CloudViewer::handleAction(QAction * a)
+{
+	if(a == _aSetTrajectorySize)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set trajectory size"), tr("Size (0=infinite)"), _maxTrajectorySize, 0, 10000, 10, &ok);
+		if(ok)
+		{
+			_maxTrajectorySize = value;
+		}
+	}
+	else if(a == _aClearTrajectory)
+	{
+		_trajectory->clear();
+		_visualizer->removeShape("trajectory");
+		this->render();
+	}
+	else if(a == _aResetCamera)
+	{
+		_visualizer->setCameraPosition(
+				-1, 0, 0,
+				0, 0, 0,
+				0, 0, 1);
+		this->render();
+	}
+	else if(a == _aShowGrid)
+	{
+		if(_aShowGrid->isChecked())
+		{
+			float cellSize = 1.0f;
+			int cellCount = 50;
+			double r=0.5;
+			double g=0.5;
+			double b=0.5;
+			int id = 0;
+			float min = -float(cellCount/2) * cellSize;
+			float max = float(cellCount/2) * cellSize;
+			std::string name;
+			for(float i=min; i<=max; i += cellSize)
+			{
+				//over x
+				name = uFormat("line%d", ++id);
+				_visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name);
+				_gridLines.push_back(name);
+				//over y
+				name = uFormat("line%d", ++id);
+				_visualizer->addLine(pcl::PointXYZ(min, i, 0.0f), pcl::PointXYZ(max, i, 0.0f), r, g, b, name);
+				_gridLines.push_back(name);
+			}
+		}
+		else
+		{
+			for(std::list<std::string>::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter)
+			{
+				_visualizer->removeShape(*iter);
+			}
+			_gridLines.clear();
+		}
+
+		this->render();
+	}
+}
+
+} /* namespace rtabmap */

guilib/src/DataRecorder.cpp

@@ -0,0 +1,130 @@
+/*
+ * CloudRecorder.cpp
+ *
+ *  Created on: 2013-10-30
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/gui/DataRecorder.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/CameraEvent.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/UCv2Qt.h>
+#include <QtCore/QMetaType>
+#include <QtGui/QHBoxLayout>
+
+namespace rtabmap {
+
+
+DataRecorder::DataRecorder(QWidget * parent) :
+		QWidget(parent),
+	memory_(0),
+	imageView_(new ImageView(this)),
+	dataQueue_(0)
+{
+	qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
+
+	QHBoxLayout * layout = new QHBoxLayout(this);
+	layout->addWidget(imageView_);
+	this->setLayout(layout);
+}
+bool DataRecorder::init(const QString & path)
+{
+	if(!memory_)
+	{
+		ParametersMap customParameters;
+		customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+		customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
+		customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true")); // to keep images
+		memory_ = new Memory();
+		if(!memory_->init(path.toStdString(), true, customParameters, false))
+		{
+			delete memory_;
+			memory_ = 0;
+			UERROR("Error initializing the memory.");
+			return false;
+		}
+		return true;
+	}
+	else
+	{
+		UERROR("Already initialized, close it first.");
+		return false;
+	}
+}
+
+void DataRecorder::close()
+{
+	if(memory_)
+	{
+		delete memory_;
+		memory_ = 0;
+	}
+}
+
+DataRecorder::~DataRecorder()
+{
+	this->close();
+}
+
+void DataRecorder::addData(const rtabmap::Image & image)
+{
+	if(memory_)
+	{
+		//save to database
+		UTimer time;
+		memory_->update(image);
+		memory_->cleanup();
+
+		if(image.id() % 30)
+		{
+			memory_->emptyTrash();
+		}
+
+		UDEBUG("Time to process a message = %f s", time.ticks());
+	}
+	else
+	{
+		UWARN("CloudRecorder not initialized!");
+	}
+	--dataQueue_;
+}
+
+void DataRecorder::showImage(const rtabmap::Image & image)
+{
+	if(this->isVisible() && !image.empty())
+	{
+		imageView_->setImage(uCvMat2QImage(image.image()));
+		imageView_->setImageDepth(uCvMat2QImage(image.depth()));
+		imageView_->fitInView(imageView_->sceneRect(), Qt::KeepAspectRatio);
+	}
+}
+
+void DataRecorder::handleEvent(UEvent * event)
+{
+	if(event->getClassName().compare("CameraEvent") == 0)
+	{
+		CameraEvent * camEvent = (CameraEvent*)event;
+		if(camEvent->getCode() == CameraEvent::kCodeImageDepth ||
+		   camEvent->getCode() == CameraEvent::kCodeImage)
+		{
+			if(!camEvent->image().empty())
+			{
+				UINFO("Receiving rate = %f Hz", 1.0f/timer_.ticks());
+				QMetaObject::invokeMethod(this, "addData", Q_ARG(rtabmap::Image, camEvent->image()));
+				++dataQueue_;
+
+				if(dataQueue_ < 2 && this->isVisible())
+				{
+					QMetaObject::invokeMethod(this, "showImage", Q_ARG(rtabmap::Image, camEvent->image()));
+				}
+			}
+		}
+	}
+}
+
+} /* namespace rtabmap */

guilib/src/DatabaseViewer.cpp

@@ -32,6 +32,13 @@
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/gui/KeypointItem.h"
+#include "rtabmap/gui/UCv2Qt.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Signature.h"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/common/transforms.h>
 
 DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	QMainWindow(parent),
@@ -54,9 +61,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
 	connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
 	connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
-
-	ui_->graphicsView_A->setScene(new QGraphicsScene(this));
-	ui_->graphicsView_B->setScene(new QGraphicsScene(this));
+	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 
 	ui_->horizontalSlider_A->setTracking(false);
 	ui_->horizontalSlider_B->setTracking(false);
@@ -99,6 +104,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			delete memory_;
 			memory_ = 0;
 			imagesMap_.clear();
+			depthImagesMap_.clear();
 			ids_.clear();
 		}
 
@@ -135,9 +141,8 @@ void DatabaseViewer::updateIds()
 
 	std::set<int> ids = memory_->getAllSignatureIds();
 	ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
-	ids_.prepend(0);
 
-	UDEBUG("Loaded %d ids", ids_.size());
+	UINFO("Loaded %d ids", ids_.size());
 
 	if(ids_.size())
 	{
@@ -145,12 +150,12 @@ void DatabaseViewer::updateIds()
 		ui_->horizontalSlider_B->setMinimum(0);
 		ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
 		ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
-		ui_->horizontalSlider_A->setSliderPosition(0);
-		ui_->horizontalSlider_B->setSliderPosition(0);
 		ui_->horizontalSlider_A->setEnabled(true);
 		ui_->horizontalSlider_B->setEnabled(true);
-		ui_->label_idA->setText("0");
-		ui_->label_idB->setText("0");
+		ui_->horizontalSlider_A->setSliderPosition(0);
+		ui_->horizontalSlider_B->setSliderPosition(0);
+		sliderAValueChanged(0);
+		sliderBValueChanged(0);
 	}
 	else
 	{
@@ -217,31 +222,93 @@ void DatabaseViewer::generateLocalGraph()
 	}
 }
 
-void DatabaseViewer::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, QGraphicsScene * scene)
+void DatabaseViewer::generate3DMap()
 {
-	if(!scene)
+	if(!ids_.size() || !memory_)
 	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
 		return;
 	}
-	rtabmap::KeypointItem * item = 0;
-	int alpha = 70;
-	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
+	bool ok = false;
+	int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
+
+	if(ok)
 	{
-		const cv::KeyPoint & r = (*i).second;
-		int id = (*i).first;
-		QString info = QString( "WordRef = %1\n"
-								"Laplacian = %2\n"
-								"Dir = %3\n"
-								"Hessian = %4\n"
-								"X = %5\n"
-								"Y = %6\n"
-								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
-		float radius = r.size*1.2/9.*2;
+		int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
+		if(ok)
+		{
+			float voxelSize = QInputDialog::getDouble(this, tr("Voxel size?"), tr("Voxel Size"), 0.01, 0, 0.1, 3, &ok);
+			if(ok)
+			{
+				QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Map" + QString::number(id) + ".pcd", tr("PCL file (*.pcd)"));
+				if(!path.isEmpty())
+				{
+					std::map<int, int> ids = memory_->getNeighborsId(id, margin, -1, false);
+					if(ids.size() > 0)
+					{
+						std::map<int, rtabmap::Transform> poses, optimizedPoses;
+						std::multimap<int, std::pair<int, rtabmap::Transform> > edgeConstraints;
+						memory_->getMetricConstraints(uKeys(ids), memory_->getSignature(id)->mapId(), poses, edgeConstraints, true);
 
-		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+						UINFO("Poses=%d, constraints=%d", poses.size(), edgeConstraints.size());
 
-		scene->addItem(item);
-		item->setZValue(1);
+						rtabmap::util3d::saveTOROGraph("toro1.graph", poses, edgeConstraints);
+
+						rtabmap::Transform mapCorrection;
+						rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, 100, optimizedPoses, mapCorrection);
+
+						rtabmap::util3d::saveTOROGraph("toro2.graph", optimizedPoses, edgeConstraints);
+
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+						for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+						{
+							rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
+							if(!pose.isNull())
+							{
+								std::vector<unsigned char> image, depth, depth2d;
+								float depthConstant;
+								rtabmap::Transform localTransform;
+								memory_->getImageDepth(iter->first, image, depth, depth2d, depthConstant, localTransform);
+								pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+								cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
+								cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+								cloud = rtabmap::util3d::cloudFromDepthRGB(
+										imageMat,
+										depthMat,
+										depthMat.cols/2, depthMat.rows/2,
+										1.0f/depthConstant, 1.0f/depthConstant);
+
+								if(voxelSize > 0.0f)
+								{
+									cloud = rtabmap::util3d::voxelize(cloud, voxelSize);
+								}
+
+								cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
+
+								*assembledCloud += *cloud;
+							}
+						}
+
+						if(voxelSize > 0.0f)
+						{
+							pcl::VoxelGrid<pcl::PointXYZRGB> filter;
+							filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+							filter.setInputCloud(assembledCloud);
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
+							filter.filter(*tmp);
+							assembledCloud = tmp;
+						}
+
+						pcl::io::savePCDFile(path.toStdString(), *assembledCloud, true);
+						QMessageBox::information(this, "Generated Map", tr("Map saved to %1!\n(%2 nodes, %3 points)").arg(path).arg(ids.size()).arg(assembledCloud->size()));
+					}
+					else
+					{
+						QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
+					}
+				}
+			}
+		}
 	}
 }
 
@@ -272,7 +339,7 @@ void DatabaseViewer::update(int value,
 						QLabel * labelActions,
 						QLabel * labelParents,
 						QLabel * labelChildren,
-						QGraphicsView * view,
+						rtabmap::ImageView * view,
 						QLabel * labelId)
 {
 	UTimer timer;
@@ -282,23 +349,33 @@ void DatabaseViewer::update(int value,
 	labelChildren->clear();
 	if(value >= 0 && value < ids_.size())
 	{
-		view->scene()->clear();
+		view->clear();
 		int id = ids_.at(value);
 		labelId->setText(QString::number(id));
 		if(id>0)
 		{
 			//image
 			QImage img;
+			QImage imgDepth;
 			QMap<int, QByteArray>::iterator iter = imagesMap_.find(id);
+			QMap<int, QByteArray>::iterator iterDepth = depthImagesMap_.find(id);
 			if(iter == imagesMap_.end())
 			{
 				if(memory_)
 				{
-					cv::Mat image = memory_->getImage(id);
+					std::vector<unsigned char> image, depth, depth2d;
+					float depthConstant;
+					rtabmap::Transform localTransform;
+					memory_->getImageDepth(id, image, depth, depth2d, depthConstant, localTransform);
+					cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
+					cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
+					UINFO("loaded image(%d/%d) depth(%d/%d) depthConstant(%f)",
+							imageMat.cols, imageMat.rows,
+							depthMat.cols, depthMat.rows,
+							depthConstant);
 					if(!image.empty())
 					{
-						IplImage iplImg = image;
-						img = ipl2QImage(&iplImg);
+						img = uCvMat2QImage(imageMat);
 						if(!img.isNull())
 						{
 							QByteArray ba;
@@ -308,11 +385,27 @@ void DatabaseViewer::update(int value,
 							imagesMap_.insert(id, ba);
 						}
 					}
+					if(!depth.empty())
+					{
+						imgDepth = uCvMat2QImage(depthMat);
+						if(!imgDepth.isNull())
+						{
+							QByteArray ba;
+							QBuffer buffer(&ba);
+							buffer.open(QIODevice::WriteOnly);
+							QPixmap::fromImage(imgDepth).save(&buffer, "PGM"); // writes image into ba in PGM format
+							depthImagesMap_.insert(id, ba);
+						}
+					}
 				}
 			}
 			else
 			{
 				img.loadFromData(iter.value(), "BMP");
+				if(iterDepth != depthImagesMap_.end())
+				{
+					imgDepth.loadFromData(iterDepth.value(), "PGM");
+				}
 			}
 
 			if(memory_)
@@ -320,38 +413,47 @@ void DatabaseViewer::update(int value,
 				std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
 				if(words.size())
 				{
-					drawKeypoints(words, view->scene());
+					view->setFeatures(words);
 				}
 			}
 
 			if(!img.isNull())
 			{
-				view->scene()->addPixmap(QPixmap::fromImage(img));
+				view->setImage(img);
 			}
 			else
 			{
 				ULOGGER_DEBUG("Image is empty");
 			}
+			if(!imgDepth.isNull())
+			{
+				view->setImageDepth(imgDepth);
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image depth is empty");
+			}
+			view->fitInView(view->sceneRect(), Qt::KeepAspectRatio);
 
 			// loops
-			std::set<int> parents;
-			std::set<int> children;
+			std::map<int, rtabmap::Transform> parents;
+			std::map<int, rtabmap::Transform> children;
 			memory_->getLoopClosureIds(id, parents, children, true);
 			if(parents.size())
 			{
 				QString str;
-				for(std::set<int>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
+				for(std::map<int, rtabmap::Transform>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
 				{
-					str.append(QString("%1 ").arg(*iter));
+					str.append(QString("%1 ").arg(iter->first));
 				}
 				labelParents->setText(str);
 			}
 			if(children.size())
 			{
 				QString str;
-				for(std::set<int>::iterator iter=children.begin(); iter!=children.end(); ++iter)
+				for(std::map<int, rtabmap::Transform>::iterator iter=children.begin(); iter!=children.end(); ++iter)
 				{
-					str.append(QString("%1 ").arg(*iter));
+					str.append(QString("%1 ").arg(iter->first));
 				}
 				labelChildren->setText(str);
 			}
@@ -392,29 +494,3 @@ void DatabaseViewer::sliderBMoved(int value)
 		ULOGGER_ERROR("Slider index out of range ?");
 	}
 }
-
-QImage DatabaseViewer::ipl2QImage(const IplImage *newImage)
-{
-	QImage qtemp;
-	if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
-	{
-		int x;
-		int y;
-		char* data = newImage->imageData;
-
-		qtemp= QImage(newImage->width, newImage->height,QImage::Format_RGB32 );
-		for( y = 0; y < newImage->height; y++, data +=newImage->widthStep )
-		{
-			for( x = 0; x < newImage->width; x++)
-			{
-				uint *p = (uint*)qtemp.scanLine (y) + x;
-				*p = qRgb(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels]);
-			}
-		}
-	}
-	else
-	{
-		ULOGGER_ERROR("Wrong IplImage format");
-	}
- return qtemp;
-}

guilib/src/DetailedProgressDialog.cpp

@@ -20,12 +20,13 @@
 #include "DetailedProgressDialog.h"
 #include <QtGui/QLayout>
 #include <QtGui/QProgressBar>
-#include <QtGui/QPlainTextEdit>
+#include <QtGui/QTextEdit>
 #include <QtGui/QLabel>
 #include <QtGui/QPushButton>
 #include <QtGui/QCloseEvent>
 #include <QtGui/QCheckBox>
 #include <QtCore/QTimer>
+#include <QtCore/QTime>
 #include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap {
@@ -39,9 +40,9 @@ DetailedProgressDialog::DetailedProgressDialog(QWidget *parent, Qt::WindowFlags
 	_text->setWordWrap(true);
 	_progressBar = new QProgressBar(this);
 	_progressBar->setMaximum(1);
-	_detailedText  = new QPlainTextEdit(this);
+	_detailedText  = new QTextEdit(this);
 	_detailedText->setReadOnly(true);
-	_detailedText->setLineWrapMode(QPlainTextEdit::NoWrap);
+	_detailedText->setLineWrapMode(QTextEdit::NoWrap);
 	_closeButton = new QPushButton(this);
 	_closeButton->setText("Close");
 	_closeWhenDoneCheckBox = new QCheckBox(this);
@@ -76,7 +77,9 @@ void DetailedProgressDialog::setAutoClose(bool on, int delayedClosingTimeSec)
 void DetailedProgressDialog::appendText(const QString & text)
 {
 	_text->setText(text);
-	_detailedText->appendPlainText(text);
+	QString html = tr("<html><font color=\"#999999\">%1 </font>%2</html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(text);
+	_detailedText->append(html);
+	_detailedText->ensureCursorVisible();
 }
 void DetailedProgressDialog::setValue(int value)
 {
@@ -122,6 +125,12 @@ void DetailedProgressDialog::clear()
 	_closeButton->setEnabled(false);
 }
 
+void DetailedProgressDialog::resetProgress()
+{
+	_progressBar->reset();
+	_closeButton->setEnabled(false);
+}
+
 void DetailedProgressDialog::closeEvent(QCloseEvent *event)
 {
 	if(_progressBar->value() == _progressBar->maximum())

guilib/src/DetailedProgressDialog.h

@@ -23,7 +23,7 @@
 #include <QtGui/QDialog>
 
 class QLabel;
-class QPlainTextEdit;
+class QTextEdit;
 class QProgressBar;
 class QPushButton;
 class QCheckBox;
@@ -51,10 +51,11 @@ public slots:
 	void appendText(const QString & text);
 	void incrementStep();
 	void clear();
+	void resetProgress();
 
 private:
 	QLabel * _text;
-	QPlainTextEdit * _detailedText;
+	QTextEdit * _detailedText;
 	QProgressBar * _progressBar;
 	QPushButton * _closeButton;
 	QCheckBox * _closeWhenDoneCheckBox;

guilib/src/GuiLib.qrc

@@ -6,6 +6,7 @@
     <file>images/Play1Normal.png</file>
     <file>images/Pause.ico</file>
     <file>images/PauseOnLoop.ico</file>
+	<file>images/PauseOnLocalLoop.ico</file>
     <file>images/PauseLoopRejected.ico</file>
     <file>qss/default.qss</file>
     <file>images/Plot16.png</file>

guilib/src/ImageView.cpp

@@ -25,6 +25,7 @@
 #include <QtGui/QFileDialog>
 #include <QtCore/QDir>
 #include <QtGui/QAction>
+#include <QtGui/QGraphicsEffect>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
 
@@ -34,7 +35,9 @@ ImageView::ImageView(QWidget * parent) :
 		QGraphicsView(parent),
 		_zoom(250),
 		_minZoom(250),
-		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png")
+		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
+		_image(0),
+		_imageDepth(0)
 {
 	this->setTransformationAnchor(QGraphicsView::AnchorUnderMouse);
 	this->setScene(new QGraphicsScene(this));
@@ -44,6 +47,9 @@ ImageView::ImageView(QWidget * parent) :
 	_showImage = _menu->addAction(tr("Show image"));
 	_showImage->setCheckable(true);
 	_showImage->setChecked(true);
+	_showImageDepth = _menu->addAction(tr("Show image depth"));
+	_showImageDepth->setCheckable(true);
+	_showImageDepth->setChecked(false);
 	_showFeatures = _menu->addAction(tr("Show features"));
 	_showFeatures->setCheckable(true);
 	_showFeatures->setChecked(true);
@@ -54,7 +60,7 @@ ImageView::ImageView(QWidget * parent) :
 }
 
 ImageView::~ImageView() {
-
+	clear();
 }
 
 void ImageView::resetZoom()
@@ -68,6 +74,11 @@ bool ImageView::isImageShown()
 	return _showImage->isChecked();
 }
 
+bool ImageView::isImageDepthShown()
+{
+	return _showImageDepth->isChecked();
+}
+
 bool ImageView::isFeaturesShown()
 {
 	return _showFeatures->isChecked();
@@ -76,7 +87,30 @@ bool ImageView::isFeaturesShown()
 void ImageView::setFeaturesShown(bool shown)
 {
 	_showFeatures->setChecked(shown);
-	this->updateItemsShown();
+	for(int i=0; i<_features.size(); ++i)
+	{
+		_features[i]->setVisible(_showFeatures->isChecked());
+	}
+}
+
+void ImageView::setImageShown(bool shown)
+{
+	_showImage->setChecked(shown);
+	if(_image)
+	{
+		_image->setVisible(_showImage->isChecked());
+		this->updateOpacity();
+	}
+}
+
+void ImageView::setImageDepthShown(bool shown)
+{
+	_showImageDepth->setChecked(shown);
+	if(_imageDepth)
+	{
+		_imageDepth->setVisible(_showImageDepth->isChecked());
+		this->updateOpacity();
+	}
 }
 
 bool ImageView::isLinesShown()
@@ -87,7 +121,14 @@ bool ImageView::isLinesShown()
 void ImageView::setLinesShown(bool shown)
 {
 	_showLines->setChecked(shown);
-	this->updateItemsShown();
+	QList<QGraphicsItem*> items = this->scene()->items();
+	for(int i=0; i<items.size(); ++i)
+	{
+		if( qgraphicsitem_cast<QGraphicsLineItem*>(items.at(i)))
+		{
+			items.at(i)->setVisible(_showLines->isChecked());
+		}
+	}
 }
 
 void ImageView::contextMenuEvent(QContextMenuEvent * e)
@@ -110,28 +151,46 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 			img.save(text);
 		}
 	}
-	else if(action == _showFeatures || action == _showImage || action == _showLines)
+	else if(action == _showFeatures)
 	{
-		this->updateItemsShown();
+		this->setFeaturesShown(_showFeatures->isChecked());
+	}
+	else if(action == _showImage)
+	{
+		this->setImageShown(_showImage->isChecked());
+	}
+	else if(action == _showImageDepth)
+	{
+		this->setImageDepthShown(_showImageDepth->isChecked());
+	}
+	else if(action == _showLines)
+	{
+		this->setLinesShown(_showLines->isChecked());
+	}
+
+	if(action == _showImage || action ==_showImageDepth)
+	{
+		this->updateOpacity();
 	}
 }
 
-void ImageView::updateItemsShown()
+void ImageView::updateOpacity()
 {
-	QList<QGraphicsItem*> items = this->scene()->items();
-	for(int i=0; i<items.size(); ++i)
+	if(_image && _imageDepth)
 	{
-		if(qgraphicsitem_cast<KeypointItem*>(items.at(i)))
+		if(_image->isVisible() && _imageDepth->isVisible())
 		{
-			items.at(i)->setVisible(_showFeatures->isChecked());
+			QGraphicsOpacityEffect * effect = new QGraphicsOpacityEffect();
+			QGraphicsOpacityEffect * effect2 = new QGraphicsOpacityEffect();
+			effect->setOpacity(0.5);
+			effect2->setOpacity(0.5);
+			_image->setGraphicsEffect(effect);
+			_imageDepth->setGraphicsEffect(effect2);
 		}
-		else if( qgraphicsitem_cast<QGraphicsLineItem*>(items.at(i)))
+		else
 		{
-			items.at(i)->setVisible(_showLines->isChecked());
-		}
-		else if(qgraphicsitem_cast<QGraphicsPixmapItem*>(items.at(i)))
-		{
-			items.at(i)->setVisible(_showImage->isChecked());
+			_image->setGraphicsEffect(0);
+			_imageDepth->setGraphicsEffect(0);
 		}
 	}
 }
@@ -175,4 +234,94 @@ void ImageView::wheelEvent(QWheelEvent * e)
 	this->setMatrix(matrix);
 }
 
+void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
+{
+	for(int i=0; i<_features.size(); ++i)
+	{
+		scene()->removeItem(_features[i]);
+		delete _features[i];
+	}
+	_features.clear();
+
+	rtabmap::KeypointItem * item = 0;
+	int alpha = 70;
+	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
+	{
+		const cv::KeyPoint & r = (*i).second;
+		int id = (*i).first;
+		QString info = QString( "WordRef = %1\n"
+								"Laplacian = %2\n"
+								"Dir = %3\n"
+								"Hessian = %4\n"
+								"X = %5\n"
+								"Y = %6\n"
+								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
+		float radius = r.size*1.2/9.*2;
+
+		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+
+		scene()->addItem(item);
+		_features.append(item);
+		item->setVisible(_showFeatures->isChecked());
+		item->setZValue(1);
+	}
+}
+
+void ImageView::setImage(const QImage & image)
+{
+	if(_image)
+	{
+		_image->setPixmap(QPixmap::fromImage(image));
+	}
+	else
+	{
+		_image = scene()->addPixmap(QPixmap::fromImage(image));
+		_image->setVisible(_showImage->isChecked());
+		_showImage->setEnabled(true);
+		this->updateOpacity();
+	}
+}
+
+void ImageView::setImageDepth(const QImage & imageDepth)
+{
+	if(_imageDepth)
+	{
+		_imageDepth->setPixmap(QPixmap::fromImage(imageDepth));
+	}
+	else
+	{
+		_imageDepth = scene()->addPixmap(QPixmap::fromImage(imageDepth));
+		_imageDepth->setVisible(_showImageDepth->isChecked());
+		_showImageDepth->setEnabled(true);
+		this->updateOpacity();
+	}
+}
+
+void ImageView::clear()
+{
+	for(int i=0; i<_features.size(); ++i)
+	{
+		scene()->removeItem(_features[i]);
+		delete _features[i];
+	}
+	_features.clear();
+
+	if(_image)
+	{
+		scene()->removeItem(_image);
+		delete _image;
+		_image = 0;
+		_showImage->setEnabled(false);
+	}
+
+	if(_imageDepth)
+	{
+		scene()->removeItem(_imageDepth);
+		delete _imageDepth;
+		_imageDepth = 0;
+		_showImageDepth->setEnabled(false);
+	}
+	scene()->clear();
+}
+
 }

guilib/src/LoopClosureViewer.cpp

@@ -0,0 +1,212 @@
+/*
+ * LoopClosureViewer.cpp
+ *
+ *  Created on: 2013-10-21
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/gui/LoopClosureViewer.h"
+#include "ui_loopClosureViewer.h"
+
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+#include <QtCore/QTimer>
+
+namespace rtabmap {
+
+LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
+	QWidget(parent),
+	sA_(0),
+	sB_(0),
+	decimation_(1),
+	maxDepth_(0),
+	samples_(0)
+{
+	ui_ = new Ui_loopClosureViewer();
+	ui_->setupUi(this);
+
+	connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
+}
+
+LoopClosureViewer::~LoopClosureViewer() {
+	delete ui_;
+	if(sA_)
+	{
+		delete sA_;
+	}
+	if(sB_)
+	{
+		delete sB_;
+	}
+}
+
+void LoopClosureViewer::setData(Signature * sA, Signature * sB)
+{
+	if(sA_)
+	{
+		delete sA_;
+	}
+	if(sB_)
+	{
+		delete sB_;
+	}
+	sA_ = sA;
+	sB_ = sB;
+	if(sA_ && sB_)
+	{
+		ui_->label_idA->setText(QString("[%1-%2]").arg(sA->id()).arg(sB->id()));
+	}
+}
+
+void LoopClosureViewer::updateView(const Transform & transform)
+{
+	if(sA_ && sB_)
+	{
+		int decimation = 1;
+		float maxDepth = 0;
+		int samples = 0;
+
+		if(!ui_->checkBox_rawCloud->isChecked())
+		{
+			decimation = decimation_;
+			maxDepth = maxDepth_;
+			samples = samples_;
+		}
+
+		UDEBUG("decimation = %d", decimation);
+		UDEBUG("maxDepth = %d", maxDepth);
+		UDEBUG("samples = %d", samples);
+
+		Transform t;
+		if(!transform.isNull())
+		{
+			transform_ = transform;
+			t = transform;
+		}
+		else if(!transform_.isNull())
+		{
+			t = transform_;
+		}
+		else
+		{
+			t = sB_->getPose();
+		}
+
+		UDEBUG("t= %s", t.prettyPrint().c_str());
+		ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
+		if(!t.isNull())
+		{
+			util3d::CompressionThread ctiA(sA_->getImage(), true);
+			util3d::CompressionThread ctdA(sA_->getDepth(), true);
+			util3d::CompressionThread ctiB(sB_->getImage(), true);
+			util3d::CompressionThread ctdB(sB_->getDepth(), true);
+			util3d::CompressionThread ct2dA(sA_->getDepth2D(), false);
+			util3d::CompressionThread ct2dB(sB_->getDepth2D(), false);
+			ctiA.start();
+			ctdA.start();
+			ctiB.start();
+			ctdB.start();
+			ct2dA.start();
+			ct2dB.start();
+			ctiA.join();
+			ctdA.join();
+			ctiB.join();
+			ctdB.join();
+			ct2dA.join();
+			ct2dB.join();
+			cv::Mat imageA = ctiA.getUncompressedData();
+			cv::Mat depthA = ctdA.getUncompressedData();
+			cv::Mat imageB = ctiB.getUncompressedData();
+			cv::Mat depthB = ctdB.getUncompressedData();
+			cv::Mat depth2dA = ct2dA.getUncompressedData();
+			cv::Mat depth2dB = ct2dB.getUncompressedData();
+
+			//cloud 3d
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
+			cloudA = util3d::cloudFromDepthRGB(
+					imageA,
+					depthA,
+					depthA.cols/2,
+					depthA.rows/2,
+					1.0f/sA_->getDepthConstant(),
+					1.0f/sA_->getDepthConstant(),
+					decimation);
+
+			cloudA = util3d::removeNaNFromPointCloud(cloudA);
+
+			if(maxDepth>0.0)
+			{
+				cloudA = util3d::passThrough(cloudA, "z", 0, maxDepth);
+			}
+			if(samples>0 && (int)cloudA->size() > samples)
+			{
+				cloudA = util3d::sampling(cloudA, samples);
+			}
+			cloudA = util3d::transformPointCloud(cloudA, sA_->getLocalTransform());
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
+			cloudB = util3d::cloudFromDepthRGB(
+					imageB,
+					depthB,
+					depthB.cols/2,
+					depthB.rows/2,
+					1.0f/sB_->getDepthConstant(),
+					1.0f/sB_->getDepthConstant(),
+					decimation);
+
+			cloudB = util3d::removeNaNFromPointCloud(cloudB);
+
+			if(maxDepth>0.0)
+			{
+				cloudB = util3d::passThrough(cloudB, "z", 0, maxDepth);
+			}
+			if(samples>0 && (int)cloudB->size() > samples)
+			{
+				cloudB = util3d::sampling(cloudB, samples);
+			}
+			cloudB = util3d::transformPointCloud(cloudB, t*sB_->getLocalTransform());
+
+			//cloud 2d
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
+			scanA = util3d::depth2DToPointCloud(depth2dA);
+			scanB = util3d::depth2DToPointCloud(depth2dB);
+			scanB = util3d::transformPointCloud(scanB, t);
+
+			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_->id()).arg(cloudB->size()).arg(sA_->id()).arg(cloudA->size()));
+
+			if(cloudA->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("cloud0", cloudA);
+			}
+			if(cloudB->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("cloud1", cloudB);
+			}
+			if(scanA->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("scan0", scanA);
+			}
+			if(scanB->size())
+			{
+				ui_->cloudViewerTransform->addOrUpdateCloud("scan1", scanB);
+			}
+		}
+		else
+		{
+			ui_->cloudViewerTransform->removeAllClouds();
+		}
+		ui_->cloudViewerTransform->render();
+	}
+}
+
+
+void LoopClosureViewer::showEvent(QShowEvent * event)
+{
+	QWidget::showEvent( event );
+	QTimer::singleShot(500, this, SLOT(updateView())); // make sure the QVTKWidget is shown!
+}
+

guilib/src/OdometryViewer.cpp

@@ -0,0 +1,174 @@
+/*
+ * OdometryViewer.cpp
+ *
+ *  Created on: 2013-10-15
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/gui/OdometryViewer.h"
+
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <pcl/common/transforms.h>
+#include <pcl/io/pcd_io.h>
+#include <QtGui/QInputDialog>
+#include <QtGui/QAction>
+#include <QtGui/QMenu>
+#include <QtGui/QKeyEvent>
+
+namespace rtabmap {
+
+
+OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, QWidget * parent) :
+		CloudViewer(parent),
+		maxClouds_(maxClouds),
+		voxelSize_(voxelSize),
+		decimation_(decimation),
+		id_(0),
+		_aSetVoxelSize(0),
+		_aSetDecimation(0),
+		_aSetCloudHistorySize(0),
+		_aPause(0)
+{
+
+	//add actions to CloudViewer menu
+	_aSetVoxelSize = new QAction("Set voxel size...", this);
+	_aSetDecimation = new QAction("Set depth image decimation...", this);
+	_aSetCloudHistorySize = new QAction("Set cloud history size...", this);
+	_aPause = new QAction("Pause", this);
+	_aPause->setCheckable(true);
+	menu()->addAction(_aSetVoxelSize);
+	menu()->addAction(_aSetDecimation);
+	menu()->addAction(_aSetCloudHistorySize);
+	menu()->addAction(_aPause);
+}
+
+void OdometryViewer::processData()
+{
+	rtabmap::Image data;
+	dataMutex_.lock();
+	if(buffer_.size())
+	{
+		data = buffer_.back();
+		buffer_.clear();
+	}
+	dataMutex_.unlock();
+
+	if(!data.empty() && this->isVisible())
+	{
+		UINFO("New pose = %s", data.pose().prettyPrint().c_str());
+
+		// visualization: buffering the clouds
+		// Create the new cloud
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		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(),
+			   decimation_);
+
+		if(voxelSize_ > 0.0f)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize_);
+		}
+
+		cloud = util3d::transformPointCloud(cloud, data.localTransform());
+
+		data.id()?id_=data.id():++id_;
+
+		clouds_.insert(std::make_pair(id_, cloud));
+
+		if((int)clouds_.size() > maxClouds_)
+		{
+			this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
+			clouds_.erase(clouds_.begin());
+		}
+
+		if(clouds_.size())
+		{
+			this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
+		}
+
+		this->updateCameraPosition(data.pose());
+
+		this->setBackgroundColor(Qt::black);
+
+		this->render();
+	}
+}
+
+void OdometryViewer::handleEvent(UEvent * event)
+{
+	if(!_aPause->isChecked())
+	{
+		if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
+
+			if(odomEvent->isValid())
+			{
+				bool empty = false;
+				dataMutex_.lock();
+				if(buffer_.empty())
+				{
+					buffer_.push_back(odomEvent->data());
+					empty= true;
+				}
+				else
+				{
+					buffer_.back() = odomEvent->data();
+				}
+				dataMutex_.unlock();
+				if(empty)
+				{
+					QMetaObject::invokeMethod(this, "processData");
+				}
+			}
+			else
+			{
+				//UWARN("odom=%fs, Cannot compute odometry!!!", timer_.restart());
+				QMetaObject::invokeMethod(this, "setBackgroundColor", Q_ARG(QColor, Qt::darkRed));
+				QMetaObject::invokeMethod(this, "render");
+			}
+		}
+	}
+}
+
+void OdometryViewer::handleAction(QAction * a)
+{
+	CloudViewer::handleAction(a);
+	if(a == _aSetVoxelSize)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set voxel size"), tr("Size (0=disabled)"), voxelSize_, 0.0, 0.1, 2, &ok);
+		if(ok)
+		{
+			voxelSize_ = value;
+		}
+	}
+	else if(a == _aSetCloudHistorySize)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set cloud history size"), tr("Size (0=infinite)"), maxClouds_, 0, 100, 1, &ok);
+		if(ok)
+		{
+			maxClouds_ = value;
+		}
+	}
+	else if(a == _aSetDecimation)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set depth image decimation"), tr("Decimation (0=infinite)"), decimation_, 1, 8, 1, &ok);
+		if(ok)
+		{
+			decimation_ = value;
+		}
+	}
+}
+
+} /* namespace rtabmap */

guilib/src/PdfPlot.cpp

@@ -19,6 +19,8 @@
 
 #include "PdfPlot.h"
 #include <rtabmap/utilite/ULogger.h>
+#include "rtabmap/gui/UCv2Qt.h"
+#include "rtabmap/core/util3d.h"
 
 namespace rtabmap {
 
@@ -59,15 +61,13 @@ void PdfPlotItem::showDescription(bool shown)
 		if(!_img && _imagesRef)
 		{
 			QImage img;
-			QMap<int, QByteArray>::const_iterator iter = _imagesRef->find(int(this->data().x()));
+			QMap<int, std::vector<unsigned char> >::const_iterator iter = _imagesRef->find(int(this->data().x()));
 			if(iter != _imagesRef->constEnd())
 			{
-				if(img.loadFromData(iter.value(), "JPEG"))
-				{
-					QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
-					_img = new QGraphicsPixmapItem(scaled, this);
-					_img->setVisible(false);
-				}
+				img = uCvMat2QImage(util3d::uncompressImage(iter.value()));
+				QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
+				_img = new QGraphicsPixmapItem(scaled, this);
+				_img->setVisible(false);
 			}
 		}
 
@@ -103,7 +103,7 @@ void PdfPlotItem::showDescription(bool shown)
 
 
 
-PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, QByteArray> * imagesMapRef = 0, QObject * parent) :
+PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef = 0, QObject * parent) :
 	UPlotCurve(name, parent),
 	_imagesMapRef(imagesMapRef)
 {

guilib/src/PdfPlot.h

@@ -21,6 +21,7 @@
 #define PDFPLOT_H_
 
 #include <utilite/UPlot.h>
+#include "opencv2/opencv.hpp"
 
 namespace rtabmap {
 
@@ -31,7 +32,7 @@ public:
 	virtual ~PdfPlotItem();
 
 	void setLikelihood(int id, float value, int childCount);
-	void setImagesRef(const QMap<int, QByteArray> * imagesRef) {_imagesRef = imagesRef;}
+	void setImagesRef(const QMap<int, std::vector<unsigned char> > * imagesRef) {_imagesRef = imagesRef;}
 
 	float value() const {return this->data().y();}
 	int id() const {return this->data().x();}
@@ -42,7 +43,7 @@ protected:
 private:
 	QGraphicsPixmapItem * _img;
 	int _childCount;
-	const QMap<int, QByteArray> * _imagesRef;
+	const QMap<int, std::vector<unsigned char> > * _imagesRef;
 	QGraphicsTextItem * _text;
 
 };
@@ -52,14 +53,14 @@ class PdfPlotCurve : public UPlotCurve
 	Q_OBJECT
 
 public:
-	PdfPlotCurve(const QString & name, const QMap<int, QByteArray> * imagesMapRef, QObject * parent = 0);
+	PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef, QObject * parent = 0);
 	virtual ~PdfPlotCurve();
 
 	virtual void clear();
 	void setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap);
 
 private:
-	const QMap<int, QByteArray> * _imagesMapRef;
+	const QMap<int, std::vector<unsigned char> > * _imagesMapRef;
 };
 
 }

guilib/src/qtipl.cpp

@@ -1,53 +0,0 @@
-/*
- * 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/gui/qtipl.h"
-#include "rtabmap/utilite/ULogger.h"
-#include <opencv2/core/core_c.h>
-
-namespace rtabmap {
-
-// TODO : support only from gray 8bits ?
-QImage Ipl2QImage(const IplImage *newImage, int alpha)
-{
-	QImage qtemp;
-	if (newImage && newImage->depth == IPL_DEPTH_8U && cvGetSize(newImage).width>0)
-	{
-		int x;
-		int y;
-		char* data = newImage->imageData;
-		
-		qtemp= QImage(newImage->width, newImage->height,QImage::Format_ARGB32 );
-		for( y = 0; y < newImage->height; y++, data +=newImage->widthStep )
-		{
-			for( x = 0; x < newImage->width; x++)
-			{
-				uint *p = (uint*)qtemp.scanLine (y) + x;
-				*p = qRgba(data[x * newImage->nChannels+2], data[x * newImage->nChannels+1],data[x * newImage->nChannels], alpha);
-			}
-		}
-	}
-	else
-	{
-		ULOGGER_ERROR("Wrong IplImage format");
-	}
- return qtemp;	
-}
-
-}

guilib/src/ui/DatabaseViewer.ui

@@ -29,7 +29,7 @@
          <number>0</number>
         </property>
         <item>
-         <widget class="QGraphicsView" name="graphicsView_A"/>
+         <widget class="rtabmap::ImageView" name="graphicsView_A"/>
         </item>
         <item>
          <widget class="QScrollArea" name="scrollArea_2">
@@ -153,7 +153,7 @@
          <number>0</number>
         </property>
         <item>
-         <widget class="QGraphicsView" name="graphicsView_B"/>
+         <widget class="rtabmap::ImageView" name="graphicsView_B"/>
         </item>
         <item>
          <widget class="QScrollArea" name="scrollArea">
@@ -288,7 +288,7 @@
      <x>0</x>
      <y>0</y>
      <width>736</width>
-     <height>22</height>
+     <height>21</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -309,7 +309,7 @@
     <addaction name="actionClean_database"/>
     <addaction name="actionClean_local_graph"/>
     <addaction name="separator"/>
-    <addaction name="actionUpdate_base_ids"/>
+    <addaction name="actionGenerate_3D_map_pcd"/>
    </widget>
    <addaction name="menuFile"/>
    <addaction name="menuEdit"/>
@@ -354,7 +354,19 @@
     <string>Update base ids</string>
    </property>
   </action>
+  <action name="actionGenerate_3D_map_pcd">
+   <property name="text">
+    <string>Generate 3D map (.pcd) ...</string>
+   </property>
+  </action>
  </widget>
+ <customwidgets>
+  <customwidget>
+   <class>rtabmap::ImageView</class>
+   <extends>QGraphicsView</extends>
+   <header>rtabmap/gui/ImageView.h</header>
+  </customwidget>
+ </customwidgets>
  <resources/>
  <connections/>
 </ui>

guilib/src/ui/aboutDialog.ui

@@ -82,10 +82,10 @@ p, li { white-space: pre-wrap; }
         </item>
         <item>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_2">
+          <item row="7" column="0">
+           <widget class="QLabel" name="label_6">
             <property name="text">
-             <string>Author :</string>
+             <string>Version :</string>
             </property>
            </widget>
           </item>
@@ -96,10 +96,10 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="7" column="0">
-           <widget class="QLabel" name="label_6">
+          <item row="1" column="0">
+           <widget class="QLabel" name="label_2">
             <property name="text">
-             <string>Version :</string>
+             <string>Author :</string>
             </property>
            </widget>
           </item>
@@ -174,6 +174,23 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
+          <item row="9" column="0">
+           <widget class="QLabel" name="label_11">
+            <property name="text">
+             <string>PCL version :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="1">
+           <widget class="QLabel" name="label_pcl_version">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
          </layout>
         </item>
         <item>

guilib/src/ui/loopClosureViewer.ui

@@ -0,0 +1,95 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>loopClosureViewer</class>
+ <widget class="QWidget" name="loopClosureViewer">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>515</width>
+    <height>390</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout" stretch="0,1">
+   <property name="spacing">
+    <number>0</number>
+   </property>
+   <property name="margin">
+    <number>0</number>
+   </property>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout_2">
+     <item>
+      <widget class="QLabel" name="label_idA">
+       <property name="text">
+        <string/>
+       </property>
+       <property name="alignment">
+        <set>Qt::AlignCenter</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_idB">
+       <property name="text">
+        <string/>
+       </property>
+       <property name="alignment">
+        <set>Qt::AlignCenter</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_transform">
+       <property name="text">
+        <string/>
+       </property>
+       <property name="alignment">
+        <set>Qt::AlignCenter</set>
+       </property>
+       <property name="textInteractionFlags">
+        <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer_2">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QCheckBox" name="checkBox_rawCloud">
+       <property name="text">
+        <string>Raw  cloud</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item>
+    <widget class="rtabmap::CloudViewer" name="cloudViewerTransform" native="true"/>
+   </item>
+  </layout>
+ </widget>
+ <customwidgets>
+  <customwidget>
+   <class>rtabmap::CloudViewer</class>
+   <extends>QWidget</extends>
+   <header>rtabmap/gui/CloudViewer.h</header>
+   <container>1</container>
+  </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>

guilib/src/ui/mainWindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1056</width>
-    <height>642</height>
+    <width>809</width>
+    <height>661</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -101,8 +101,8 @@
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>1056</width>
-     <height>22</height>
+     <width>809</width>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -115,35 +115,25 @@
     <property name="title">
      <string>Edit</string>
     </property>
-    <widget class="QMenu" name="menuAspect_ratio">
-     <property name="title">
-      <string>Aspect ratio</string>
-     </property>
-     <addaction name="action16_9"/>
-     <addaction name="action16_10"/>
-     <addaction name="action4_3"/>
-     <addaction name="separator"/>
-     <addaction name="action1080p"/>
-     <addaction name="action720p"/>
-     <addaction name="action480p"/>
-     <addaction name="action360p"/>
-     <addaction name="action240p"/>
-    </widget>
     <addaction name="actionApply_settings_to_the_detector"/>
-    <addaction name="actionClear_cache"/>
+    <addaction name="actionDownload_all_clouds"/>
     <addaction name="separator"/>
     <addaction name="actionOpen_working_directory"/>
+    <addaction name="actionPrint_loop_closure_IDs_to_console"/>
+    <addaction name="separator"/>
+    <addaction name="actionReset_the_memory"/>
     <addaction name="actionDelete_memory"/>
+    <addaction name="actionClear_cache"/>
     <addaction name="separator"/>
     <addaction name="actionDump_the_memory"/>
     <addaction name="actionDump_the_prediction_matrix"/>
-    <addaction name="actionPrint_loop_closure_IDs_to_console"/>
     <addaction name="actionGenerate_map"/>
     <addaction name="actionGenerate_local_map"/>
-    <addaction name="actionReset_the_memory"/>
     <addaction name="separator"/>
-    <addaction name="actionAuto_screen_capture"/>
-    <addaction name="menuAspect_ratio"/>
+    <addaction name="actionView_high_res_point_cloud"/>
+    <addaction name="actionSave_point_cloud"/>
+    <addaction name="actionView_point_cloud_as_mesh"/>
+    <addaction name="actionSave_mesh_ply_vtk_stl"/>
    </widget>
    <widget class="QMenu" name="menu6">
     <property name="title">
@@ -171,6 +161,7 @@
      </widget>
      <addaction name="menuImage"/>
      <addaction name="actionDatabase"/>
+     <addaction name="actionOpenni_RGBD"/>
     </widget>
     <addaction name="menuSelect_source"/>
     <addaction name="separator"/>
@@ -180,6 +171,13 @@
     <addaction name="separator"/>
     <addaction name="actionPause_on_match"/>
     <addaction name="actionPause_when_a_loop_hypothesis_is_rejected"/>
+    <addaction name="actionPause_on_local_loop_detection"/>
+    <addaction name="separator"/>
+    <addaction name="actionSLAM_mode"/>
+    <addaction name="actionLocalization_mode"/>
+    <addaction name="separator"/>
+    <addaction name="actionReset_Odometry"/>
+    <addaction name="actionTrigger_a_new_map"/>
    </widget>
    <widget class="QMenu" name="menuWindow">
     <property name="title">
@@ -198,8 +196,24 @@
      <addaction name="actionSave_state"/>
      <addaction name="actionLoad_state"/>
     </widget>
+    <widget class="QMenu" name="menuAspect_ratio_2">
+     <property name="title">
+      <string>Aspect ratio</string>
+     </property>
+     <addaction name="action16_9"/>
+     <addaction name="action16_10"/>
+     <addaction name="action4_3"/>
+     <addaction name="separator"/>
+     <addaction name="action1080p"/>
+     <addaction name="action720p"/>
+     <addaction name="action480p"/>
+     <addaction name="action360p"/>
+     <addaction name="action240p"/>
+    </widget>
     <addaction name="menuShow_view"/>
     <addaction name="menuFigures"/>
+    <addaction name="actionAuto_screen_capture"/>
+    <addaction name="menuAspect_ratio_2"/>
     <addaction name="separator"/>
     <addaction name="actionPreferences"/>
    </widget>
@@ -250,6 +264,11 @@
    <addaction name="separator"/>
    <addaction name="actionPause_on_match"/>
    <addaction name="actionPause_when_a_loop_hypothesis_is_rejected"/>
+   <addaction name="actionPause_on_local_loop_detection"/>
+   <addaction name="separator"/>
+   <addaction name="actionSLAM_mode"/>
+   <addaction name="actionLocalization_mode"/>
+   <addaction name="separator"/>
   </widget>
   <widget class="QDockWidget" name="dockWidget_statsV2">
    <property name="floating">
@@ -277,6 +296,28 @@
        <property name="verticalSpacing">
         <number>2</number>
        </property>
+       <item row="3" column="1">
+        <widget class="QDoubleSpinBox" name="doubleSpinBox_stats_timeLimit">
+         <property name="alignment">
+          <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+         </property>
+         <property name="suffix">
+          <string> ms</string>
+         </property>
+         <property name="decimals">
+          <number>0</number>
+         </property>
+         <property name="maximum">
+          <double>99999.000000000000000</double>
+         </property>
+         <property name="singleStep">
+          <double>50.000000000000000</double>
+         </property>
+         <property name="value">
+          <double>450.000000000000000</double>
+         </property>
+        </widget>
+       </item>
        <item row="0" column="0">
         <widget class="QLabel" name="label_24">
          <property name="text">
@@ -316,7 +357,7 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="0">
+       <item row="4" column="0">
         <widget class="QLabel" name="label_45">
          <property name="text">
           <string>Elapsed time (hh:mm:ss)</string>
@@ -326,7 +367,7 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="1">
+       <item row="4" column="1">
         <widget class="QLabel" name="label_elapsedTime">
          <property name="text">
           <string>Unknown</string>
@@ -336,14 +377,14 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
+       <item row="5" column="0">
         <widget class="QLabel" name="label_20">
          <property name="text">
           <string>Current image id</string>
          </property>
         </widget>
        </item>
-       <item row="4" column="1">
+       <item row="5" column="1">
         <widget class="QLabel" name="label_stats_imageNumber">
          <property name="text">
           <string>Unknown</string>
@@ -353,14 +394,14 @@
          </property>
         </widget>
        </item>
-       <item row="5" column="0">
+       <item row="6" column="0">
         <widget class="QLabel" name="label_12">
          <property name="text">
           <string>Loop closures detected</string>
          </property>
         </widget>
        </item>
-       <item row="5" column="1">
+       <item row="6" column="1">
         <widget class="QLabel" name="label_stats_loopClosuresDetected">
          <property name="text">
           <string>0</string>
@@ -370,7 +411,7 @@
          </property>
         </widget>
        </item>
-       <item row="6" column="0">
+       <item row="7" column="0">
         <widget class="QLabel" name="label_38">
          <property name="text">
           <string>Loop closures detected 
@@ -378,7 +419,7 @@
          </property>
         </widget>
        </item>
-       <item row="6" column="1">
+       <item row="7" column="1">
         <widget class="QLabel" name="label_stats_loopClosuresReactivatedDetected">
          <property name="text">
           <string>0</string>
@@ -388,14 +429,14 @@
          </property>
         </widget>
        </item>
-       <item row="7" column="0">
+       <item row="8" column="0">
         <widget class="QLabel" name="label_15">
          <property name="text">
           <string>Loop closures rejected</string>
          </property>
         </widget>
        </item>
-       <item row="7" column="1">
+       <item row="8" column="1">
         <widget class="QLabel" name="label_stats_loopClosuresRejected">
          <property name="text">
           <string>0</string>
@@ -406,14 +447,14 @@
         </widget>
        </item>
        <item row="1" column="0">
-        <widget class="QLabel" name="label">
+        <widget class="QLabel" name="doubleSpinBox_stats_imgRate_label">
          <property name="text">
           <string>Image rate</string>
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
-        <widget class="QLabel" name="label_2">
+       <item row="3" column="0">
+        <widget class="QLabel" name="label_timeLimit">
          <property name="text">
           <string>Time limit processing</string>
          </property>
@@ -423,24 +464,31 @@
         </widget>
        </item>
        <item row="2" column="1">
-        <widget class="QDoubleSpinBox" name="doubleSpinBox_stats_timeLimit">
+        <widget class="QDoubleSpinBox" name="doubleSpinBox_stats_detectionRate">
          <property name="alignment">
           <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
          </property>
          <property name="suffix">
-          <string> ms</string>
+          <string> Hz</string>
          </property>
          <property name="decimals">
-          <number>0</number>
+          <number>1</number>
          </property>
          <property name="maximum">
-          <double>99999.000000000000000</double>
+          <double>100.000000000000000</double>
          </property>
          <property name="singleStep">
-          <double>50.000000000000000</double>
+          <double>0.100000000000000</double>
          </property>
          <property name="value">
-          <double>450.000000000000000</double>
+          <double>2.000000000000000</double>
+         </property>
+        </widget>
+       </item>
+       <item row="2" column="0">
+        <widget class="QLabel" name="doubleSpinBox_stats_imgRate_label_2">
+         <property name="text">
+          <string>RTAB-Map update rate</string>
          </property>
         </widget>
        </item>
@@ -480,13 +528,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -537,6 +579,48 @@
     </layout>
    </widget>
   </widget>
+  <widget class="QDockWidget" name="dockWidget_cloudViewer">
+   <property name="windowTitle">
+    <string>3D Map</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>4</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents_5">
+    <layout class="QVBoxLayout" name="verticalLayout_3">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <item>
+      <widget class="rtabmap::CloudViewer" name="widget_cloudViewer" native="true"/>
+     </item>
+    </layout>
+   </widget>
+  </widget>
+  <widget class="QDockWidget" name="dockWidget_loopClosureViewer">
+   <property name="windowTitle">
+    <string>3D Loop closure</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>8</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents_7">
+    <layout class="QVBoxLayout" name="verticalLayout_7">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <item>
+      <widget class="rtabmap::LoopClosureViewer" name="widget_loopClosureViewer" native="true"/>
+     </item>
+    </layout>
+   </widget>
+  </widget>
   <action name="actionExit">
    <property name="text">
     <string>Exit</string>
@@ -593,7 +677,7 @@
      <normaloff>:/images/PauseOnLoop.ico</normaloff>:/images/PauseOnLoop.ico</iconset>
    </property>
    <property name="text">
-    <string>Pause on match</string>
+    <string>Pause on loop closure detection</string>
    </property>
   </action>
   <action name="actionStop">
@@ -612,7 +696,7 @@
   </action>
   <action name="actionApply_settings_to_the_detector">
    <property name="text">
-    <string>Apply settings to the detector</string>
+    <string>Apply settings to detector</string>
    </property>
   </action>
   <action name="actionDump_the_memory">
@@ -629,7 +713,7 @@
      <normaloff>:/images/PauseLoopRejected.ico</normaloff>:/images/PauseLoopRejected.ico</iconset>
    </property>
    <property name="text">
-    <string>Pause when a loop hypothesis is rejected</string>
+    <string>Pause on loop closure rejection</string>
    </property>
   </action>
   <action name="actionClear_cache">
@@ -662,7 +746,7 @@
   </action>
   <action name="actionGenerate_map">
    <property name="text">
-    <string>Generate map...</string>
+    <string>Generate graph map (*.dot)...</string>
    </property>
   </action>
   <action name="actionDelete_memory">
@@ -749,7 +833,7 @@
   </action>
   <action name="actionGenerate_local_map">
    <property name="text">
-    <string>Generate local map...</string>
+    <string>Generate graph local map (*.dot)...</string>
    </property>
   </action>
   <action name="actionPrint_loop_closure_IDs_to_console">
@@ -757,6 +841,80 @@
     <string>Print loop closure IDs to console</string>
    </property>
   </action>
+  <action name="actionOpenni_RGBD">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Openni (RGBD)</string>
+   </property>
+  </action>
+  <action name="actionSave_point_cloud">
+   <property name="text">
+    <string>Save high-res point clouds (*.pcd *.ply *.vtk)...</string>
+   </property>
+  </action>
+  <action name="actionDownload_all_clouds">
+   <property name="text">
+    <string>Download all clouds (update cache)</string>
+   </property>
+  </action>
+  <action name="actionPause_on_local_loop_detection">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="icon">
+    <iconset resource="../GuiLib.qrc">
+     <normaloff>:/images/PauseOnLocalLoop.ico</normaloff>:/images/PauseOnLocalLoop.ico</iconset>
+   </property>
+   <property name="text">
+    <string>Pause on local loop closure detection</string>
+   </property>
+  </action>
+  <action name="actionSLAM_mode">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Mapping</string>
+   </property>
+   <property name="toolTip">
+    <string>Simultaneous Localization And Mapping (SLAM)</string>
+   </property>
+  </action>
+  <action name="actionLocalization_mode">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Localization</string>
+   </property>
+  </action>
+  <action name="actionReset_Odometry">
+   <property name="text">
+    <string>Reset Odometry</string>
+   </property>
+  </action>
+  <action name="actionView_high_res_point_cloud">
+   <property name="text">
+    <string>View high-res point clouds</string>
+   </property>
+  </action>
+  <action name="actionView_point_cloud_as_mesh">
+   <property name="text">
+    <string>View meshes</string>
+   </property>
+  </action>
+  <action name="actionSave_mesh_ply_vtk_stl">
+   <property name="text">
+    <string>Save meshes (*.ply *.vtk)...</string>
+   </property>
+  </action>
+  <action name="actionTrigger_a_new_map">
+   <property name="text">
+    <string>Trigger a new map</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>
@@ -782,6 +940,18 @@
    <header>ConsoleWidget.h</header>
    <container>1</container>
   </customwidget>
+  <customwidget>
+   <class>rtabmap::CloudViewer</class>
+   <extends>QWidget</extends>
+   <header>../include/rtabmap/gui/CloudViewer.h</header>
+   <container>1</container>
+  </customwidget>
+  <customwidget>
+   <class>rtabmap::LoopClosureViewer</class>
+   <extends>QWidget</extends>
+   <header>../include/rtabmap/gui/LoopClosureViewer.h</header>
+   <container>1</container>
+  </customwidget>
  </customwidgets>
  <resources>
   <include location="../GuiLib.qrc"/>